diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 36bd56642a..189f7b6632 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -64,6 +64,7 @@ AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) : _sport_status(0) {} +// init - perform require initialisation including detecting which protocol to use void AP_Frsky_Telem::init(const AP_SerialManager& serial_manager) { // initialise port to null @@ -97,11 +98,232 @@ void AP_Frsky_Telem::init(const AP_SerialManager& serial_manager) } } +/* + send_frames - sends updates down telemetry link for both DPORT and SPORT protocols + should be called by main program at 50hz to allow poll for serial bytes + coming from the receiver for the SPort protocol +*/ +void AP_Frsky_Telem::send_frames(uint8_t control_mode) +{ + // return immediately if not initialised + if (!_initialised) { + return; + } + + if (_protocol == FrSkySPORT) { + if (!_mode_data_ready) { + _mode=control_mode; + _mode_data_ready = true; + } + if (!_baro_data_ready) { + calc_baro_alt(); + _baro_data_ready = true; + } + if (!_gps_data_ready) { + calc_gps_position(); + _gps_data_ready = true; + } + if (!_sats_data_ready) { + calc_gps_sats(); + _sats_data_ready = true; + } + if (!_battery_data_ready) { + calc_battery(); + _battery_data_ready = true; + } + } else { + _mode=control_mode; + send_hub_frame(); + } +} + +/* + send_hub_frame - send frame1 and frame2 when protocol is FrSkyDPORT + frame 1 is sent every 200ms with baro alt, nb sats, batt volts and amp, control_mode + frame 2 is sent every second with gps position data +*/ +void AP_Frsky_Telem::send_hub_frame() +{ + uint32_t now = hal.scheduler->millis(); + + // send frame1 every 200ms + if (now - _last_frame1_ms > 200) { + _last_frame1_ms = now; + calc_gps_sats(); + send_gps_sats(); + send_mode(); + + calc_battery(); + send_batt_remain(); + send_batt_volts(); + send_current(); + + calc_baro_alt(); + send_baro_alt_m(); + send_baro_alt_cm(); + } + // send frame2 every second + if (now - _last_frame2_ms > 1000) { + _last_frame2_ms = now; + send_heading(); + calc_gps_position(); + if (_pos_gps_ok) { + send_gps_lat_dd(); + send_gps_lat_mm(); + send_gps_lat_ns(); + send_gps_lon_dd(); + send_gps_lon_mm(); + send_gps_lon_ew(); + send_gps_speed_meter(); + send_gps_speed_cm(); + send_gps_alt_meter(); + send_gps_alt_cm(); + } + } +} + +/* + sport_tick - main call to send updates to transmitter when protocol is FrSkySPORT + called by scheduler at a high rate +*/ +void AP_Frsky_Telem::sport_tick(void) +{ + if (!_initialised) { + _port->begin(57600); + _initialised = true; + } + + int16_t numc; + numc = _port->available(); + + // check if available is negative + if (numc < 0) { + return; + } + + // this is the constant for hub data frame + if (_port->txspace() < 19) { + return; + } + + for (int16_t i = 0; i < numc; i++) { + int16_t readbyte = _port->read(); + if (_sport_status == 0) { + if (readbyte == SPORT_START_FRAME) { + _sport_status = 1; + } + } else { + switch (readbyte) { + case DATA_ID_FAS: + if (_battery_data_ready) { + switch (_fas_call) { + case 0: + send_batt_volts(); + break; + case 1: + send_current(); + break; + } + _fas_call++; + if (_fas_call > 1) { + _fas_call = 0; + } + _battery_data_ready = false; + } + break; + case DATA_ID_GPS: + if (_gps_data_ready) { + switch (_gps_call) { + case 0: + send_gps_lat_dd(); + break; + case 1: + send_gps_lat_mm(); + break; + case 2: + send_gps_lat_ns(); + break; + case 3: + send_gps_lon_dd(); + break; + case 4: + send_gps_lon_mm(); + break; + case 5: + send_gps_lon_ew(); + break; + case 6: + send_gps_speed_meter(); + break; + case 7: + send_gps_speed_cm(); + break; + case 8: + send_gps_alt_meter(); + break; + case 9: + send_gps_alt_cm(); + break; + case 10: + send_heading(); + break; + } + + _gps_call++; + if (_gps_call > 10) { + _gps_call = 0; + _gps_data_ready = false; + } + } + break; + case DATA_ID_VARIO: + if (_baro_data_ready) { + switch (_vario_call) { + case 0 : + send_baro_alt_m(); + break; + case 1: + send_baro_alt_cm(); + break; + } + _vario_call ++; + if (_vario_call > 1) { + _vario_call = 0; + _baro_data_ready = false; + } + } + break; + case DATA_ID_SP2UR: + switch (_various_call) { + case 0 : + if ( _sats_data_ready ) { + send_gps_sats(); + _sats_data_ready = false; + } + break; + case 1: + if ( _mode_data_ready ) { + send_mode(); + _mode_data_ready = false; + } + break; + } + _various_call++; + if (_various_call > 1) { + _various_call = 0; + } + break; + } + _sport_status = 0; + } + } +} + /* simple crc implementation for FRSKY telem S-PORT */ -void AP_Frsky_Telem::calc_crc (uint8_t byte) +void AP_Frsky_Telem::calc_crc(uint8_t byte) { _crc += byte; //0-1FF _crc += _crc >> 8; //0-100 @@ -213,7 +435,7 @@ void AP_Frsky_Telem::frsky_send_data(uint8_t id, int16_t data) } /* - * frsky_send_baro_meters : send altitude in Meters based on ahrs estimate + * calc_baro_alt : send altitude in Meters based on ahrs estimate */ void AP_Frsky_Telem::calc_baro_alt() { @@ -244,7 +466,7 @@ float AP_Frsky_Telem::frsky_format_gps(float dec) } /* - * prepare lattitude and longitude information stored in member variables + * prepare latitude and longitude information stored in member variables */ void AP_Frsky_Telem::calc_gps_position() @@ -293,7 +515,7 @@ void AP_Frsky_Telem::calc_gps_position() /* * prepare battery information stored in member variables */ -void AP_Frsky_Telem::calc_battery () +void AP_Frsky_Telem::calc_battery() { _batt_remaining = roundf(_battery.capacity_remaining_pct()); _batt_volts = roundf(_battery.voltage() * 10.0f); @@ -303,7 +525,7 @@ void AP_Frsky_Telem::calc_battery () /* * prepare sats information stored in member variables */ -void AP_Frsky_Telem::calc_gps_sats () +void AP_Frsky_Telem::calc_gps_sats() { // GPS status is sent as num_sats*10 + status, to fit into a uint8_t const AP_GPS &gps = _ahrs.get_gps(); @@ -313,7 +535,7 @@ void AP_Frsky_Telem::calc_gps_sats () /* * send number of gps satellite and gps status eg: 73 means 7 satellite and 3d lock */ -void AP_Frsky_Telem::send_gps_sats () +void AP_Frsky_Telem::send_gps_sats() { frsky_send_data(FRSKY_ID_TEMP2, gps_sats); } @@ -321,7 +543,7 @@ void AP_Frsky_Telem::send_gps_sats () /* * send control_mode as Temperature 1 (TEMP1) */ -void AP_Frsky_Telem::send_mode (void) +void AP_Frsky_Telem::send_mode(void) { frsky_send_data(FRSKY_ID_TEMP1, _mode); } @@ -329,7 +551,7 @@ void AP_Frsky_Telem::send_mode (void) /* * send barometer altitude integer part . Initialize baro altitude */ -void AP_Frsky_Telem::send_baro_alt_m (void) +void AP_Frsky_Telem::send_baro_alt_m(void) { frsky_send_data(FRSKY_ID_BARO_ALT_BP, _baro_alt_meters); } @@ -337,7 +559,7 @@ void AP_Frsky_Telem::send_baro_alt_m (void) /* * send barometer altitude decimal part */ -void AP_Frsky_Telem::send_baro_alt_cm (void) +void AP_Frsky_Telem::send_baro_alt_cm(void) { frsky_send_data(FRSKY_ID_BARO_ALT_AP, _baro_alt_cm); } @@ -345,7 +567,7 @@ void AP_Frsky_Telem::send_baro_alt_cm (void) /* * send battery remaining */ -void AP_Frsky_Telem::send_batt_remain (void) +void AP_Frsky_Telem::send_batt_remain(void) { frsky_send_data(FRSKY_ID_FUEL, _batt_remaining); } @@ -353,7 +575,7 @@ void AP_Frsky_Telem::send_batt_remain (void) /* * send battery voltage */ -void AP_Frsky_Telem::send_batt_volts (void) +void AP_Frsky_Telem::send_batt_volts(void) { frsky_send_data(FRSKY_ID_VFAS, _batt_volts); } @@ -361,7 +583,7 @@ void AP_Frsky_Telem::send_batt_volts (void) /* * send current consumptiom */ -void AP_Frsky_Telem::send_current (void) +void AP_Frsky_Telem::send_current(void) { frsky_send_data(FRSKY_ID_CURRENT, _batt_amps); } @@ -369,7 +591,7 @@ void AP_Frsky_Telem::send_current (void) /* * send heading in degree based on AHRS and not GPS */ -void AP_Frsky_Telem::send_heading (void) +void AP_Frsky_Telem::send_heading(void) { frsky_send_data(FRSKY_ID_GPS_COURS_BP, _course_in_degrees); } @@ -377,7 +599,7 @@ void AP_Frsky_Telem::send_heading (void) /* * send gps lattitude degree and minute integer part; Initialize gps info */ -void AP_Frsky_Telem::send_gps_lat_dd (void) +void AP_Frsky_Telem::send_gps_lat_dd(void) { frsky_send_data(FRSKY_ID_GPS_LAT_BP, _latdddmm); } @@ -385,7 +607,7 @@ void AP_Frsky_Telem::send_gps_lat_dd (void) /* * send gps lattitude minutes decimal part */ -void AP_Frsky_Telem::send_gps_lat_mm (void) +void AP_Frsky_Telem::send_gps_lat_mm(void) { frsky_send_data(FRSKY_ID_GPS_LAT_AP, _latmmmm); } @@ -393,7 +615,7 @@ void AP_Frsky_Telem::send_gps_lat_mm (void) /* * send gps North / South information */ -void AP_Frsky_Telem::send_gps_lat_ns (void) +void AP_Frsky_Telem::send_gps_lat_ns(void) { frsky_send_data(FRSKY_ID_GPS_LAT_NS, _lat_ns); } @@ -401,7 +623,7 @@ void AP_Frsky_Telem::send_gps_lat_ns (void) /* * send gps longitude degree and minute integer part */ -void AP_Frsky_Telem::send_gps_lon_dd (void) +void AP_Frsky_Telem::send_gps_lon_dd(void) { frsky_send_data(FRSKY_ID_GPS_LONG_BP, _londddmm); } @@ -409,7 +631,7 @@ void AP_Frsky_Telem::send_gps_lon_dd (void) /* * send gps longitude minutes decimal part */ -void AP_Frsky_Telem::send_gps_lon_mm (void) +void AP_Frsky_Telem::send_gps_lon_mm(void) { frsky_send_data(FRSKY_ID_GPS_LONG_AP, _lonmmmm); } @@ -417,7 +639,7 @@ void AP_Frsky_Telem::send_gps_lon_mm (void) /* * send gps East / West information */ -void AP_Frsky_Telem::send_gps_lon_ew (void) +void AP_Frsky_Telem::send_gps_lon_ew(void) { frsky_send_data(FRSKY_ID_GPS_LONG_EW, _lon_ew); } @@ -425,7 +647,7 @@ void AP_Frsky_Telem::send_gps_lon_ew (void) /* * send gps speed integer part */ -void AP_Frsky_Telem::send_gps_speed_meter (void) +void AP_Frsky_Telem::send_gps_speed_meter(void) { frsky_send_data(FRSKY_ID_GPS_SPEED_BP, _speed_in_meter); } @@ -433,7 +655,7 @@ void AP_Frsky_Telem::send_gps_speed_meter (void) /* * send gps speed decimal part */ -void AP_Frsky_Telem::send_gps_speed_cm (void) +void AP_Frsky_Telem::send_gps_speed_cm(void) { frsky_send_data(FRSKY_ID_GPS_SPEED_AP, _speed_in_centimeter); } @@ -441,7 +663,7 @@ void AP_Frsky_Telem::send_gps_speed_cm (void) /* * send gps altitude integer part */ -void AP_Frsky_Telem::send_gps_alt_meter (void) +void AP_Frsky_Telem::send_gps_alt_meter(void) { frsky_send_data(FRSKY_ID_GPS_ALT_BP, _alt_gps_meters); } @@ -449,225 +671,7 @@ void AP_Frsky_Telem::send_gps_alt_meter (void) /* * send gps altitude decimals */ -void AP_Frsky_Telem::send_gps_alt_cm (void) +void AP_Frsky_Telem::send_gps_alt_cm(void) { frsky_send_data(FRSKY_ID_GPS_ALT_AP, _alt_gps_cm); } - -/* - * send frame 1 every 200ms with baro alt, nb sats, batt volts and amp, control_mode - */ - -void AP_Frsky_Telem::send_hub_frame() -{ - uint32_t now = hal.scheduler->millis(); - - // send frame1 every 200ms - if (now - _last_frame1_ms > 200) { - _last_frame1_ms = now; - calc_gps_sats(); - send_gps_sats(); - send_mode(); - - calc_battery(); - send_batt_remain(); - send_batt_volts(); - send_current(); - - calc_baro_alt(); - send_baro_alt_m(); - send_baro_alt_cm(); - } - // send frame2 every second - if (now - _last_frame2_ms > 1000) { - _last_frame2_ms = now; - send_heading(); - calc_gps_position(); - if (_pos_gps_ok) { - send_gps_lat_dd(); - send_gps_lat_mm(); - send_gps_lat_ns(); - send_gps_lon_dd(); - send_gps_lon_mm(); - send_gps_lon_ew(); - send_gps_speed_meter(); - send_gps_speed_cm(); - send_gps_alt_meter(); - send_gps_alt_cm(); - } - } -} - - -/* - send telemetry frames. Should be called at 50Hz. The high rate is to - allow this code to poll for serial bytes coming from the receiver - for the SPort protocol -*/ -void AP_Frsky_Telem::send_frames(uint8_t control_mode) -{ - // return immediately if not initialised - if (!_initialised) { - return; - } - - if (_protocol == FrSkySPORT) { - if (!_mode_data_ready) { - _mode=control_mode; - _mode_data_ready = true; - } - if (!_baro_data_ready) { - calc_baro_alt(); - _baro_data_ready = true; - } - if (!_gps_data_ready) { - calc_gps_position(); - _gps_data_ready = true; - } - if (!_sats_data_ready) { - calc_gps_sats(); - _sats_data_ready = true; - } - if (!_battery_data_ready) { - calc_battery(); - _battery_data_ready = true; - } - } else { - _mode=control_mode; - send_hub_frame(); - } -} - - -void AP_Frsky_Telem::sport_tick(void) -{ - if (!_initialised) { - _port->begin(57600); - _initialised = true; - } - - int16_t numc; - numc = _port->available(); - - // check if available is negative - if (numc < 0) { - return; - } - - // this is the constant for hub data frame - if (_port->txspace() < 19) { - return; - } - - for (int16_t i = 0; i < numc; i++) { - int16_t readbyte = _port->read(); - if (_sport_status == 0) { - if (readbyte == SPORT_START_FRAME) { - _sport_status = 1; - } - } else { - switch (readbyte) { - case DATA_ID_FAS: - if (_battery_data_ready) { - switch (_fas_call) { - case 0: - send_batt_volts(); - break; - case 1: - send_current(); - break; - } - _fas_call++; - if (_fas_call > 1) { - _fas_call = 0; - } - _battery_data_ready = false; - } - break; - case DATA_ID_GPS: - if (_gps_data_ready) { - switch (_gps_call) { - case 0: - send_gps_lat_dd(); - break; - case 1: - send_gps_lat_mm(); - break; - case 2: - send_gps_lat_ns(); - break; - case 3: - send_gps_lon_dd(); - break; - case 4: - send_gps_lon_mm(); - break; - case 5: - send_gps_lon_ew(); - break; - case 6: - send_gps_speed_meter(); - break; - case 7: - send_gps_speed_cm(); - break; - case 8: - send_gps_alt_meter(); - break; - case 9: - send_gps_alt_cm(); - break; - case 10: - send_heading(); - break; - } - - _gps_call++; - if (_gps_call > 10) { - _gps_call = 0; - _gps_data_ready = false; - } - } - break; - case DATA_ID_VARIO: - if (_baro_data_ready) { - switch (_vario_call) { - case 0 : - send_baro_alt_m(); - break; - case 1: - send_baro_alt_cm(); - break; - } - _vario_call ++; - if (_vario_call > 1) { - _vario_call = 0; - _baro_data_ready = false; - } - } - break; - case DATA_ID_SP2UR: - switch (_various_call) { - case 0 : - if ( _sats_data_ready ) { - send_gps_sats(); - _sats_data_ready = false; - } - break; - case 1: - if ( _mode_data_ready ) { - send_mode(); - _mode_data_ready = false; - } - break; - } - _various_call++; - if (_various_call > 1) { - _various_call = 0; - } - break; - } - _sport_status = 0; - } - } -} diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index f1a99e3e00..06dbb54118 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -70,71 +70,77 @@ #define SPORT_START_FRAME 0x7E -#define HUB_PORT true -#define S_PORT false - class AP_Frsky_Telem { public: //constructor AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery); - // these enums must match up with TELEM2_PROTOCOL in vehicle code + // supported protocols enum FrSkyProtocol { FrSkyUnknown = 0, FrSkyDPORT = 2, FrSkySPORT = 3 }; + // init - perform require initialisation including detecting which protocol to use void init(const AP_SerialManager& serial_manager); - void send_frames(uint8_t control_mode); + // send_frames - sends updates down telemetry link for both DPORT and SPORT protocols + // should be called by main program at 50hz to allow poll for serial bytes + // coming from the receiver for the SPort protocol + void send_frames(uint8_t control_mode); private: - void calc_crc (uint8_t byte); - void send_crc(); + // send_hub_frame - main transmission function when protocol is FrSkyDPORT + void send_hub_frame(); + // sport_tick - main call to send updates to transmitter when protocol is FrSkySPORT + // called by scheduler at a high rate + void sport_tick(); + + // methods related to the nuts-and-bolts of sending data + void calc_crc(uint8_t byte); + void send_crc(); void frsky_send_byte(uint8_t value); void frsky_send_hub_startstop(); void frsky_send_sport_prim(); - void frsky_send_data(uint8_t id, int16_t data); + // methods to convert flight controller data to frsky telemetry format void calc_baro_alt(); float frsky_format_gps(float dec); void calc_gps_position(); void calc_battery(); void calc_gps_sats(); - void send_gps_sats (void); - void send_mode (void); - void send_baro_alt_m (void); - void send_baro_alt_cm (void); - void send_batt_remain (void); - void send_batt_volts (void); - void send_current (void); - void send_prearm_error (void); - void send_heading (void); - void send_gps_lat_dd (void); - void send_gps_lat_mm (void); - void send_gps_lat_ns (void); - void send_gps_lon_dd (void); - void send_gps_lon_mm (void); - void send_gps_lon_ew (void); - void send_gps_speed_meter (void); - void send_gps_speed_cm (void); - void send_gps_alt_meter (void); - void send_gps_alt_cm (void); + // methods to send individual pieces of data down telemetry link + void send_gps_sats(void); + void send_mode(void); + void send_baro_alt_m(void); + void send_baro_alt_cm(void); + void send_batt_remain(void); + void send_batt_volts(void); + void send_current(void); + void send_prearm_error(void); + void send_heading(void); + void send_gps_lat_dd(void); + void send_gps_lat_mm(void); + void send_gps_lat_ns(void); + void send_gps_lon_dd(void); + void send_gps_lon_mm(void); + void send_gps_lon_ew(void); + void send_gps_speed_meter(void); + void send_gps_speed_cm(void); + void send_gps_alt_meter(void); + void send_gps_alt_cm(void); - void send_hub_frame(); - void sport_tick (); - - AP_AHRS &_ahrs; - AP_BattMonitor &_battery; - AP_HAL::UARTDriver *_port; - bool _initialised; - enum FrSkyProtocol _protocol; + AP_AHRS &_ahrs; // reference to attitude estimate + AP_BattMonitor &_battery; // reference to battery monitor object + AP_HAL::UARTDriver *_port; // UART used to send data to receiver + bool _initialised; // true when we have detected the protocol to use + enum FrSkyProtocol _protocol; // protocol used - detected using SerialManager's SERIALX_PROTOCOL parameter uint16_t _crc; uint32_t _last_frame1_ms; @@ -157,18 +163,18 @@ private: uint16_t _londddmm; uint16_t _lonmmmm; uint16_t _alt_gps_meters; - uint16_t _alt_gps_cm ; + uint16_t _alt_gps_cm; int16_t _speed_in_meter; uint16_t _speed_in_centimeter; bool _baro_data_ready; - int16_t _baro_alt_meters ; - uint16_t _baro_alt_cm ; + int16_t _baro_alt_meters; + uint16_t _baro_alt_cm; bool _mode_data_ready; uint8_t _mode; - uint8_t _fas_call ; + uint8_t _fas_call; uint8_t _gps_call; uint8_t _vario_call; uint8_t _various_call;