Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <util/delay.h>
- #include "spektrum.h"
- #include "motors.h"
- float map_range(float input, float in_low, float in_high, float out_low, float out_high);
- float map_range_with_center(float input, float in_low, float in_high, float out_low, float out_high, float in_center);
- uint16_t spektrum_build_value(char c, uint16_t parent_state);
- Spektrum *spektrum_instance(){
- static Spektrum *spektrum = NULL;
- if (spektrum == NULL){
- spektrum = malloc(sizeof(Spektrum));
- spektrum->packet_counter = 0;
- spektrum->state = 0;
- }
- return spektrum;
- }
- // call this once in your loop to grab fresh values
- void spektrum_block_until_refresh(){
- usart_enable();
- float start_time = get_time_in_ms(0);
- while (spektrum_instance()->state == 0){
- if (get_time_in_ms(0) - start_time > 2000){
- // in this application, it made the most sense to halt the program and wait for signal re-aquisition
- motors_disable();
- clearbit(PORTC, BIT(1));
- usart_send("spektrum timeout...");
- _delay_ms(500);
- }
- }
- spektrum_instance()->state = 0;
- usart_disable();
- }
- // call this at any time to get the currently stored value for a channel
- float spektrum_get(int channel){
- static int32_t spektrum_config[4][6] =
- { // channel type low hi mid
- { 1, 0, 2391, 3750, 3057}, // 0 - aileron
- { 3, 0, 4439, 5801, 5075}, // 1 - elevator
- { 10, 1, 352, 1709, 1024}, // 2 - throttle
- { 2, 0, 10582, 11946, 11264}
- };
- int32_t data_channel = spektrum_config[channel][0];
- float input = (float) spektrum_instance()->data[data_channel];
- float spektrum_value;
- // if type == 0
- if (spektrum_config[channel][1] == 0){
- spektrum_value = map_range_with_center(input, spektrum_config[channel][2], spektrum_config[channel][3], -1.0f, 1.0f, spektrum_config[channel][4]);
- // if type == 1
- }else{
- spektrum_value = map_range(input, spektrum_config[channel][2], spektrum_config[channel][3], 0.0f, 1.0f);
- }
- return spektrum_value;
- }
- uint16_t spektrum_parse_rx(char c){
- spektrum_instance()->packet_counter ++;
- static uint16_t state = 0;
- // state 0, searching for beginning of sync sequence
- if (state == 0){
- if (c == 0x3b){
- return state ++;
- }
- else return state = 0;
- }
- // state 1 - 10, searching for the 9 0xff's
- if (state >= 1 && state <= 9){
- if (c == 0xff) return state ++;
- return state = 0;
- }
- if (state > 9){
- spektrum_build_value(c, state);
- state ++;
- }
- if (state >= 32){
- state = 0;
- spektrum_instance()->state = 1;
- }
- return state;
- }
- uint16_t spektrum_build_value(char c, uint16_t parent_state){
- static uint16_t state = 0;
- static uint16_t index = 0;
- if (parent_state == 10){
- state = 0;
- index = 0;
- }
- if (state == 0){
- spektrum_instance()->data[index] = c;
- spektrum_instance()->data[index] = spektrum_instance()->data[index] << 8;
- return state = 1;
- }
- if (state == 1){
- spektrum_instance()->data[index] += c;
- state = 0;
- return index ++;
- }
- return state;
- }
- float map_range(float input, float in_low, float in_high, float out_low, float out_high){
- return out_low + (((input - in_low) / (in_high - in_low)) * (out_high - out_low));
- }
- float map_range_with_center(float input, float in_low, float in_high, float out_low, float out_high, float in_center){
- float out_center = out_low + (out_high - out_low) / 2;
- if (input > in_center) return map_range(input, in_center, in_high, out_center, out_high);
- else return map_range(input, in_low, in_center, out_low, out_center);
- }
- void usart_rx_callback_new(char c){
- spektrum_parse_rx(c);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement