// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
Inspired by work done here https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado <px4@sradonia.net>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
FRSKY Telemetry library
*/
#include "AP_Frsky_Telem.h"
extern const AP_HAL::HAL& hal;
//constructor
AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) :
_ahrs(ahrs),
_battery(battery),
_port(NULL),
_initialised_uart(false),
_protocol(FrSkyUnknown),
_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)
{}
// init - perform require initialisation including detecting which protocol to use
void AP_Frsky_Telem::init(const AP_SerialManager& serial_manager)
{
// 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));
}
if (_port != NULL) {
// 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
*/
void AP_Frsky_Telem::send_frames(uint8_t control_mode)
{
// return immediately if not initialised
if (!_initialised_uart) {
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();
}
}
/*
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 = 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)
{
// 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 {
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)
{
_crc += byte; //0-1FF
_crc += _crc >> 8; //0-100
_crc &= 0x00ff;
_crc += _crc >> 8; //0-0FF
_crc &= 0x00ff;
}
/*
* send the crc at the end of the S-PORT frame
*/
void AP_Frsky_Telem::send_crc()
{
frsky_send_byte(0x00ff-_crc);
_crc = 0;
}
/*
send 1 byte and do the byte stuffing Frsky stuff
This can send more than 1 byte eventually
*/
void AP_Frsky_Telem::frsky_send_byte(uint8_t value)
{
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;
case 0x5D:
_port->write( x5D, sizeof(x5D));
break;
default:
_port->write(&value, sizeof(value));
break;
}
} 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;
}
}
}
/**
* 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));
}
/*
add sport protocol for frsky tx module
*/
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)
{
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
*/
void AP_Frsky_Telem::calc_gps_position()
{
_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';
lon = frsky_format_gps(fabsf(loc.lng/10000000.0f));
_londddmm = lon;
_lonmmmm = (lon - _londddmm) * 10000;
_lon_ew = (loc.lng < 0) ? 'W' : 'E';
alt = loc.alt * 0.01f;
_alt_gps_meters = (int16_t)alt;
_alt_gps_cm = (alt - abs(_alt_gps_meters)) * 100;
speed = gps.ground_speed();
_speed_in_meter = speed;
_speed_in_centimeter = (speed - _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;
}
}
/*
* 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 lattitude 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 lattitude 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);
}