Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: added initial Frsky telemetry library

Browse Source 
based on work from the PX4Firmware tree, this adds a library which can
create and send Frsky telemetry packets
This commit is contained in:
Matthias Badaire 2014-07-29 08:21:07 +10:00 committed by Craig Elder
parent 728da12441
commit c7cd1c9fde
2 changed files with 328 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

228
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp Normal file
View File

@ -0,0 +1,228 @@
/*
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();
}
}

100
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h Normal file
View File

@ -0,0 +1,100 @@
/*
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/>.
*/
#ifndef __AP_FRSKY_TELEM_H__
#define __AP_FRSKY_TELEM_H__
#include <AP_HAL.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_GPS.h>
#include <AP_AHRS.h>
#include <AP_Baro.h>
#include <AP_BattMonitor.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
#define HUB_PORT true
#define S_PORT false
class AP_Frsky_Telem
{
public:
//constructor
AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) :
_initialised(false),
_ahrs(ahrs),
_battery(battery)
{}
// these enums must match up with TELEM2_PROTOCOL in vehicle code
enum FrSkyProtocol {
FrSkyDPORT = 2,
FrSkySPORT = 3
};
void init(AP_HAL::UARTDriver *port, uint8_t frsky_type);
void send_frames(uint8_t control_mode, enum FrSkyProtocol protocol);
private:
void frsky_send_data(uint8_t id, int16_t data);
void frsky_send_frame1(uint8_t mode);
void frsky_send_frame2();
void check_sport_input(void);
float frsky_format_gps(float dec);
void frsky_send_byte(uint8_t value);
void frsky_send_hub_startstop();
AP_HAL::UARTDriver *_port;
bool _initialised;
AP_AHRS &_ahrs;
AP_BattMonitor &_battery;
uint32_t _last_frame1_ms;
uint32_t _last_frame2_ms;
};
#endif