AP_FrSky: simplified lib rework
This commit is contained in:
floaledm
Andrew Tridgell
parent
f73fa1fc80
commit
de2a7013b4
@ -24,666 +24,350 @@
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
//constructor
|
||||
AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) :
|
||||
AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery) :
|
||||
_ahrs(ahrs),
|
||||
_battery(battery),
|
||||
_port(NULL),
|
||||
_initialised_uart(false),
|
||||
_protocol(FrSkyUnknown),
|
||||
_protocol(),
|
||||
_crc(0),
|
||||
_last_frame1_ms(0),
|
||||
_last_frame2_ms(0),
|
||||
_battery_data_ready(false),
|
||||
_batt_remaining(0),
|
||||
_batt_volts(0),
|
||||
_batt_amps(0),
|
||||
_sats_data_ready(false),
|
||||
gps_sats(0),
|
||||
_gps_data_ready(false),
|
||||
_pos_gps_ok(false),
|
||||
_course_in_degrees(0),
|
||||
_lat_ns(0),
|
||||
_lon_ew(0),
|
||||
_latdddmm(0),
|
||||
_latmmmm(0),
|
||||
_londddmm(0),
|
||||
_lonmmmm(0),
|
||||
_alt_gps_meters(0),
|
||||
_alt_gps_cm(0),
|
||||
_speed_in_meter(0),
|
||||
_speed_in_centimeter(0),
|
||||
_baro_data_ready(false),
|
||||
_baro_alt_meters(0),
|
||||
_baro_alt_cm(0),
|
||||
_mode_data_ready(false),
|
||||
_mode(0),
|
||||
_fas_call(0),
|
||||
_gps_call(0),
|
||||
_vario_call(0),
|
||||
_various_call(0),
|
||||
_sport_status(0)
|
||||
_control_mode(),
|
||||
_gps()
|
||||
{}
|
||||
|
||||
// init - perform require initialisation including detecting which protocol to use
|
||||
void AP_Frsky_Telem::init(const AP_SerialManager& serial_manager)
|
||||
/*
|
||||
* init - perform required initialisation
|
||||
*/
|
||||
void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, uint8_t *control_mode)
|
||||
{
|
||||
// check for FRSky_DPort
|
||||
if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FRSky_DPort, 0))) {
|
||||
_protocol = FrSkyDPORT;
|
||||
_initialised_uart = true; // SerialManager initialises uart for us
|
||||
} else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FRSky_SPort, 0))) {
|
||||
// check for FRSky_SPort
|
||||
_protocol = FrSkySPORT;
|
||||
_gps_call = 0;
|
||||
_fas_call = 0;
|
||||
_vario_call = 0 ;
|
||||
_various_call = 0 ;
|
||||
_gps_data_ready = false;
|
||||
_battery_data_ready = false;
|
||||
_baro_data_ready = false;
|
||||
_mode_data_ready = false;
|
||||
_sats_data_ready = false;
|
||||
_sport_status = 0;
|
||||
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Frsky_Telem::sport_tick, void));
|
||||
// check for protocol configured for a serial port - only the first serial port with one of these protocols will then run (cannot have FrSky on multiple serial ports)
|
||||
if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) {
|
||||
_protocol = AP_SerialManager::SerialProtocol_FrSky_D; // FrSky D protocol (D-receivers)
|
||||
} else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) {
|
||||
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort; // FrSky SPort protocol (X-receivers)
|
||||
}
|
||||
|
||||
_control_mode = control_mode;
|
||||
|
||||
if (_port != NULL) {
|
||||
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Frsky_Telem::tick, void));
|
||||
// we don't want flow control for either protocol
|
||||
_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
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
|
||||
* send data
|
||||
* for FrSky SPort protocol (X-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::send_frames(uint8_t control_mode)
|
||||
void AP_Frsky_Telem::send_FrSky_SPort(void)
|
||||
{
|
||||
// return immediately if not initialised
|
||||
if (!_initialised_uart) {
|
||||
return;
|
||||
}
|
||||
// variables keeping track of which data to send
|
||||
static uint8_t fas_call = 1;
|
||||
static uint8_t gps_call = 1;
|
||||
static uint8_t vario_call = 1;
|
||||
static uint8_t various_call = 1;
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
init_uart_for_sport - initialise uart for use by sport
|
||||
this must be called from sport_tick which is called from the 1khz scheduler
|
||||
because the UART begin must be called from the same thread as it is used from
|
||||
*/
|
||||
void AP_Frsky_Telem::init_uart_for_sport()
|
||||
{
|
||||
// sanity check protocol
|
||||
if (_protocol != FrSkySPORT) {
|
||||
return;
|
||||
}
|
||||
|
||||
// initialise uart
|
||||
_port->begin(AP_SERIALMANAGER_FRSKY_SPORT_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX);
|
||||
_initialised_uart = true;
|
||||
}
|
||||
|
||||
/*
|
||||
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 = AP_HAL::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)
|
||||
{
|
||||
// check UART has been initialised
|
||||
if (!_initialised_uart) {
|
||||
init_uart_for_sport();
|
||||
}
|
||||
|
||||
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 {
|
||||
while (_port->available()) {
|
||||
uint8_t readbyte = _port->read();
|
||||
if (readbyte == 0x7E) { // byte 0x7E is the header of each poll request
|
||||
readbyte = _port->read();
|
||||
switch(readbyte) {
|
||||
case DATA_ID_FAS:
|
||||
if (_battery_data_ready) {
|
||||
switch (_fas_call) {
|
||||
case 0:
|
||||
send_batt_volts();
|
||||
break;
|
||||
case SENSOR_ID_FAS:
|
||||
switch (fas_call) {
|
||||
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();
|
||||
frsky_send_data(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
|
||||
break;
|
||||
case 2:
|
||||
send_gps_lat_ns();
|
||||
frsky_send_data(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
|
||||
break;
|
||||
case 3:
|
||||
send_gps_lon_dd();
|
||||
frsky_send_data(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption
|
||||
break;
|
||||
}
|
||||
if (fas_call++ > 3) fas_call = 0;
|
||||
break;
|
||||
case SENSOR_ID_GPS:
|
||||
switch (gps_call) {
|
||||
case 1:
|
||||
calc_gps_position(); // gps data is not recalculated until all of it has been sent
|
||||
frsky_send_data(DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
|
||||
break;
|
||||
case 2:
|
||||
frsky_send_data(DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part
|
||||
break;
|
||||
case 3:
|
||||
frsky_send_data(DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information
|
||||
break;
|
||||
case 4:
|
||||
send_gps_lon_mm();
|
||||
frsky_send_data(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part
|
||||
break;
|
||||
case 5:
|
||||
send_gps_lon_ew();
|
||||
frsky_send_data(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part
|
||||
break;
|
||||
case 6:
|
||||
send_gps_speed_meter();
|
||||
frsky_send_data(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information
|
||||
break;
|
||||
case 7:
|
||||
send_gps_speed_cm();
|
||||
frsky_send_data(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
|
||||
break;
|
||||
case 8:
|
||||
send_gps_alt_meter();
|
||||
frsky_send_data(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
|
||||
break;
|
||||
case 9:
|
||||
send_gps_alt_cm();
|
||||
frsky_send_data(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
|
||||
break;
|
||||
case 10:
|
||||
send_heading();
|
||||
frsky_send_data(DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimals
|
||||
break;
|
||||
case 11:
|
||||
frsky_send_data(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
|
||||
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();
|
||||
if (gps_call++ > 11) gps_call = 1;
|
||||
break;
|
||||
case SENSOR_ID_VARIO:
|
||||
switch (vario_call) {
|
||||
case 1 :
|
||||
send_baro_alt_cm();
|
||||
calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent
|
||||
frsky_send_data(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send altitude integer part
|
||||
break;
|
||||
case 2:
|
||||
frsky_send_data(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send altitude decimal part
|
||||
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;
|
||||
}
|
||||
if (vario_call++ > 2) vario_call = 1;
|
||||
break;
|
||||
case SENSOR_ID_SP2UR:
|
||||
switch (various_call) {
|
||||
case 1 :
|
||||
if ( _mode_data_ready ) {
|
||||
send_mode();
|
||||
_mode_data_ready = false;
|
||||
}
|
||||
frsky_send_data(DATA_ID_TEMP2, (uint16_t)(_ahrs.get_gps().num_sats() * 10 + _ahrs.get_gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
|
||||
break;
|
||||
case 2:
|
||||
frsky_send_data(DATA_ID_TEMP1, (uint16_t)*_control_mode); // send flight mode
|
||||
break;
|
||||
}
|
||||
_various_call++;
|
||||
if (_various_call > 1) {
|
||||
_various_call = 0;
|
||||
}
|
||||
if (various_call++ > 2) various_call = 1;
|
||||
break;
|
||||
}
|
||||
_sport_status = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
simple crc implementation for FRSKY telem S-PORT
|
||||
* send frame1 and frame2
|
||||
* one frame (frame1) is sent every 200ms with baro alt, nb sats, batt volts and amp, control_mode
|
||||
* a second frame (frame2) is sent every second (1000ms) with gps position data, and ahrs.yaw_sensor heading (instead of GPS heading)
|
||||
* for FrSky D protocol (D-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::calc_crc(uint8_t byte)
|
||||
void AP_Frsky_Telem::send_FrSky_D(void)
|
||||
{
|
||||
static uint32_t last_200ms_frame = 0;
|
||||
static uint32_t last_1000ms_frame = 0;
|
||||
|
||||
uint32_t now = AP_HAL::millis();
|
||||
// send frame1 every 200ms
|
||||
if (now - last_200ms_frame > 200) {
|
||||
last_200ms_frame = now;
|
||||
frsky_send_data(DATA_ID_TEMP2, (uint16_t)(_ahrs.get_gps().num_sats() * 10 + _ahrs.get_gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
|
||||
frsky_send_data(DATA_ID_TEMP1, (uint16_t)*_control_mode); // send flight mode
|
||||
frsky_send_data(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
|
||||
frsky_send_data(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
|
||||
frsky_send_data(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption
|
||||
calc_nav_alt();
|
||||
frsky_send_data(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send nav altitude integer part
|
||||
frsky_send_data(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send nav altitude decimal part
|
||||
}
|
||||
// send frame2 every second
|
||||
if (now - last_1000ms_frame > 1000) {
|
||||
last_1000ms_frame = now;
|
||||
frsky_send_data(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
|
||||
calc_gps_position();
|
||||
if (_ahrs.get_gps().status() >= 3) {
|
||||
frsky_send_data(DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
|
||||
frsky_send_data(DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part
|
||||
frsky_send_data(DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information
|
||||
frsky_send_data(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part
|
||||
frsky_send_data(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part
|
||||
frsky_send_data(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information
|
||||
frsky_send_data(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
|
||||
frsky_send_data(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
|
||||
frsky_send_data(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
|
||||
frsky_send_data(DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimal part
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* tick - main call to send data to the receiver (called by scheduler at 1kHz)
|
||||
*/
|
||||
void AP_Frsky_Telem::tick(void)
|
||||
{
|
||||
static bool initialised_uart = false; // true when we have detected the protocol and UART has been initialised
|
||||
// check UART has been initialised
|
||||
if (!initialised_uart) {
|
||||
// initialise uart (this must be called from within tick b/c the UART begin must be called from the same thread as it is used from)
|
||||
if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers)
|
||||
_port->begin(AP_SERIALMANAGER_FRSKY_D_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX);
|
||||
} else { // FrSky SPort protocol (X-receivers)
|
||||
_port->begin(AP_SERIALMANAGER_FRSKY_SPORT_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX);
|
||||
}
|
||||
initialised_uart = true;
|
||||
}
|
||||
|
||||
if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers)
|
||||
send_FrSky_D();
|
||||
} else if (_protocol == AP_SerialManager::SerialProtocol_FrSky_SPort) { // FrSky SPort protocol (X-receivers)
|
||||
send_FrSky_SPort();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* build up the frame's crc
|
||||
* for FrSky SPort protocol (X-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::frsky_calc_crc(uint8_t byte)
|
||||
{
|
||||
_crc += byte; //0-1FF
|
||||
_crc += _crc >> 8; //0-100
|
||||
_crc &= 0x00ff;
|
||||
_crc += _crc >> 8; //0-0FF
|
||||
_crc &= 0x00ff;
|
||||
_crc &= 0xFF;
|
||||
}
|
||||
|
||||
/*
|
||||
* send the crc at the end of the S-PORT frame
|
||||
* send the frame's crc at the end of the frame
|
||||
* for FrSky SPort protocol (X-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::send_crc()
|
||||
void AP_Frsky_Telem::frsky_send_crc(void)
|
||||
{
|
||||
frsky_send_byte(0x00ff-_crc);
|
||||
frsky_send_byte(0xFF - _crc);
|
||||
_crc = 0;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
send 1 byte and do the byte stuffing Frsky stuff
|
||||
This can send more than 1 byte eventually
|
||||
send 1 byte and do byte stuffing
|
||||
*/
|
||||
void AP_Frsky_Telem::frsky_send_byte(uint8_t value)
|
||||
void AP_Frsky_Telem::frsky_send_byte(uint8_t byte)
|
||||
{
|
||||
if (_protocol == FrSkyDPORT) {
|
||||
const uint8_t x5E[] = { 0x5D, 0x3E };
|
||||
const uint8_t x5D[] = { 0x5D, 0x3D };
|
||||
switch (value) {
|
||||
case 0x5E:
|
||||
_port->write( x5E, sizeof(x5E));
|
||||
break;
|
||||
if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers)
|
||||
if (byte == 0x5E) {
|
||||
_port->write(0x5D);
|
||||
_port->write(0x3E);
|
||||
} else if (byte == 0x5D) {
|
||||
_port->write(0x5D);
|
||||
_port->write(0x3D);
|
||||
} else {
|
||||
_port->write(byte);
|
||||
}
|
||||
} else { // FrSky SPort protocol (X-receivers)
|
||||
if (byte == 0x7E) {
|
||||
_port->write(0x7D);
|
||||
_port->write(0x5E);
|
||||
} else if (byte == 0x7D) {
|
||||
_port->write(0x7D);
|
||||
_port->write(0x5D);
|
||||
} else {
|
||||
_port->write(byte);
|
||||
}
|
||||
frsky_calc_crc(byte);
|
||||
}
|
||||
}
|
||||
|
||||
case 0x5D:
|
||||
_port->write( x5D, sizeof(x5D));
|
||||
break;
|
||||
/*
|
||||
* send one frame of FrSky data
|
||||
*/
|
||||
void AP_Frsky_Telem::frsky_send_data(uint16_t id, uint32_t data)
|
||||
{
|
||||
if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers)
|
||||
_port->write(0x5E); // send a 0x5E start byte
|
||||
uint8_t *bytes = (uint8_t*)&id;
|
||||
frsky_send_byte(bytes[0]);
|
||||
bytes = (uint8_t*)&data;
|
||||
frsky_send_byte(bytes[0]); // LSB
|
||||
frsky_send_byte(bytes[1]); // MSB
|
||||
} else { // FrSky SPort protocol (X-receivers)
|
||||
frsky_send_byte(0x10); // DATA_FRAME
|
||||
uint8_t *bytes = (uint8_t*)&id;
|
||||
frsky_send_byte(bytes[0]); // LSB
|
||||
frsky_send_byte(bytes[1]); // MSB
|
||||
bytes = (uint8_t*)&data;
|
||||
frsky_send_byte(bytes[0]); // LSB
|
||||
frsky_send_byte(bytes[1]);
|
||||
frsky_send_byte(bytes[2]);
|
||||
frsky_send_byte(bytes[3]); // MSB
|
||||
frsky_send_crc();
|
||||
}
|
||||
}
|
||||
|
||||
default:
|
||||
_port->write(&value, sizeof(value));
|
||||
break;
|
||||
/*
|
||||
* prepare altitude between vehicle and home location data
|
||||
* for FrSky D and SPort protocols
|
||||
*/
|
||||
void AP_Frsky_Telem::calc_nav_alt(void)
|
||||
{
|
||||
Location current_loc;
|
||||
float current_height; // in centimeters above home
|
||||
if (_ahrs.get_position(current_loc)) {
|
||||
if (current_loc.flags.relative_alt) {
|
||||
current_height = current_loc.alt*0.01f;
|
||||
} else {
|
||||
current_height = (current_loc.alt - _ahrs.get_home().alt)*0.01f;
|
||||
}
|
||||
} else {
|
||||
//SPORT
|
||||
calc_crc(value);
|
||||
const uint8_t x7E[] = { 0x7D, 0x5E };
|
||||
const uint8_t x7D[] = { 0x7D, 0x5D };
|
||||
switch (value) {
|
||||
case 0x7E:
|
||||
_port->write( x7E, sizeof(x7E));
|
||||
break;
|
||||
|
||||
case 0x7D:
|
||||
_port->write( x7D, sizeof(x7D));
|
||||
break;
|
||||
|
||||
default:
|
||||
_port->write(&value, sizeof(value));
|
||||
break;
|
||||
current_height = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends a 0x5E start/stop byte.
|
||||
*/
|
||||
void AP_Frsky_Telem::frsky_send_hub_startstop()
|
||||
{
|
||||
static const uint8_t c = 0x5E;
|
||||
_port->write(&c, sizeof(c));
|
||||
_gps.alt_nav_meters = (int16_t)current_height;
|
||||
_gps.alt_nav_cm = (current_height - _gps.alt_nav_meters) * 100;
|
||||
}
|
||||
|
||||
/*
|
||||
add sport protocol for frsky tx module
|
||||
* format the decimal latitude/longitude to the required degrees/minutes
|
||||
* for FrSky D and SPort protocols
|
||||
*/
|
||||
void AP_Frsky_Telem::frsky_send_sport_prim()
|
||||
{
|
||||
static const uint8_t c = 0x10;
|
||||
frsky_send_byte(c);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Sends one data id/value pair.
|
||||
*/
|
||||
void AP_Frsky_Telem::frsky_send_data(uint8_t id, int16_t data)
|
||||
{
|
||||
static const uint8_t zero = 0x0;
|
||||
|
||||
/* Cast data to unsigned, because signed shift might behave incorrectly */
|
||||
uint16_t udata = data;
|
||||
|
||||
if (_protocol == FrSkySPORT) {
|
||||
frsky_send_sport_prim();
|
||||
frsky_send_byte(id);
|
||||
frsky_send_byte(zero);
|
||||
} else {
|
||||
frsky_send_hub_startstop();
|
||||
frsky_send_byte(id);
|
||||
}
|
||||
|
||||
frsky_send_byte(udata); /* LSB */
|
||||
frsky_send_byte(udata >> 8); /* MSB */
|
||||
|
||||
if (_protocol == FrSkySPORT) {
|
||||
//Sport expect 32 bits data but we use only 16 bits data, so we send 0 for MSB
|
||||
frsky_send_byte(zero);
|
||||
frsky_send_byte(zero);
|
||||
send_crc();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* calc_baro_alt : send altitude in Meters based on ahrs estimate
|
||||
*/
|
||||
void AP_Frsky_Telem::calc_baro_alt()
|
||||
{
|
||||
struct Location loc;
|
||||
float baro_alt = 0; // in meters
|
||||
bool posok = _ahrs.get_position(loc);
|
||||
if (posok) {
|
||||
baro_alt = loc.alt * 0.01f; // convert to meters
|
||||
if (!loc.flags.relative_alt) {
|
||||
baro_alt -= _ahrs.get_home().alt * 0.01f; // subtract home if set
|
||||
}
|
||||
}
|
||||
/*
|
||||
Note that this isn't actually barometric altitude, it is the
|
||||
inertial nav estimate of altitude above home.
|
||||
*/
|
||||
_baro_alt_meters = (int16_t)baro_alt;
|
||||
_baro_alt_cm = (baro_alt - abs(_baro_alt_meters)) * 100;
|
||||
}
|
||||
|
||||
/**
|
||||
* Formats the decimal latitude/longitude to the required degrees/minutes.
|
||||
*/
|
||||
float AP_Frsky_Telem::frsky_format_gps(float dec)
|
||||
float AP_Frsky_Telem::format_gps(float dec)
|
||||
{
|
||||
uint8_t dm_deg = (uint8_t) dec;
|
||||
return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare latitude and longitude information stored in member variables
|
||||
* prepare gps data
|
||||
* for FrSky D and SPort protocols
|
||||
*/
|
||||
|
||||
void AP_Frsky_Telem::calc_gps_position()
|
||||
void AP_Frsky_Telem::calc_gps_position(void)
|
||||
{
|
||||
_course_in_degrees = (_ahrs.yaw_sensor / 100) % 360;
|
||||
|
||||
const AP_GPS &gps = _ahrs.get_gps();
|
||||
float lat;
|
||||
float lon;
|
||||
float alt;
|
||||
float speed;
|
||||
_pos_gps_ok = (gps.status() >= 3);
|
||||
if (_pos_gps_ok) {
|
||||
Location loc = gps.location();//get gps instance 0
|
||||
|
||||
lat = frsky_format_gps(fabsf(loc.lat/10000000.0f));
|
||||
_latdddmm = lat;
|
||||
_latmmmm = (lat - _latdddmm) * 10000;
|
||||
_lat_ns = (loc.lat < 0) ? 'S' : 'N';
|
||||
if (_ahrs.get_gps().status() >= 3) {
|
||||
Location loc = _ahrs.get_gps().location(); //get gps instance 0
|
||||
lat = format_gps(fabsf(loc.lat/10000000.0f));
|
||||
_gps.latdddmm = lat;
|
||||
_gps.latmmmm = (lat - _gps.latdddmm) * 10000;
|
||||
_gps.lat_ns = (loc.lat < 0) ? 'S' : 'N';
|
||||
|
||||
lon = frsky_format_gps(fabsf(loc.lng/10000000.0f));
|
||||
_londddmm = lon;
|
||||
_lonmmmm = (lon - _londddmm) * 10000;
|
||||
_lon_ew = (loc.lng < 0) ? 'W' : 'E';
|
||||
lon = format_gps(fabsf(loc.lng/10000000.0f));
|
||||
_gps.londddmm = lon;
|
||||
_gps.lonmmmm = (lon - _gps.londddmm) * 10000;
|
||||
_gps.lon_ew = (loc.lng < 0) ? 'W' : 'E';
|
||||
|
||||
alt = loc.alt * 0.01f;
|
||||
_alt_gps_meters = (int16_t)alt;
|
||||
_alt_gps_cm = (alt - _alt_gps_meters) * 100;
|
||||
_gps.alt_gps_meters = (int16_t)alt;
|
||||
_gps.alt_gps_cm = (alt - _gps.alt_gps_meters) * 100;
|
||||
|
||||
speed = gps.ground_speed();
|
||||
_speed_in_meter = speed;
|
||||
_speed_in_centimeter = (speed - _speed_in_meter) * 100;
|
||||
speed = _ahrs.get_gps().ground_speed();
|
||||
_gps.speed_in_meter = speed;
|
||||
_gps.speed_in_centimeter = (speed - _gps.speed_in_meter) * 100;
|
||||
} else {
|
||||
_latdddmm = 0;
|
||||
_latmmmm = 0;
|
||||
_lat_ns = 0;
|
||||
_londddmm = 0;
|
||||
_lonmmmm = 0;
|
||||
_alt_gps_meters = 0;
|
||||
_alt_gps_cm = 0;
|
||||
_speed_in_meter = 0;
|
||||
_speed_in_centimeter = 0;
|
||||
_gps.latdddmm = 0;
|
||||
_gps.latmmmm = 0;
|
||||
_gps.lat_ns = 0;
|
||||
_gps.londddmm = 0;
|
||||
_gps.lonmmmm = 0;
|
||||
_gps.alt_gps_meters = 0;
|
||||
_gps.alt_gps_cm = 0;
|
||||
_gps.speed_in_meter = 0;
|
||||
_gps.speed_in_centimeter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare battery information stored in member variables
|
||||
*/
|
||||
void AP_Frsky_Telem::calc_battery()
|
||||
{
|
||||
_batt_remaining = roundf(_battery.capacity_remaining_pct());
|
||||
_batt_volts = roundf(_battery.voltage() * 10.0f);
|
||||
_batt_amps = roundf(_battery.current_amps() * 10.0f);
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare sats information stored in member variables
|
||||
*/
|
||||
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();
|
||||
gps_sats = gps.num_sats() * 10 + gps.status();
|
||||
}
|
||||
|
||||
/*
|
||||
* send number of gps satellite and gps status eg: 73 means 7 satellite and 3d lock
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_sats()
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_TEMP2, gps_sats);
|
||||
}
|
||||
|
||||
/*
|
||||
* send control_mode as Temperature 1 (TEMP1)
|
||||
*/
|
||||
void AP_Frsky_Telem::send_mode(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_TEMP1, _mode);
|
||||
}
|
||||
|
||||
/*
|
||||
* send barometer altitude integer part . Initialize baro altitude
|
||||
*/
|
||||
void AP_Frsky_Telem::send_baro_alt_m(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_BARO_ALT_BP, _baro_alt_meters);
|
||||
}
|
||||
|
||||
/*
|
||||
* send barometer altitude decimal part
|
||||
*/
|
||||
void AP_Frsky_Telem::send_baro_alt_cm(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_BARO_ALT_AP, _baro_alt_cm);
|
||||
}
|
||||
|
||||
/*
|
||||
* send battery remaining
|
||||
*/
|
||||
void AP_Frsky_Telem::send_batt_remain(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_FUEL, _batt_remaining);
|
||||
}
|
||||
|
||||
/*
|
||||
* send battery voltage
|
||||
*/
|
||||
void AP_Frsky_Telem::send_batt_volts(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_VFAS, _batt_volts);
|
||||
}
|
||||
|
||||
/*
|
||||
* send current consumptiom
|
||||
*/
|
||||
void AP_Frsky_Telem::send_current(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_CURRENT, _batt_amps);
|
||||
}
|
||||
|
||||
/*
|
||||
* send heading in degree based on AHRS and not GPS
|
||||
*/
|
||||
void AP_Frsky_Telem::send_heading(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_COURS_BP, _course_in_degrees);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps latitude degree and minute integer part; Initialize gps info
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_lat_dd(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_LAT_BP, _latdddmm);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps latitude minutes decimal part
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_lat_mm(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_LAT_AP, _latmmmm);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps North / South information
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_lat_ns(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_LAT_NS, _lat_ns);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps longitude degree and minute integer part
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_lon_dd(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_LONG_BP, _londddmm);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps longitude minutes decimal part
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_lon_mm(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_LONG_AP, _lonmmmm);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps East / West information
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_lon_ew(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_LONG_EW, _lon_ew);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps speed integer part
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_speed_meter(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_SPEED_BP, _speed_in_meter);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps speed decimal part
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_speed_cm(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_SPEED_AP, _speed_in_centimeter);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps altitude integer part
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_alt_meter(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_ALT_BP, _alt_gps_meters);
|
||||
}
|
||||
|
||||
/*
|
||||
* send gps altitude decimals
|
||||
*/
|
||||
void AP_Frsky_Telem::send_gps_alt_cm(void)
|
||||
{
|
||||
frsky_send_data(FRSKY_ID_GPS_ALT_AP, _alt_gps_cm);
|
||||
}
|
||||
|
||||
@ -16,169 +16,106 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_BattMonitor/AP_BattMonitor.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
|
||||
/* FrSky sensor hub data IDs */
|
||||
#define FRSKY_ID_GPS_ALT_BP 0x01
|
||||
#define FRSKY_ID_TEMP1 0x02
|
||||
#define FRSKY_ID_RPM 0x03
|
||||
#define FRSKY_ID_FUEL 0x04
|
||||
#define FRSKY_ID_TEMP2 0x05
|
||||
#define FRSKY_ID_VOLTS 0x06
|
||||
#define FRSKY_ID_GPS_ALT_AP 0x09
|
||||
#define FRSKY_ID_BARO_ALT_BP 0x10
|
||||
#define FRSKY_ID_GPS_SPEED_BP 0x11
|
||||
#define FRSKY_ID_GPS_LONG_BP 0x12
|
||||
#define FRSKY_ID_GPS_LAT_BP 0x13
|
||||
#define FRSKY_ID_GPS_COURS_BP 0x14
|
||||
#define FRSKY_ID_GPS_DAY_MONTH 0x15
|
||||
#define FRSKY_ID_GPS_YEAR 0x16
|
||||
#define FRSKY_ID_GPS_HOUR_MIN 0x17
|
||||
#define FRSKY_ID_GPS_SEC 0x18
|
||||
#define FRSKY_ID_GPS_SPEED_AP 0x19
|
||||
#define FRSKY_ID_GPS_LONG_AP 0x1A
|
||||
#define FRSKY_ID_GPS_LAT_AP 0x1B
|
||||
#define FRSKY_ID_GPS_COURS_AP 0x1C
|
||||
#define FRSKY_ID_BARO_ALT_AP 0x21
|
||||
#define FRSKY_ID_GPS_LONG_EW 0x22
|
||||
#define FRSKY_ID_GPS_LAT_NS 0x23
|
||||
#define FRSKY_ID_ACCEL_X 0x24
|
||||
#define FRSKY_ID_ACCEL_Y 0x25
|
||||
#define FRSKY_ID_ACCEL_Z 0x26
|
||||
#define FRSKY_ID_CURRENT 0x28
|
||||
#define FRSKY_ID_VARIO 0x30
|
||||
#define FRSKY_ID_VFAS 0x39
|
||||
#define FRSKY_ID_VOLTS_BP 0x3A
|
||||
#define FRSKY_ID_VOLTS_AP 0x3B
|
||||
/* FrSky sensor hub data IDs
|
||||
for FrSky D protocol (D-receivers)
|
||||
*/
|
||||
#define DATA_ID_GPS_ALT_BP 0x01
|
||||
#define DATA_ID_TEMP1 0x02
|
||||
#define DATA_ID_RPM 0x03
|
||||
#define DATA_ID_FUEL 0x04
|
||||
#define DATA_ID_TEMP2 0x05
|
||||
#define DATA_ID_VOLTS 0x06
|
||||
#define DATA_ID_GPS_ALT_AP 0x09
|
||||
#define DATA_ID_BARO_ALT_BP 0x10
|
||||
#define DATA_ID_GPS_SPEED_BP 0x11
|
||||
#define DATA_ID_GPS_LONG_BP 0x12
|
||||
#define DATA_ID_GPS_LAT_BP 0x13
|
||||
#define DATA_ID_GPS_COURS_BP 0x14
|
||||
#define DATA_ID_GPS_DAY_MONTH 0x15
|
||||
#define DATA_ID_GPS_YEAR 0x16
|
||||
#define DATA_ID_GPS_HOUR_MIN 0x17
|
||||
#define DATA_ID_GPS_SEC 0x18
|
||||
#define DATA_ID_GPS_SPEED_AP 0x19
|
||||
#define DATA_ID_GPS_LONG_AP 0x1A
|
||||
#define DATA_ID_GPS_LAT_AP 0x1B
|
||||
#define DATA_ID_GPS_COURS_AP 0x1C
|
||||
#define DATA_ID_BARO_ALT_AP 0x21
|
||||
#define DATA_ID_GPS_LONG_EW 0x22
|
||||
#define DATA_ID_GPS_LAT_NS 0x23
|
||||
#define DATA_ID_ACCEL_X 0x24
|
||||
#define DATA_ID_ACCEL_Y 0x25
|
||||
#define DATA_ID_ACCEL_Z 0x26
|
||||
#define DATA_ID_CURRENT 0x28
|
||||
#define DATA_ID_VARIO 0x30
|
||||
#define DATA_ID_VFAS 0x39
|
||||
#define DATA_ID_VOLTS_BP 0x3A
|
||||
#define DATA_ID_VOLTS_AP 0x3B
|
||||
|
||||
// Default sensor data IDs (Physical IDs + CRC)
|
||||
#define DATA_ID_VARIO 0x00 // 0
|
||||
#define DATA_ID_FLVSS 0xA1 // 1
|
||||
#define DATA_ID_FAS 0x22 // 2
|
||||
#define DATA_ID_GPS 0x83 // 3
|
||||
#define DATA_ID_RPM 0xE4 // 4
|
||||
#define DATA_ID_SP2UH 0x45 // 5
|
||||
#define DATA_ID_SP2UR 0xC6 // 6
|
||||
|
||||
#define SPORT_START_FRAME 0x7E
|
||||
/* Standard FrSky Sensor IDs
|
||||
for FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers)
|
||||
*/
|
||||
#define SENSOR_ID_VARIO 0x00 // 0
|
||||
#define SENSOR_ID_FLVSS 0xA1 // 1
|
||||
#define SENSOR_ID_FAS 0x22 // 2
|
||||
#define SENSOR_ID_GPS 0x83 // 3
|
||||
#define SENSOR_ID_RPM 0xE4 // 4
|
||||
#define SENSOR_ID_SP2UH 0x45 // 5
|
||||
#define SENSOR_ID_SP2UR 0xC6 // 6
|
||||
|
||||
class AP_Frsky_Telem
|
||||
{
|
||||
public:
|
||||
//constructor
|
||||
AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery);
|
||||
|
||||
// supported protocols
|
||||
enum FrSkyProtocol {
|
||||
FrSkyUnknown = 0,
|
||||
FrSkyDPORT = 2,
|
||||
FrSkySPORT = 3
|
||||
};
|
||||
AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery);
|
||||
|
||||
// init - perform require initialisation including detecting which protocol to use
|
||||
void 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 send_frames(uint8_t control_mode);
|
||||
void init(const AP_SerialManager &serial_manager, uint8_t *control_mode);
|
||||
|
||||
private:
|
||||
// send_FrSky_SPort - main transmission function when protocol is send_FrSky_SPort
|
||||
void send_FrSky_SPort(void);
|
||||
// send_FrSky_D - main transmission function when protocol is FrSky_D
|
||||
void send_FrSky_D(void);
|
||||
|
||||
// init_uart_for_sport - initialise uart for use by sport
|
||||
void init_uart_for_sport();
|
||||
|
||||
// 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();
|
||||
// tick - main call to send updates to transmitter (called by scheduler at 1kHz)
|
||||
void tick(void);
|
||||
|
||||
// methods related to the nuts-and-bolts of sending data
|
||||
void calc_crc(uint8_t byte);
|
||||
void send_crc();
|
||||
void frsky_calc_crc(uint8_t byte);
|
||||
void frsky_send_crc(void);
|
||||
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);
|
||||
void frsky_send_data(uint16_t id, uint32_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();
|
||||
|
||||
// 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 calc_nav_alt(void);
|
||||
float format_gps(float dec);
|
||||
void calc_gps_position(void);
|
||||
|
||||
AP_AHRS &_ahrs; // reference to attitude estimate
|
||||
AP_BattMonitor &_battery; // reference to battery monitor object
|
||||
const AP_BattMonitor &_battery; // reference to battery monitor object
|
||||
AP_HAL::UARTDriver *_port; // UART used to send data to receiver
|
||||
bool _initialised_uart; // true when we have detected the protocol and UART has been initialised
|
||||
enum FrSkyProtocol _protocol; // protocol used - detected using SerialManager's SERIALX_PROTOCOL parameter
|
||||
AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter
|
||||
|
||||
uint16_t _crc;
|
||||
uint32_t _last_frame1_ms;
|
||||
uint32_t _last_frame2_ms;
|
||||
|
||||
bool _battery_data_ready;
|
||||
uint16_t _batt_remaining;
|
||||
uint16_t _batt_volts;
|
||||
uint16_t _batt_amps;
|
||||
uint8_t *_control_mode;
|
||||
|
||||
bool _sats_data_ready;
|
||||
uint16_t gps_sats;
|
||||
|
||||
bool _gps_data_ready;
|
||||
bool _pos_gps_ok;
|
||||
uint16_t _course_in_degrees;
|
||||
char _lat_ns, _lon_ew;
|
||||
uint16_t _latdddmm;
|
||||
uint16_t _latmmmm;
|
||||
uint16_t _londddmm;
|
||||
uint16_t _lonmmmm;
|
||||
uint16_t _alt_gps_meters;
|
||||
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;
|
||||
|
||||
bool _mode_data_ready;
|
||||
uint8_t _mode;
|
||||
|
||||
uint8_t _fas_call;
|
||||
uint8_t _gps_call;
|
||||
uint8_t _vario_call;
|
||||
uint8_t _various_call;
|
||||
|
||||
uint8_t _sport_status;
|
||||
struct
|
||||
{
|
||||
char lat_ns, lon_ew;
|
||||
uint16_t latdddmm;
|
||||
uint16_t latmmmm;
|
||||
uint16_t londddmm;
|
||||
uint16_t lonmmmm;
|
||||
uint16_t alt_gps_meters;
|
||||
uint16_t alt_gps_cm;
|
||||
uint16_t alt_nav_meters;
|
||||
uint16_t alt_nav_cm;
|
||||
int16_t speed_in_meter;
|
||||
uint16_t speed_in_centimeter;
|
||||
} _gps;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user