Advertisement
Guest User

Untitled

a guest
Feb 2nd, 2015
485
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 3.55 KB | None | 0 0
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement