aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_FrSky: simplified lib rework

Browse Source
This commit is contained in:
floaledm 2016-05-03 12:43:16 -05:00 committed by Andrew Tridgell
parent f73fa1fc80
commit de2a7013b4
2 changed files with 340 additions and 719 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

840
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -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
*/
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
* send data
* for FrSky SPort protocol (X-receivers)
*/
void AP_Frsky_Telem::init_uart_for_sport()
void AP_Frsky_Telem::send_FrSky_SPort(void)
{
// sanity check protocol
if (_protocol != FrSkySPORT) {
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;
// initialise uart
_port->begin(AP_SERIALMANAGER_FRSKY_SPORT_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX);
_initialised_uart = true;
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 SENSOR_ID_FAS:
switch (fas_call) {
case 1:
frsky_send_data(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
break;
case 2:
frsky_send_data(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
break;
case 3:
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:
frsky_send_data(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part
break;
case 5:
frsky_send_data(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part
break;
case 6:
frsky_send_data(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information
break;
case 7:
frsky_send_data(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
break;
case 8:
frsky_send_data(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
break;
case 9:
frsky_send_data(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
break;
case 10:
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;
}
if (gps_call++ > 11) gps_call = 1;
break;
case SENSOR_ID_VARIO:
switch (vario_call) {
case 1 :
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;
}
if (vario_call++ > 2) vario_call = 1;
break;
case SENSOR_ID_SP2UR:
switch (various_call) {
case 1 :
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;
}
if (various_call++ > 2) various_call = 1;
break;
}
}
}
}
/*
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()
* 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::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_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();
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_frame2_ms > 1000) {
_last_frame2_ms = now;
send_heading();
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 (_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();
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
}
}
}
/*
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)
* 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) {
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;
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();
}
}
/*
simple crc implementation for FRSKY telem S-PORT
*/
void AP_Frsky_Telem::calc_crc(uint8_t byte)
* 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;
case 0x5D:
_port->write( x5D, sizeof(x5D));
break;
default:
_port->write(&value, sizeof(value));
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 {
//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;
} 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);
}
}
}
/**
* 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();
frsky_calc_crc(byte);
}
}
/*
* calc_baro_alt : send altitude in Meters based on ahrs estimate
* send one frame of FrSky data
*/
void AP_Frsky_Telem::calc_baro_alt()
void AP_Frsky_Telem::frsky_send_data(uint16_t id, uint32_t data)
{
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
}
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();
}
/*
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.
/*
* prepare altitude between vehicle and home location data
* for FrSky D and SPort protocols
*/
float AP_Frsky_Telem::frsky_format_gps(float dec)
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 {
current_height = 0;
}
_gps.alt_nav_meters = (int16_t)current_height;
_gps.alt_nav_cm = (current_height - _gps.alt_nav_meters) * 100;
}
/*
* format the decimal latitude/longitude to the required degrees/minutes
* for FrSky D and SPort protocols
*/
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 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);
}

217
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -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();
void calc_nav_alt(void);
float format_gps(float dec);
void calc_gps_position(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);
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_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_AHRS &_ahrs; // reference to attitude estimate
const AP_BattMonitor &_battery; // reference to battery monitor object
AP_HAL::UARTDriver *_port; // UART used to send data to receiver
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;
};