ardupilot/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

229 lines
6.2 KiB
C++

/*
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
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>
extern const AP_HAL::HAL& hal;
void AP_Frsky_Telem::init(AP_HAL::UARTDriver *port, uint8_t frsky_type)
{
if (port == NULL) {
return;
}
_port = port;
_port->begin(9600);
_initialised = true;
}
void AP_Frsky_Telem::frsky_send_byte(uint8_t value)
{
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;
}
}
/**
* 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));
}
/**
* Sends one data id/value pair.
*/
void AP_Frsky_Telem::frsky_send_data(uint8_t id, int16_t data)
{
/* Cast data to unsigned, because signed shift might behave incorrectly */
uint16_t udata = data;
frsky_send_hub_startstop();
frsky_send_byte(id);
frsky_send_byte(udata); /* LSB */
frsky_send_byte(udata >> 8); /* MSB */
}
/**
* Sends frame 1 (every 200ms):
* barometer altitude, battery voltage & current
*/
void AP_Frsky_Telem::frsky_send_frame1(uint8_t mode)
{
struct Location loc;
float battery_amps = _battery.current_amps();
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
}
}
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
AHRS estimate of altitdue above home.
*/
uint16_t baro_alt_meters = (uint16_t)baro_alt;
uint16_t baro_alt_cm = (baro_alt - 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));
}
/**
* 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;
}
/**
* Sends frame 2 (every 1000ms):
* 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
uint16_t course_in_degrees = (_ahrs.yaw_sensor / 100) % 360;
frsky_send_data(FRSKY_ID_GPS_COURS_BP, course_in_degrees);
const AP_GPS &gps = _ahrs.get_gps();
bool posok = (gps.status() >= 3);
if (posok){
// send formatted frame
float lat = 0, lon = 0, alt = 0, speed= 0;
char lat_ns = 0, lon_ew = 0;
Location loc = gps.location();//get gps instance 0
lat = frsky_format_gps(fabsf(loc.lat/10000000.0));
uint16_t latdddmm = lat;
uint16_t latmmmm = (lat - latdddmm) * 10000;
lat_ns = (loc.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(fabsf(loc.lng/10000000.0));
uint16_t londddmm = lon;
uint16_t lonmmmm = (lon - londddmm) * 10000;
lon_ew = (loc.lng < 0) ? 'W' : 'E';
alt = loc.alt / 100;
uint16_t alt_gps_meters = alt;
uint16_t alt_gps_cm = (alt - alt_gps_meters) * 100;
speed = gps.ground_speed ();
uint16_t speed_in_meter = speed;
uint16_t speed_in_centimeter = (speed - speed_in_meter) * 100;
frsky_send_data(FRSKY_ID_GPS_LAT_BP, latdddmm);
frsky_send_data(FRSKY_ID_GPS_LAT_AP, latmmmm);
frsky_send_data(FRSKY_ID_GPS_LAT_NS, lat_ns);
frsky_send_data(FRSKY_ID_GPS_LONG_BP, londddmm);
frsky_send_data(FRSKY_ID_GPS_LONG_AP, lonmmmm);
frsky_send_data(FRSKY_ID_GPS_LONG_EW, lon_ew);
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);
}
}
/*
check for input bytes from sport
*/
void AP_Frsky_Telem::check_sport_input(void)
{
}
/*
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
for the SPort protocol
*/
void AP_Frsky_Telem::send_frames(uint8_t control_mode, enum FrSkyProtocol protocol)
{
if (!_initialised) {
return;
}
if (protocol == FrSkySPORT) {
// check for sport bytes
check_sport_input();
}
uint32_t now = hal.scheduler->millis();
// send frame1 every 200ms
if (now - _last_frame1_ms > 200) {
_last_frame1_ms = now;
frsky_send_frame1(control_mode);
}
// send frame2 every second
if (now - _last_frame2_ms > 1000) {
_last_frame2_ms = now;
frsky_send_frame2();
}
}