mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Frsky_Telem: added initial Frsky telemetry library
based on work from the PX4Firmware tree, this adds a library which can create and send Frsky telemetry packets
This commit is contained in:
parent
728da12441
commit
c7cd1c9fde
228
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
Normal file
228
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
Normal 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
100
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
Normal 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
|
Loading…
Reference in New Issue
Block a user