AP_Frsky_Telem: add SBUS support
add sbus support using a timer on a thread
This commit is contained in:
Matthias Badaire
Andrew Tridgell
parent
ee00302b62
commit
f59f85d4a4
@ -1,3 +1,4 @@
|
|||||||
|
// -*- 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>
|
Inspired by work done here https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado <px4@sradonia.net>
|
||||||
@ -18,44 +19,99 @@
|
|||||||
|
|
||||||
/*
|
/*
|
||||||
FRSKY Telemetry library
|
FRSKY Telemetry library
|
||||||
for the moment it only handle hub port telemetry
|
|
||||||
the sport reference are only here to simulate the frsky module and use opentx simulator. it will eventually be removed
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <AP_Frsky_Telem.h>
|
#include <AP_Frsky_Telem.h>
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
void AP_Frsky_Telem::init(AP_HAL::UARTDriver *port, uint8_t frsky_type)
|
void AP_Frsky_Telem::init(AP_HAL::UARTDriver *port, enum FrSkyProtocol protocol)
|
||||||
{
|
{
|
||||||
if (port == NULL) {
|
if (port == NULL) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
_port = port;
|
_port = port;
|
||||||
_port->begin(9600);
|
_protocol = protocol;
|
||||||
_initialised = true;
|
if (_protocol == FrSkySPORT) {
|
||||||
}
|
_port->begin(57600);
|
||||||
|
_gps_call = 0;
|
||||||
void AP_Frsky_Telem::frsky_send_byte(uint8_t value)
|
_fas_call = 0;
|
||||||
{
|
_vario_call = 0 ;
|
||||||
const uint8_t x5E[] = { 0x5D, 0x3E };
|
_various_call = 0 ;
|
||||||
const uint8_t x5D[] = { 0x5D, 0x3D };
|
_gps_data_ready = false;
|
||||||
switch (value) {
|
_battery_data_ready = false;
|
||||||
case 0x5E:
|
_baro_data_ready = false;
|
||||||
_port->write( x5E, sizeof(x5E));
|
_mode_data_ready = false;
|
||||||
break;
|
_sats_data_ready = false;
|
||||||
|
_sport_status = 0;
|
||||||
case 0x5D:
|
hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&AP_Frsky_Telem::sport_tick));
|
||||||
_port->write( x5D, sizeof(x5D));
|
}else {// if this is D-port then spec says 9600 baud
|
||||||
break;
|
_port->begin(9600);
|
||||||
|
_initialised = true;
|
||||||
default:
|
|
||||||
_port->write(&value, sizeof(value));
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
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.
|
* Sends a 0x5E start/stop byte.
|
||||||
*/
|
*/
|
||||||
@ -65,61 +121,65 @@ void AP_Frsky_Telem::frsky_send_hub_startstop()
|
|||||||
_port->write(&c, sizeof(c));
|
_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.
|
* Sends one data id/value pair.
|
||||||
*/
|
*/
|
||||||
void AP_Frsky_Telem::frsky_send_data(uint8_t id, int16_t data)
|
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 */
|
/* Cast data to unsigned, because signed shift might behave incorrectly */
|
||||||
uint16_t udata = data;
|
uint16_t udata = data;
|
||||||
|
|
||||||
frsky_send_hub_startstop();
|
if (_protocol == FrSkySPORT) {
|
||||||
frsky_send_byte(id);
|
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); /* LSB */
|
||||||
frsky_send_byte(udata >> 8); /* MSB */
|
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();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/*
|
||||||
* Sends frame 1 (every 200ms):
|
* frsky_send_baro_meters : send altitude in Meters based on ahrs estimate
|
||||||
* barometer altitude, battery voltage & current
|
|
||||||
*/
|
*/
|
||||||
void AP_Frsky_Telem::frsky_send_frame1(uint8_t mode)
|
void AP_Frsky_Telem::calc_baro_alt(){
|
||||||
{
|
|
||||||
struct Location loc;
|
struct Location loc;
|
||||||
float battery_amps = _battery.current_amps();
|
|
||||||
float baro_alt = 0; // in meters
|
float baro_alt = 0; // in meters
|
||||||
bool posok = _ahrs.get_position(loc);
|
bool posok = _ahrs.get_position(loc);
|
||||||
if (posok) {
|
if (posok) {
|
||||||
baro_alt = loc.alt * 0.01f; // convert to meters
|
baro_alt = loc.alt * 0.01f; // convert to meters
|
||||||
if (!loc.flags.relative_alt) {
|
if (!loc.flags.relative_alt) {
|
||||||
baro_alt -= _ahrs.get_home().alt * 0.01f; // subtract home if set
|
baro_alt -= _ahrs.get_home().alt * 0.01f; // subtract home if set
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
const AP_GPS &gps = _ahrs.get_gps();
|
|
||||||
|
|
||||||
// GPS status is sent as num_sats*10 + status, to fit into a
|
|
||||||
// uint8_t
|
|
||||||
uint8_t T2 = gps.num_sats() * 10 + gps.status();
|
|
||||||
|
|
||||||
frsky_send_data(FRSKY_ID_TEMP1, mode);
|
|
||||||
frsky_send_data(FRSKY_ID_TEMP2, T2);
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Note that this isn't actually barometric altitdue, it is the
|
Note that this isn't actually barometric altitude, it is the
|
||||||
AHRS estimate of altitdue above home.
|
inertial nav estimate of altitude above home.
|
||||||
*/
|
*/
|
||||||
uint16_t baro_alt_meters = (uint16_t)baro_alt;
|
_baro_alt_meters = (int16_t)baro_alt;
|
||||||
uint16_t baro_alt_cm = (baro_alt - baro_alt_meters) * 100;
|
_baro_alt_cm = (baro_alt - abs(_baro_alt_meters)) * 100;
|
||||||
frsky_send_data(FRSKY_ID_BARO_ALT_BP, baro_alt_meters);
|
|
||||||
frsky_send_data(FRSKY_ID_BARO_ALT_AP, baro_alt_cm);
|
|
||||||
|
|
||||||
frsky_send_data(FRSKY_ID_FUEL, roundf(_battery.capacity_remaining_pct()));
|
|
||||||
|
|
||||||
frsky_send_data(FRSKY_ID_VFAS, roundf(_battery.voltage() * 10.0f));
|
|
||||||
frsky_send_data(FRSKY_ID_CURRENT, (battery_amps < 0) ? 0 : roundf(battery_amps * 10.0f));
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -131,68 +191,241 @@ float AP_Frsky_Telem::frsky_format_gps(float dec)
|
|||||||
return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
|
return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/*
|
||||||
* Sends frame 2 (every 1000ms):
|
* prepare lattitude and longitude information stored in member variables
|
||||||
* course(heading), latitude, longitude, ground speed, GPS altitude
|
|
||||||
*/
|
*/
|
||||||
void AP_Frsky_Telem::frsky_send_frame2()
|
|
||||||
{
|
|
||||||
|
|
||||||
// we send the heading based on the ahrs instead of GPS course which is not very usefull
|
void AP_Frsky_Telem::calc_gps_position(){
|
||||||
uint16_t course_in_degrees = (_ahrs.yaw_sensor / 100) % 360;
|
|
||||||
frsky_send_data(FRSKY_ID_GPS_COURS_BP, course_in_degrees);
|
_course_in_degrees = (_ahrs.yaw_sensor / 100) % 360;
|
||||||
|
|
||||||
const AP_GPS &gps = _ahrs.get_gps();
|
const AP_GPS &gps = _ahrs.get_gps();
|
||||||
bool posok = (gps.status() >= 3);
|
float lat;
|
||||||
if (posok){
|
float lon ;
|
||||||
// send formatted frame
|
float alt ;
|
||||||
float lat = 0, lon = 0, alt = 0, speed= 0;
|
float speed;
|
||||||
char lat_ns = 0, lon_ew = 0;
|
_pos_gps_ok = (gps.status() >= 3);
|
||||||
|
if (_pos_gps_ok){
|
||||||
Location loc = gps.location();//get gps instance 0
|
Location loc = gps.location();//get gps instance 0
|
||||||
|
|
||||||
lat = frsky_format_gps(fabsf(loc.lat/10000000.0));
|
lat = frsky_format_gps(fabsf(loc.lat/10000000.0));
|
||||||
uint16_t latdddmm = lat;
|
_latdddmm = lat;
|
||||||
uint16_t latmmmm = (lat - latdddmm) * 10000;
|
_latmmmm = (lat - _latdddmm) * 10000;
|
||||||
lat_ns = (loc.lat < 0) ? 'S' : 'N';
|
_lat_ns = (loc.lat < 0) ? 'S' : 'N';
|
||||||
|
|
||||||
lon = frsky_format_gps(fabsf(loc.lng/10000000.0));
|
lon = frsky_format_gps(fabsf(loc.lng/10000000.0));
|
||||||
uint16_t londddmm = lon;
|
_londddmm = lon;
|
||||||
uint16_t lonmmmm = (lon - londddmm) * 10000;
|
_lonmmmm = (lon - _londddmm) * 10000;
|
||||||
lon_ew = (loc.lng < 0) ? 'W' : 'E';
|
_lon_ew = (loc.lng < 0) ? 'W' : 'E';
|
||||||
|
|
||||||
alt = loc.alt / 100;
|
alt = loc.alt * 0.01f;
|
||||||
uint16_t alt_gps_meters = alt;
|
_alt_gps_meters = (int16_t)alt;
|
||||||
uint16_t alt_gps_cm = (alt - alt_gps_meters) * 100;
|
_alt_gps_cm = (alt - abs(_alt_gps_meters)) * 100;
|
||||||
|
|
||||||
speed = gps.ground_speed ();
|
speed = gps.ground_speed ();
|
||||||
uint16_t speed_in_meter = speed;
|
_speed_in_meter = speed;
|
||||||
uint16_t speed_in_centimeter = (speed - speed_in_meter) * 100;
|
_speed_in_centimeter = (speed - _speed_in_meter) * 100;
|
||||||
|
} else {
|
||||||
|
_latdddmm = 0;
|
||||||
frsky_send_data(FRSKY_ID_GPS_LAT_BP, latdddmm);
|
_latmmmm = 0;
|
||||||
frsky_send_data(FRSKY_ID_GPS_LAT_AP, latmmmm);
|
_lat_ns = 0;
|
||||||
frsky_send_data(FRSKY_ID_GPS_LAT_NS, lat_ns);
|
_londddmm = 0;
|
||||||
|
_lonmmmm = 0;
|
||||||
frsky_send_data(FRSKY_ID_GPS_LONG_BP, londddmm);
|
_alt_gps_meters = 0;
|
||||||
frsky_send_data(FRSKY_ID_GPS_LONG_AP, lonmmmm);
|
_alt_gps_cm = 0;
|
||||||
frsky_send_data(FRSKY_ID_GPS_LONG_EW, lon_ew);
|
_speed_in_meter = 0;
|
||||||
|
_speed_in_centimeter = 0;
|
||||||
frsky_send_data(FRSKY_ID_GPS_SPEED_BP, speed_in_meter);
|
|
||||||
frsky_send_data(FRSKY_ID_GPS_SPEED_AP, speed_in_centimeter);
|
|
||||||
|
|
||||||
frsky_send_data(FRSKY_ID_GPS_ALT_BP, alt_gps_meters);
|
|
||||||
frsky_send_data(FRSKY_ID_GPS_ALT_AP, alt_gps_cm);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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);
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
check for input bytes from sport
|
* prepare sats information stored in member variables
|
||||||
*/
|
*/
|
||||||
void AP_Frsky_Telem::check_sport_input(void)
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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 ();
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -200,29 +433,168 @@ void AP_Frsky_Telem::check_sport_input(void)
|
|||||||
send telemetry frames. Should be called at 50Hz. The high rate is to
|
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
|
allow this code to poll for serial bytes coming from the receiver
|
||||||
for the SPort protocol
|
for the SPort protocol
|
||||||
*/
|
*/
|
||||||
void AP_Frsky_Telem::send_frames(uint8_t control_mode, enum FrSkyProtocol protocol)
|
void AP_Frsky_Telem::send_frames(uint8_t control_mode)
|
||||||
{
|
{
|
||||||
if (!_initialised) {
|
if (!_initialised) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (protocol == FrSkySPORT) {
|
if (_protocol == FrSkySPORT) {
|
||||||
// check for sport bytes
|
if (!_mode_data_ready){
|
||||||
check_sport_input();
|
_mode=control_mode;
|
||||||
}
|
_mode_data_ready = true;
|
||||||
|
}
|
||||||
uint32_t now = hal.scheduler->millis();
|
if (!_baro_data_ready){
|
||||||
|
calc_baro_alt();
|
||||||
// send frame1 every 200ms
|
_baro_data_ready = true;
|
||||||
if (now - _last_frame1_ms > 200) {
|
}
|
||||||
_last_frame1_ms = now;
|
if (!_gps_data_ready){
|
||||||
frsky_send_frame1(control_mode);
|
calc_gps_position();
|
||||||
}
|
_gps_data_ready = true;
|
||||||
|
}
|
||||||
// send frame2 every second
|
if (!_sats_data_ready){
|
||||||
if (now - _last_frame2_ms > 1000) {
|
calc_gps_sats();
|
||||||
_last_frame2_ms = now;
|
_sats_data_ready = true;
|
||||||
frsky_send_frame2();
|
}
|
||||||
|
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;
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
/*
|
/*
|
||||||
This program is free software: you can redistribute it and/or modify
|
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
|
it under the terms of the GNU General Public License as published by
|
||||||
@ -57,18 +58,29 @@
|
|||||||
#define FRSKY_ID_VOLTS_BP 0x3A
|
#define FRSKY_ID_VOLTS_BP 0x3A
|
||||||
#define FRSKY_ID_VOLTS_AP 0x3B
|
#define FRSKY_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
|
||||||
|
|
||||||
#define HUB_PORT true
|
#define HUB_PORT true
|
||||||
#define S_PORT false
|
#define S_PORT false
|
||||||
|
|
||||||
class AP_Frsky_Telem
|
class AP_Frsky_Telem
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
//constructor
|
//constructor
|
||||||
AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) :
|
AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) :
|
||||||
_initialised(false),
|
_initialised(false),
|
||||||
_ahrs(ahrs),
|
_ahrs(ahrs),
|
||||||
_battery(battery)
|
_battery(battery)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
// these enums must match up with TELEM2_PROTOCOL in vehicle code
|
// these enums must match up with TELEM2_PROTOCOL in vehicle code
|
||||||
enum FrSkyProtocol {
|
enum FrSkyProtocol {
|
||||||
@ -76,25 +88,93 @@ class AP_Frsky_Telem
|
|||||||
FrSkySPORT = 3
|
FrSkySPORT = 3
|
||||||
};
|
};
|
||||||
|
|
||||||
void init(AP_HAL::UARTDriver *port, uint8_t frsky_type);
|
void init(AP_HAL::UARTDriver *port, enum FrSkyProtocol protocol);
|
||||||
void send_frames(uint8_t control_mode, enum FrSkyProtocol protocol);
|
void send_frames(uint8_t control_mode);
|
||||||
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void frsky_send_data(uint8_t id, int16_t data);
|
|
||||||
void frsky_send_frame1(uint8_t mode);
|
void calc_crc (uint8_t byte);
|
||||||
void frsky_send_frame2();
|
void send_crc();
|
||||||
void check_sport_input(void);
|
|
||||||
|
|
||||||
float frsky_format_gps(float dec);
|
|
||||||
void frsky_send_byte(uint8_t value);
|
void frsky_send_byte(uint8_t value);
|
||||||
void frsky_send_hub_startstop();
|
void frsky_send_hub_startstop();
|
||||||
|
void frsky_send_sport_prim();
|
||||||
|
|
||||||
|
void frsky_send_data(uint8_t id, int16_t data);
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
void send_hub_frame();
|
||||||
|
void sport_tick ();
|
||||||
|
|
||||||
AP_HAL::UARTDriver *_port;
|
AP_HAL::UARTDriver *_port;
|
||||||
bool _initialised;
|
bool _initialised;
|
||||||
AP_AHRS &_ahrs;
|
AP_AHRS &_ahrs;
|
||||||
AP_BattMonitor &_battery;
|
AP_BattMonitor &_battery;
|
||||||
|
enum FrSkyProtocol _protocol;
|
||||||
|
|
||||||
|
uint16_t _crc;
|
||||||
uint32_t _last_frame1_ms;
|
uint32_t _last_frame1_ms;
|
||||||
uint32_t _last_frame2_ms;
|
uint32_t _last_frame2_ms;
|
||||||
|
|
||||||
|
bool _battery_data_ready;
|
||||||
|
uint16_t _batt_remaining;
|
||||||
|
uint16_t _batt_volts;
|
||||||
|
uint16_t _batt_amps;
|
||||||
|
|
||||||
|
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;
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user