2020-09-28 01:31:38 -03:00
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include "AP_Frsky_Backend.h"
|
|
|
|
|
2022-04-16 23:54:07 -03:00
|
|
|
#if AP_FRSKY_SPORT_TELEM_ENABLED
|
|
|
|
|
2020-09-28 18:53:17 -03:00
|
|
|
class AP_Frsky_SPort : public AP_Frsky_Backend
|
|
|
|
{
|
2020-09-28 01:31:38 -03:00
|
|
|
|
|
|
|
public:
|
|
|
|
|
2020-12-20 15:57:08 -04:00
|
|
|
AP_Frsky_SPort(AP_HAL::UARTDriver *port) : AP_Frsky_Backend(port) {
|
|
|
|
singleton = this;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Do not allow copies */
|
2022-09-30 06:50:43 -03:00
|
|
|
CLASS_NO_COPY(AP_Frsky_SPort);
|
2020-09-28 01:31:38 -03:00
|
|
|
|
|
|
|
void send() override;
|
2020-12-20 15:57:08 -04:00
|
|
|
// send an sport packet by responding to the specified polled sensor
|
|
|
|
bool sport_telemetry_push(const uint8_t sensor, const uint8_t frame, const uint16_t appid, const int32_t data);
|
|
|
|
// utility method to pack numbers in a compact size
|
|
|
|
uint16_t prep_number(int32_t const number, const uint8_t digits, const uint8_t power);
|
2021-06-15 09:59:43 -03:00
|
|
|
|
2020-12-20 15:57:08 -04:00
|
|
|
static AP_Frsky_SPort *get_singleton(void) {
|
|
|
|
return singleton;
|
|
|
|
}
|
2020-09-28 01:31:38 -03:00
|
|
|
|
|
|
|
protected:
|
|
|
|
|
|
|
|
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
|
|
|
|
|
2021-07-06 16:47:35 -03:00
|
|
|
struct {
|
|
|
|
bool send_latitude;
|
|
|
|
bool send_airspeed; // toggles 0x5005 between airspeed and groundspeed
|
2020-09-28 01:31:38 -03:00
|
|
|
uint32_t gps_lng_sample;
|
|
|
|
uint8_t new_byte;
|
|
|
|
} _passthrough;
|
|
|
|
|
2020-09-30 22:24:10 -03:00
|
|
|
uint32_t calc_gps_latlng(bool &send_latitude);
|
2020-09-24 02:33:06 -03:00
|
|
|
static uint8_t calc_sensor_id(const uint8_t physical_id);
|
|
|
|
|
2020-12-20 15:57:08 -04:00
|
|
|
struct {
|
|
|
|
sport_packet_t packet;
|
|
|
|
bool pending = false;
|
|
|
|
HAL_Semaphore sem;
|
|
|
|
} _sport_push_buffer;
|
|
|
|
|
2020-09-28 01:31:38 -03:00
|
|
|
private:
|
|
|
|
|
2020-09-28 18:53:17 -03:00
|
|
|
struct {
|
2020-09-28 01:31:38 -03:00
|
|
|
bool sport_status;
|
|
|
|
bool gps_refresh;
|
|
|
|
bool vario_refresh;
|
|
|
|
uint8_t fas_call;
|
|
|
|
uint8_t gps_call;
|
|
|
|
uint8_t vario_call;
|
|
|
|
uint8_t various_call;
|
2021-01-29 04:50:59 -04:00
|
|
|
uint8_t rpm_call;
|
2020-09-28 01:31:38 -03:00
|
|
|
} _SPort;
|
2020-12-20 15:57:08 -04:00
|
|
|
|
|
|
|
static AP_Frsky_SPort *singleton;
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
namespace AP {
|
|
|
|
AP_Frsky_SPort *frsky_sport();
|
2020-09-28 01:31:38 -03:00
|
|
|
};
|
2022-04-16 23:54:07 -03:00
|
|
|
|
|
|
|
#endif // AP_FRSKY_SPORT_TELEM_ENABLED
|