SHARE
TWEET

Untitled

a guest Feb 2nd, 2015 238 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <util/delay.h>
  2. #include "spektrum.h"
  3. #include "motors.h"
  4.  
  5.  
  6. float map_range(float input, float in_low, float in_high, float out_low, float out_high);
  7. float map_range_with_center(float input, float in_low, float in_high, float out_low, float out_high, float in_center);
  8. uint16_t spektrum_build_value(char c, uint16_t parent_state);
  9.  
  10. Spektrum *spektrum_instance(){
  11.         static Spektrum *spektrum = NULL;
  12.         if (spektrum == NULL){
  13.                 spektrum = malloc(sizeof(Spektrum));
  14.                 spektrum->packet_counter = 0;
  15.                 spektrum->state = 0;
  16.         }
  17.         return spektrum;
  18. }
  19.  
  20. // call this once in your loop to grab fresh values
  21. void spektrum_block_until_refresh(){
  22.        
  23.         usart_enable();
  24.         float start_time = get_time_in_ms(0);
  25.         while (spektrum_instance()->state == 0){
  26.                 if (get_time_in_ms(0) - start_time > 2000){
  27.                         // in this application, it made the most sense to halt the program and wait for signal re-aquisition
  28.                         motors_disable();
  29.                         clearbit(PORTC, BIT(1));
  30.                         usart_send("spektrum timeout...");
  31.                         _delay_ms(500);
  32.                 }
  33.         }
  34.        
  35.         spektrum_instance()->state = 0;
  36.         usart_disable();
  37. }
  38.  
  39. // call this at any time to get the currently stored value for a channel
  40. float spektrum_get(int channel){
  41.        
  42.         static int32_t spektrum_config[4][6] =
  43.         {       //  channel     type    low             hi              mid            
  44.                 {       1,              0,              2391,   3750,   3057},          // 0 - aileron
  45.                 {       3,              0,              4439,   5801,   5075},          // 1 - elevator
  46.                 {       10,             1,              352,    1709,   1024},          // 2 - throttle
  47.                 {       2,              0,              10582,  11946,  11264}                 
  48.         };
  49.        
  50.         int32_t data_channel = spektrum_config[channel][0];
  51.         float input = (float) spektrum_instance()->data[data_channel];
  52.         float spektrum_value;
  53.        
  54.         // if type == 0
  55.         if (spektrum_config[channel][1] == 0){
  56.                 spektrum_value = map_range_with_center(input, spektrum_config[channel][2], spektrum_config[channel][3], -1.0f, 1.0f, spektrum_config[channel][4]);
  57.         // if type == 1
  58.         }else{
  59.                 spektrum_value = map_range(input, spektrum_config[channel][2], spektrum_config[channel][3], 0.0f, 1.0f);
  60.         }
  61.         return spektrum_value;
  62.        
  63. }
  64.  
  65. uint16_t spektrum_parse_rx(char c){
  66.         spektrum_instance()->packet_counter ++;
  67.         static uint16_t state = 0;
  68.        
  69.         // state 0, searching for beginning of sync sequence
  70.         if (state == 0){
  71.                 if (c == 0x3b){
  72.                         return state ++;
  73.                 }
  74.                 else return state = 0;
  75.         }
  76.        
  77.         // state 1 - 10, searching for the 9 0xff's
  78.         if (state >= 1 && state <= 9){
  79.                 if (c == 0xff) return state ++;
  80.                 return state = 0;
  81.         }
  82.        
  83.         if (state > 9){
  84.                 spektrum_build_value(c, state);
  85.                 state ++;
  86.         }
  87.        
  88.         if (state >= 32){
  89.                 state = 0;
  90.                 spektrum_instance()->state = 1;
  91.         }
  92.         return state;
  93. }
  94.  
  95. uint16_t spektrum_build_value(char c, uint16_t parent_state){
  96.         static uint16_t state = 0;
  97.         static uint16_t index = 0;
  98.         if (parent_state == 10){
  99.                 state = 0;
  100.                 index = 0;
  101.         }
  102.         if (state == 0){
  103.                 spektrum_instance()->data[index] = c;  
  104.                 spektrum_instance()->data[index] = spektrum_instance()->data[index] << 8;
  105.                 return state = 1;
  106.         }
  107.         if (state == 1){
  108.                 spektrum_instance()->data[index] += c;
  109.                 state = 0;
  110.                 return index ++;
  111.         }
  112.         return state;
  113. }
  114.  
  115. float map_range(float input, float in_low, float in_high, float out_low, float out_high){
  116.         return  out_low + (((input - in_low) / (in_high - in_low)) * (out_high - out_low));
  117. }
  118.  
  119. float map_range_with_center(float input, float in_low, float in_high, float out_low, float out_high, float in_center){
  120.         float out_center = out_low + (out_high - out_low) / 2;
  121.         if (input > in_center)  return map_range(input, in_center, in_high, out_center, out_high);
  122.         else                                    return map_range(input, in_low, in_center, out_low, out_center);
  123. }
  124.  
  125. void usart_rx_callback_new(char c){
  126.         spektrum_parse_rx(c);
  127. }
RAW Paste Data
Pastebin PRO Summer Special!
Get 40% OFF on Pastebin PRO accounts!
Top