mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-28 06:24:10 +01:00
Merged in LP-117_frsky_sport_telemetry (pull request #537)
LP-117 frsky sport telemetry Approved-by: Lalanne Laurent Approved-by: Vladimir Zidar
This commit is contained in:
Eric Price
commit
089a42e2ec
673
flight/libraries/frsky_packing.c
Normal file
673
flight/libraries/frsky_packing.c
Normal file
@ -0,0 +1,673 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file frsky_packing.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017-2019
|
||||
* Tau Labs, http://taulabs.org, Copyright (C) 2015
|
||||
* @brief Packs UAVObjects into FrSKY Smart Port frames
|
||||
*
|
||||
* Since there is no public documentation of SmartPort protocol available,
|
||||
* this was put together by studying OpenTx source code, reading
|
||||
* tidbits of informations on the Internet and experimenting.
|
||||
* @see https://github.com/opentx/opentx/tree/next/radio/src/telemetry
|
||||
* @see https://code.google.com/p/telemetry-convert/wiki/FrSkySPortProtocol
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* 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, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "frsky_packing.h"
|
||||
#include "frskysporttelemetrysettings.h"
|
||||
#include "attitudestate.h"
|
||||
#include "barosensor.h"
|
||||
#include "positionstate.h"
|
||||
#include "velocitystate.h"
|
||||
#include "flightbatterystate.h"
|
||||
#include "flightbatterysettings.h"
|
||||
#include "gpstime.h"
|
||||
#include "homelocation.h"
|
||||
#include "accelstate.h"
|
||||
#include "flightstatus.h"
|
||||
#include "airspeedstate.h"
|
||||
#include "nedaccel.h"
|
||||
|
||||
#define GRAVITY 9.805f // [m/s^2]
|
||||
#define KNOTS2M_PER_SECOND 0.514444444f
|
||||
|
||||
|
||||
/**
|
||||
* Encode baro altitude value
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_altitude(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (!frsky->use_baro_sensor || (PositionStateHandle() == NULL)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
// instead of encoding baro altitude directly, we will use
|
||||
// more accurate estimation in PositionState UAVO
|
||||
float down = 0;
|
||||
PositionStateDownGet(&down);
|
||||
int32_t alt = (int32_t)(-down * 100.0f);
|
||||
*value = (uint32_t)alt;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode heading value
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_gps_course(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (AttitudeStateHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
AttitudeStateData attitude;
|
||||
AttitudeStateGet(&attitude);
|
||||
float hdg = (attitude.Yaw >= 0) ? attitude.Yaw : (attitude.Yaw + 360.0f);
|
||||
*value = (uint32_t)(hdg * 100.0f);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode vertical speed value
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_vario(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (!frsky->use_baro_sensor || VelocityStateHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
float down = 0;
|
||||
VelocityStateDownGet(&down);
|
||||
int32_t vspeed = (int32_t)(-down * 100.0f);
|
||||
*value = (uint32_t)vspeed;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode battery current value
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_current(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (!frsky->use_current_sensor) {
|
||||
return false;
|
||||
}
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
float current = 0;
|
||||
FlightBatteryStateCurrentGet(¤t);
|
||||
int32_t current_frsky = (int32_t)(current * 10.0f);
|
||||
*value = (uint32_t)current_frsky;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode battery cells voltage
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[], index of battery cell pair
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_cells(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg)
|
||||
{
|
||||
if ((frsky->batt_cell_count == 0) || (frsky->batt_cell_count - 1) < (int)(arg * 2)) {
|
||||
return false;
|
||||
}
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
float voltage = 0;
|
||||
FlightBatteryStateVoltageGet(&voltage);
|
||||
|
||||
uint32_t cell_voltage = (uint32_t)((voltage * 500.0f) / frsky->batt_cell_count);
|
||||
*value = ((cell_voltage & 0xfff) << 8) | ((arg * 2) & 0x0f) | ((frsky->batt_cell_count << 4) & 0xf0);
|
||||
if (((int16_t)frsky->batt_cell_count - 1) >= (int)(arg * 2 + 1)) {
|
||||
*value |= ((cell_voltage & 0xfff) << 20);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode GPS status as T1 value
|
||||
* Right-most two digits means visible satellite count, left-most digit has following meaning:
|
||||
* 1 - no GPS connected
|
||||
* 2 - no fix
|
||||
* 3 - 2D fix
|
||||
* 4 - 3D fix
|
||||
* 5 - 3D fix and HomeLocation is SET - should be safe for navigation
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value successfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_t1(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (GPSPositionSensorHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t hl_set = HOMELOCATION_SET_FALSE;
|
||||
|
||||
if (HomeLocationHandle()) {
|
||||
HomeLocationSetGet(&hl_set);
|
||||
}
|
||||
|
||||
int32_t t1 = 0;
|
||||
switch (frsky->gps_position.Status) {
|
||||
case GPSPOSITIONSENSOR_STATUS_NOGPS:
|
||||
t1 = 100;
|
||||
break;
|
||||
case GPSPOSITIONSENSOR_STATUS_NOFIX:
|
||||
t1 = 200;
|
||||
break;
|
||||
case GPSPOSITIONSENSOR_STATUS_FIX2D:
|
||||
t1 = 300;
|
||||
break;
|
||||
case GPSPOSITIONSENSOR_STATUS_FIX3D:
|
||||
case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS:
|
||||
if (hl_set == HOMELOCATION_SET_TRUE) {
|
||||
t1 = 500;
|
||||
} else {
|
||||
t1 = 400;
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (frsky->gps_position.Satellites > 0) {
|
||||
t1 += frsky->gps_position.Satellites;
|
||||
}
|
||||
|
||||
*value = (uint32_t)t1;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode GPS hDop and vDop as T2
|
||||
* Bits 0-7 = hDop * 100, max 255 (hDop = 2.55)
|
||||
* Bits 8-15 = vDop * 100, max 255 (vDop = 2.55)
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value successfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_t2(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (GPSPositionSensorHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32_t hdop = (uint32_t)(frsky->gps_position.HDOP * 100.0f);
|
||||
|
||||
if (hdop > 255) {
|
||||
hdop = 255;
|
||||
}
|
||||
|
||||
uint32_t vdop = (uint32_t)(frsky->gps_position.VDOP * 100.0f);
|
||||
|
||||
if (vdop > 255) {
|
||||
vdop = 255;
|
||||
}
|
||||
|
||||
*value = 256 * vdop + hdop;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode consumed battery energy as fuel
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_fuel(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (!frsky->use_current_sensor) {
|
||||
return false;
|
||||
}
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32_t capacity = frsky->battery_settings.Capacity;
|
||||
float consumed_mahs = 0;
|
||||
FlightBatteryStateConsumedEnergyGet(&consumed_mahs);
|
||||
|
||||
float fuel = (uint32_t)(100.0f * (1.0f - consumed_mahs / capacity));
|
||||
fuel = boundf(fuel, 0.0f, 100.0f);
|
||||
*value = (uint32_t)fuel;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode configured values
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]; 0=X, 1=Y, 2=Z
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_acc(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg)
|
||||
{
|
||||
uint8_t accelDataSettings;
|
||||
|
||||
FrSKYSPortTelemetrySettingsAccelDataGet(&accelDataSettings);
|
||||
|
||||
float acc = 0;
|
||||
switch (accelDataSettings) {
|
||||
case FRSKYSPORTTELEMETRYSETTINGS_ACCELDATA_ACCELS:
|
||||
{
|
||||
if (AccelStateHandle() == NULL) {
|
||||
return false;
|
||||
} else if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
switch (arg) {
|
||||
case 0:
|
||||
AccelStatexGet(&acc);
|
||||
break;
|
||||
case 1:
|
||||
AccelStateyGet(&acc);
|
||||
break;
|
||||
case 2:
|
||||
AccelStatezGet(&acc);
|
||||
break;
|
||||
}
|
||||
|
||||
acc /= GRAVITY;
|
||||
acc *= 100.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
case FRSKYSPORTTELEMETRYSETTINGS_ACCELDATA_NEDACCELS:
|
||||
{
|
||||
if (NedAccelHandle() == NULL) {
|
||||
return false;
|
||||
} else if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
switch (arg) {
|
||||
case 0:
|
||||
NedAccelNorthGet(&acc);
|
||||
break;
|
||||
case 1:
|
||||
NedAccelEastGet(&acc);
|
||||
break;
|
||||
case 2:
|
||||
NedAccelDownGet(&acc);
|
||||
break;
|
||||
}
|
||||
|
||||
acc /= GRAVITY;
|
||||
acc *= 100.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
case FRSKYSPORTTELEMETRYSETTINGS_ACCELDATA_NEDVELOCITY:
|
||||
{
|
||||
if (VelocityStateHandle() == NULL) {
|
||||
return false;
|
||||
} else if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
switch (arg) {
|
||||
case 0:
|
||||
VelocityStateNorthGet(&acc);
|
||||
break;
|
||||
case 1:
|
||||
VelocityStateEastGet(&acc);
|
||||
break;
|
||||
case 2:
|
||||
VelocityStateDownGet(&acc);
|
||||
break;
|
||||
}
|
||||
|
||||
acc *= 100.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
case FRSKYSPORTTELEMETRYSETTINGS_ACCELDATA_ATTITUDEANGLES:
|
||||
{
|
||||
if (AttitudeStateHandle() == NULL) {
|
||||
return false;
|
||||
} else if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
switch (arg) {
|
||||
case 0:
|
||||
AttitudeStateRollGet(&acc);
|
||||
break;
|
||||
case 1:
|
||||
AttitudeStatePitchGet(&acc);
|
||||
break;
|
||||
case 2:
|
||||
AttitudeStateYawGet(&acc);
|
||||
break;
|
||||
}
|
||||
|
||||
acc *= 100.0f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t frsky_acc = (int32_t)acc;
|
||||
*value = (uint32_t)frsky_acc;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode gps coordinates
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]; 0=lattitude, 1=longitude
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_gps_coord(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg)
|
||||
{
|
||||
if (GPSPositionSensorHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (frsky->gps_position.Status == GPSPOSITIONSENSOR_STATUS_NOFIX
|
||||
|| frsky->gps_position.Status == GPSPOSITIONSENSOR_STATUS_NOGPS) {
|
||||
return false;
|
||||
}
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32_t frsky_coord = 0;
|
||||
int32_t coord = 0;
|
||||
if (arg == 0) {
|
||||
// lattitude
|
||||
coord = frsky->gps_position.Latitude;
|
||||
if (coord >= 0) {
|
||||
frsky_coord = 0;
|
||||
} else {
|
||||
frsky_coord = 1 << 30;
|
||||
}
|
||||
} else {
|
||||
// longitude
|
||||
coord = frsky->gps_position.Longitude;
|
||||
if (coord >= 0) {
|
||||
frsky_coord = 2 << 30;
|
||||
} else {
|
||||
frsky_coord = 3 << 30;
|
||||
}
|
||||
}
|
||||
coord = abs(coord);
|
||||
frsky_coord |= (((uint64_t)coord * 6ull) / 100);
|
||||
|
||||
*value = frsky_coord;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode gps altitude
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_gps_alt(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (GPSPositionSensorHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (frsky->gps_position.Status != GPSPOSITIONSENSOR_STATUS_FIX3D) {
|
||||
return false;
|
||||
}
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
int32_t frsky_gps_alt = (int32_t)(frsky->gps_position.Altitude * 100.0f);
|
||||
*value = (uint32_t)frsky_gps_alt;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode gps speed
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_gps_speed(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (GPSPositionSensorHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (frsky->gps_position.Status != GPSPOSITIONSENSOR_STATUS_FIX3D) {
|
||||
return false;
|
||||
}
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
int32_t frsky_speed = (int32_t)((frsky->gps_position.Groundspeed / KNOTS2M_PER_SECOND) * 1000);
|
||||
*value = frsky_speed;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode GPS UTC time
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]; 0=date, 1=time
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_gps_time(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg)
|
||||
{
|
||||
if (GPSPositionSensorHandle() == NULL || GPSTimeHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (frsky->gps_position.Status != GPSPOSITIONSENSOR_STATUS_FIX3D) {
|
||||
return false;
|
||||
}
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
GPSTimeData gps_time;
|
||||
GPSTimeGet(&gps_time);
|
||||
uint32_t frsky_time = 0;
|
||||
|
||||
if (arg == 0) {
|
||||
// encode date
|
||||
frsky_time = 0x000000ff;
|
||||
frsky_time |= gps_time.Day << 8;
|
||||
frsky_time |= gps_time.Month << 16;
|
||||
frsky_time |= (gps_time.Year % 100) << 24;
|
||||
} else {
|
||||
frsky_time = 0;
|
||||
frsky_time |= gps_time.Second << 8;
|
||||
frsky_time |= gps_time.Minute << 16;
|
||||
frsky_time |= gps_time.Hour << 24;
|
||||
}
|
||||
*value = frsky_time;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encodes ARM status and flight mode number as RPM value
|
||||
* Since there is no RPM information in any UAVO available,
|
||||
* we will intentionaly misuse this item to encode another useful information.
|
||||
* It will encode flight status as three-digit number as follow:
|
||||
* most left digit encodes arm status (1=armed, 0=disarmed)
|
||||
* two most right digits encodes flight mode number (see FlightStatus UAVO FlightMode enum)
|
||||
* To work this propperly on Taranis, you have to set Blades to "1" in telemetry setting
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_rpm(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (FlightStatusHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
FlightStatusData flight_status;
|
||||
FlightStatusGet(&flight_status);
|
||||
|
||||
*value = (flight_status.Armed == FLIGHTSTATUS_ARMED_ARMED) ? 200 : 100;
|
||||
*value += flight_status.FlightMode;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Encode true airspeed(TAS)
|
||||
* @param[out] value encoded value
|
||||
* @param[in] test_presence_only true when function should only test for availability of this value
|
||||
* @param[in] arg argument specified in frsky_value_items[]
|
||||
* @returns true when value succesfully encoded or presence test passed
|
||||
*/
|
||||
bool frsky_encode_airspeed(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg)
|
||||
{
|
||||
if (AirspeedStateHandle() == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (test_presence_only) {
|
||||
return true;
|
||||
}
|
||||
|
||||
AirspeedStateData airspeed;
|
||||
AirspeedStateGet(&airspeed);
|
||||
int32_t frsky_speed = (int32_t)((airspeed.TrueAirspeed / KNOTS2M_PER_SECOND) * 10);
|
||||
*value = (uint32_t)frsky_speed;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs byte stuffing and checksum calculation
|
||||
* @param[out] obuff buffer where byte stuffed data will came in
|
||||
* @param[in,out] chk checksum byte to update
|
||||
* @param[in] byte
|
||||
* @returns count of bytes inserted to obuff (1 or 2)
|
||||
*/
|
||||
uint8_t frsky_insert_byte(uint8_t *obuff, uint16_t *chk, uint8_t byte)
|
||||
{
|
||||
/* checksum calculation is based on data before byte-stuffing */
|
||||
*chk += byte;
|
||||
*chk += (*chk) >> 8;
|
||||
*chk &= 0x00ff;
|
||||
*chk += (*chk) >> 8;
|
||||
*chk &= 0x00ff;
|
||||
|
||||
if (byte == 0x7e || byte == 0x7d) {
|
||||
obuff[0] = 0x7d;
|
||||
obuff[1] = byte &= ~0x20;
|
||||
return 2;
|
||||
}
|
||||
|
||||
obuff[0] = byte;
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Send u32 value dataframe to FrSky SmartPort bus
|
||||
* @param[in] id FrSky value ID
|
||||
* @param[in] value value
|
||||
*/
|
||||
int32_t frsky_send_frame(uintptr_t com, enum frsky_value_id id, uint32_t value,
|
||||
bool send_prelude)
|
||||
{
|
||||
/* each call of frsky_insert_byte can add 2 bytes to the buffer at maximum
|
||||
* and therefore the worst-case is 17 bytes total (the first byte 0x10 won't be
|
||||
* escaped) */
|
||||
uint8_t tx_data[17];
|
||||
uint16_t chk = 0;
|
||||
uint8_t cnt = 0;
|
||||
|
||||
if (send_prelude) {
|
||||
tx_data[0] = 0x7e;
|
||||
tx_data[1] = 0x98;
|
||||
cnt = 2;
|
||||
}
|
||||
|
||||
cnt += frsky_insert_byte(&tx_data[cnt], &chk, 0x10);
|
||||
cnt += frsky_insert_byte(&tx_data[cnt], &chk, (uint16_t)id & 0xff);
|
||||
cnt += frsky_insert_byte(&tx_data[cnt], &chk, ((uint16_t)id >> 8) & 0xff);
|
||||
cnt += frsky_insert_byte(&tx_data[cnt], &chk, value & 0xff);
|
||||
cnt += frsky_insert_byte(&tx_data[cnt], &chk, (value >> 8) & 0xff);
|
||||
cnt += frsky_insert_byte(&tx_data[cnt], &chk, (value >> 16) & 0xff);
|
||||
cnt += frsky_insert_byte(&tx_data[cnt], &chk, (value >> 24) & 0xff);
|
||||
cnt += frsky_insert_byte(&tx_data[cnt], &chk, 0xff - chk);
|
||||
|
||||
PIOS_COM_SendBuffer(com, tx_data, cnt);
|
||||
|
||||
return cnt;
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
109
flight/libraries/inc/frsky_packing.h
Normal file
109
flight/libraries/inc/frsky_packing.h
Normal file
@ -0,0 +1,109 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file frsky_packing.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017-2019
|
||||
* Tau Labs, http://taulabs.org, Copyright (C) 2015
|
||||
* @brief Packs UAVObjects into FrSKY Smart Port frames
|
||||
*
|
||||
* Since there is no public documentation of SmartPort protocol available,
|
||||
* this was put together by studying OpenTx source code, reading
|
||||
* tidbits of informations on the Internet and experimenting.
|
||||
* @see https://github.com/opentx/opentx/tree/next/radio/src/telemetry
|
||||
* @see https://code.google.com/p/telemetry-convert/wiki/FrSkySPortProtocol
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* 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, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef FRSKY_PACKING_H
|
||||
#define FRSKY_PACKING_H
|
||||
|
||||
#include "pios.h"
|
||||
#include "openpilot.h"
|
||||
|
||||
#include "flightbatterysettings.h"
|
||||
#include "gpspositionsensor.h"
|
||||
|
||||
struct frsky_settings {
|
||||
bool use_current_sensor;
|
||||
uint8_t batt_cell_count;
|
||||
bool use_baro_sensor;
|
||||
FlightBatterySettingsData battery_settings;
|
||||
GPSPositionSensorData gps_position;
|
||||
};
|
||||
|
||||
enum frsky_value_id {
|
||||
FRSKY_ALT_ID = 0x0100,
|
||||
FRSKY_VARIO_ID = 0x110,
|
||||
FRSKY_CURR_ID = 0x0200,
|
||||
FRSKY_VFAS_ID = 0x0210,
|
||||
FRSKY_CELLS_ID = 0x0300,
|
||||
FRSKY_T1_ID = 0x0400,
|
||||
FRSKY_T2_ID = 0x0410,
|
||||
FRSKY_RPM_ID = 0x0500,
|
||||
FRSKY_FUEL_ID = 0x0600,
|
||||
FRSKY_ACCX_ID = 0x0700,
|
||||
FRSKY_ACCY_ID = 0x0710,
|
||||
FRSKY_ACCZ_ID = 0x0720,
|
||||
FRSKY_GPS_LON_LAT_ID = 0x0800,
|
||||
FRSKY_GPS_ALT_ID = 0x0820,
|
||||
FRSKY_GPS_SPEED_ID = 0x0830,
|
||||
FRSKY_GPS_COURSE_ID = 0x0840,
|
||||
FRSKY_GPS_TIME_ID = 0x0850,
|
||||
FRSKY_ADC3_ID = 0x0900,
|
||||
FRSKY_ADC4_ID = 0x0910,
|
||||
FRSKY_AIR_SPEED_ID = 0x0a00,
|
||||
FRSKY_RSSI_ID = 0xf101,
|
||||
FRSKY_ADC1_ID = 0xf102,
|
||||
FRSKY_ADC2_ID = 0xf103,
|
||||
FRSKY_BATT_ID = 0xf104,
|
||||
FRSKY_SWR_ID = 0xf105,
|
||||
};
|
||||
|
||||
struct frsky_value_item {
|
||||
enum frsky_value_id id;
|
||||
uint16_t period_ms;
|
||||
bool (*encode_value)(struct frsky_settings *sport, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
uint32_t fn_arg;
|
||||
};
|
||||
|
||||
bool frsky_encode_gps_course(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_altitude(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_vario(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_current(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_cells(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_t1(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_t2(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_fuel(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_acc(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_gps_coord(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_gps_alt(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_gps_speed(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_gps_time(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_rpm(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
bool frsky_encode_airspeed(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg);
|
||||
uint8_t frsky_insert_byte(uint8_t *obuff, uint16_t *chk, uint8_t byte);
|
||||
int32_t frsky_send_frame(uintptr_t com, enum frsky_value_id id, uint32_t value,
|
||||
bool send_prelude);
|
||||
|
||||
#endif /* FRSKY_PACKING_H */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
357
flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c
Normal file
357
flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c
Normal file
@ -0,0 +1,357 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup UAVOFrSKYSPortBridge UAVO to FrSKY S.PORT Bridge Module
|
||||
* @{
|
||||
*
|
||||
* @file uavofrskysportbridge.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017-2019
|
||||
* Tau Labs, http://taulabs.org, Copyright (C) 2014
|
||||
* @brief Bridges selected UAVObjects to FrSKY Smart Port bus
|
||||
*
|
||||
* Since there is no public documentation of SmartPort protocol available,
|
||||
* this was put together by studying OpenTx source code, reading
|
||||
* tidbits of informations on the Internet and experimenting.
|
||||
* @see https://github.com/opentx/opentx/tree/next/radio/src/telemetry
|
||||
* @see https://code.google.com/p/telemetry-convert/wiki/FrSkySPortProtocol
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* 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, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "frsky_packing.h"
|
||||
|
||||
#include "pios_board_io.h"
|
||||
|
||||
#include "barosensor.h"
|
||||
#include "flightbatterysettings.h"
|
||||
#include "flightbatterystate.h"
|
||||
#include "gpspositionsensor.h"
|
||||
#include "frskysporttelemetrysettings.h"
|
||||
#include "hwsettings.h"
|
||||
#include "taskinfo.h"
|
||||
|
||||
|
||||
#define FRSKY_POLL_REQUEST 0x7e
|
||||
#define FRSKY_MINIMUM_POLL_INTERVAL 10000
|
||||
|
||||
enum frsky_state {
|
||||
FRSKY_STATE_WAIT_POLL_REQUEST,
|
||||
FRSKY_STATE_WAIT_SENSOR_ID,
|
||||
FRSKY_STATE_WAIT_TX_DONE,
|
||||
};
|
||||
|
||||
// Set of objects sent by this module
|
||||
static const struct frsky_value_item frsky_value_items[] = {
|
||||
{ FRSKY_GPS_COURSE_ID, 100, frsky_encode_gps_course, 0 }, // attitude yaw estimate
|
||||
{ FRSKY_ALT_ID, 100, frsky_encode_altitude, 0 }, // altitude estimate
|
||||
{ FRSKY_VARIO_ID, 100, frsky_encode_vario, 0 }, // vertical speed
|
||||
{ FRSKY_CURR_ID, 300, frsky_encode_current, 0 }, // battery current
|
||||
{ FRSKY_CELLS_ID, 850, frsky_encode_cells, 0 }, // battery cells 1-2
|
||||
{ FRSKY_CELLS_ID, 850, frsky_encode_cells, 1 }, // battery cells 3-4
|
||||
{ FRSKY_CELLS_ID, 850, frsky_encode_cells, 2 }, // battery cells 5-6
|
||||
{ FRSKY_CELLS_ID, 850, frsky_encode_cells, 3 }, // battery cells 7-8
|
||||
{ FRSKY_CELLS_ID, 850, frsky_encode_cells, 4 }, // battery cells 9-10
|
||||
{ FRSKY_CELLS_ID, 850, frsky_encode_cells, 5 }, // battery cells 11-12
|
||||
{ FRSKY_T1_ID, 2000, frsky_encode_t1, 0 }, // baro temperature
|
||||
{ FRSKY_T2_ID, 1500, frsky_encode_t2, 0 }, // encodes GPS status!
|
||||
{ FRSKY_FUEL_ID, 600, frsky_encode_fuel, 0 }, // consumed battery energy
|
||||
{ FRSKY_ACCX_ID, 100, frsky_encode_acc, 0 }, // accX
|
||||
{ FRSKY_ACCY_ID, 100, frsky_encode_acc, 1 }, // accY
|
||||
{ FRSKY_ACCZ_ID, 100, frsky_encode_acc, 2 }, // accZ
|
||||
{ FRSKY_GPS_LON_LAT_ID, 100, frsky_encode_gps_coord, 0 }, // lattitude
|
||||
{ FRSKY_GPS_LON_LAT_ID, 100, frsky_encode_gps_coord, 1 }, // longitude
|
||||
{ FRSKY_GPS_ALT_ID, 750, frsky_encode_gps_alt, 0 }, // gps altitude
|
||||
{ FRSKY_GPS_SPEED_ID, 200, frsky_encode_gps_speed, 0 }, // gps speed
|
||||
{ FRSKY_GPS_TIME_ID, 8000, frsky_encode_gps_time, 0 }, // gps date
|
||||
{ FRSKY_GPS_TIME_ID, 2000, frsky_encode_gps_time, 1 }, // gps time
|
||||
{ FRSKY_RPM_ID, 1500, frsky_encode_rpm, 0 }, // encodes flight status!
|
||||
{ FRSKY_AIR_SPEED_ID, 100, frsky_encode_airspeed, 0 }, // airspeed
|
||||
};
|
||||
|
||||
struct frsky_sport_telemetry {
|
||||
enum frsky_state state;
|
||||
int32_t scheduled_item;
|
||||
uint32_t last_poll_time;
|
||||
uintptr_t com;
|
||||
bool ignore_echo;
|
||||
uint8_t ignore_rx_chars;
|
||||
struct frsky_settings frsky_settings;
|
||||
uint16_t schedule_nr;
|
||||
uint32_t item_last_triggered[NELEMENTS(frsky_value_items)];
|
||||
uint16_t item_schedule_nr[NELEMENTS(frsky_value_items)];
|
||||
};
|
||||
|
||||
static const uint8_t frsky_sensor_ids[] = { 0x1b };
|
||||
|
||||
#define FRSKY_SPORT_BAUDRATE 57600
|
||||
|
||||
#if defined(PIOS_FRSKY_SPORT_TELEMETRY_STACK_SIZE)
|
||||
#define STACK_SIZE_BYTES PIOS_FRSKY_SPORT_TELEMETRY_STACK_SIZE
|
||||
#else
|
||||
#define STACK_SIZE_BYTES 672
|
||||
#endif
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY)
|
||||
|
||||
static struct frsky_sport_telemetry *frsky = 0;
|
||||
static int32_t uavoFrSKYSPortBridgeInitialize(void);
|
||||
static void uavoFrSKYSPortBridgeTask(void *parameters);
|
||||
|
||||
/**
|
||||
* Scan for value item with the longest expired time and schedule it to send in next poll turn
|
||||
*
|
||||
*/
|
||||
|
||||
static void frsky_schedule_next_item(void)
|
||||
{
|
||||
frsky->scheduled_item = -1;
|
||||
|
||||
for (uint32_t i = 0; i < NELEMENTS(frsky_value_items); i++) {
|
||||
if (frsky_value_items[i].encode_value(&frsky->frsky_settings, 0, true, frsky_value_items[i].fn_arg)) {
|
||||
if (frsky->item_schedule_nr[i] == frsky->schedule_nr) {
|
||||
continue;
|
||||
}
|
||||
if (PIOS_DELAY_GetuSSince(frsky->item_last_triggered[i]) > (frsky_value_items[i].period_ms * 1000)) {
|
||||
frsky->scheduled_item = i;
|
||||
frsky->item_schedule_nr[i] = frsky->schedule_nr;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (frsky->scheduled_item < 0) {
|
||||
frsky->schedule_nr++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Send value item previously scheduled by frsky_schedule_next_itme()
|
||||
* @returns true when item value was sended
|
||||
*/
|
||||
static bool frsky_send_scheduled_item(void)
|
||||
{
|
||||
int32_t item = frsky->scheduled_item;
|
||||
|
||||
if ((item >= 0) && (item < (int32_t)NELEMENTS(frsky_value_items))) {
|
||||
frsky->item_last_triggered[item] = PIOS_DELAY_GetuS();
|
||||
uint32_t value = 0;
|
||||
if (frsky_value_items[item].encode_value(&frsky->frsky_settings, &value, false,
|
||||
frsky_value_items[item].fn_arg)) {
|
||||
frsky->ignore_rx_chars += frsky_send_frame(frsky->com, (uint16_t)(frsky_value_items[item].id), value, false);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Process incoming bytes from FrSky S.PORT bus
|
||||
* @param[in] b received byte
|
||||
*/
|
||||
static void frsky_receive_byte(uint8_t b)
|
||||
{
|
||||
uint32_t i = 0;
|
||||
|
||||
switch (frsky->state) {
|
||||
case FRSKY_STATE_WAIT_TX_DONE:
|
||||
// because RX and TX are connected, we need to ignore bytes
|
||||
// transmited by us
|
||||
if (--frsky->ignore_rx_chars == 0) {
|
||||
frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST;
|
||||
}
|
||||
break;
|
||||
|
||||
case FRSKY_STATE_WAIT_POLL_REQUEST:
|
||||
if (b == FRSKY_POLL_REQUEST) {
|
||||
// X8R is polling us every 13ms
|
||||
if (PIOS_DELAY_GetuSSince(frsky->last_poll_time) > FRSKY_MINIMUM_POLL_INTERVAL) {
|
||||
frsky->last_poll_time = PIOS_DELAY_GetuS();
|
||||
frsky->state = FRSKY_STATE_WAIT_SENSOR_ID;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case FRSKY_STATE_WAIT_SENSOR_ID:
|
||||
|
||||
frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST;
|
||||
|
||||
for (i = 0; i < sizeof(frsky_sensor_ids); i++) {
|
||||
if (frsky_sensor_ids[i] == b) {
|
||||
// get GPSPositionData once here to be more efficient, since
|
||||
// GPS position data are very often used by encode() handlers
|
||||
if (GPSPositionSensorHandle() != NULL) {
|
||||
GPSPositionSensorGet(&frsky->frsky_settings.gps_position);
|
||||
}
|
||||
// send item previously scheduled
|
||||
frsky_send_scheduled_item();
|
||||
|
||||
if (frsky->ignore_echo && frsky->ignore_rx_chars) {
|
||||
frsky->state = FRSKY_STATE_WAIT_TX_DONE;
|
||||
}
|
||||
|
||||
// schedule item for next poll turn
|
||||
frsky_schedule_next_item();
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Module start routine automatically called after initialization routine
|
||||
* @return 0 when was successful
|
||||
*/
|
||||
static int32_t uavoFrSKYSPortBridgeStart(void)
|
||||
{
|
||||
if (!frsky) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (FlightBatterySettingsHandle() != NULL
|
||||
&& FlightBatteryStateHandle() != NULL) {
|
||||
// TODO: maybe get this setting from somewhere else?
|
||||
// uint8_t currentPin;
|
||||
// FlightBatterySettingsCurrentPinGet(¤tPin);
|
||||
// if (currentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE)
|
||||
frsky->frsky_settings.use_current_sensor = true;
|
||||
FlightBatterySettingsGet(&frsky->frsky_settings.battery_settings);
|
||||
frsky->frsky_settings.batt_cell_count = frsky->frsky_settings.battery_settings.NbCells;
|
||||
}
|
||||
// This is just to check if barometer is enabled.
|
||||
// if (BaroSensorHandle() != NULL
|
||||
// && PIOS_SENSORS_GetQueue(PIOS_SENSOR_BARO) != NULL)
|
||||
frsky->frsky_settings.use_baro_sensor = true;
|
||||
|
||||
|
||||
xTaskHandle task;
|
||||
xTaskCreate(uavoFrSKYSPortBridgeTask, "FrSky SPort Telemetry", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &task);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FRSKYSPORTTELEMETRY, task);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void FrSKYSPortTelemetrySettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
FrSKYSPortTelemetrySettingsData settings;
|
||||
|
||||
FrSKYSPortTelemetrySettingsGet(&settings);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Module initialization routine
|
||||
* @return 0 when initialization was successful
|
||||
*/
|
||||
static int32_t uavoFrSKYSPortBridgeInitialize(void)
|
||||
{
|
||||
FrSKYSPortTelemetrySettingsInitialize();
|
||||
|
||||
if (PIOS_COM_FRSKY_SPORT) {
|
||||
frsky = pios_malloc(sizeof(struct frsky_sport_telemetry));
|
||||
|
||||
if (frsky != NULL) {
|
||||
memset(frsky, 0x00, sizeof(struct frsky_sport_telemetry));
|
||||
|
||||
frsky->frsky_settings.use_current_sensor = false;
|
||||
frsky->frsky_settings.batt_cell_count = 0;
|
||||
frsky->frsky_settings.use_baro_sensor = false;
|
||||
frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST;
|
||||
frsky->last_poll_time = PIOS_DELAY_GetuS();
|
||||
frsky->scheduled_item = -1;
|
||||
frsky->com = PIOS_COM_FRSKY_SPORT;
|
||||
frsky->ignore_echo = true; // This has to be true when RX & TX hw serial lines are connected. Otherwise false. (enforced below by setting half duplex. this makes internal connection between rx and tx
|
||||
// connect TX pin of flight controller to UNINVERTED SPort
|
||||
// (F4 based controllers do not have TX inversion capability.
|
||||
// Use external inverter or solder to uninverted receiver pin)
|
||||
// TODO: Add code/PIOS driver to enable inversion for F3 based convertes
|
||||
// TODO: implement FPORT driver
|
||||
frsky->schedule_nr = 1;
|
||||
|
||||
uint8_t i;
|
||||
for (i = 0; i < NELEMENTS(frsky_value_items); i++) {
|
||||
frsky->item_last_triggered[i] = PIOS_DELAY_GetuS();
|
||||
}
|
||||
|
||||
// Set Port options:
|
||||
// BAUD rate
|
||||
PIOS_COM_ChangeBaud(frsky->com, FRSKY_SPORT_BAUDRATE);
|
||||
|
||||
HwSettingsSPortModeOptions options;
|
||||
HwSettingsSPortModeGet(&options);
|
||||
bool halfduplex;
|
||||
enum PIOS_USART_Inverted inverted;
|
||||
switch (options) {
|
||||
case HWSETTINGS_SPORTMODE_HALFDUPLEXNONINVERTED:
|
||||
halfduplex = true;
|
||||
inverted = PIOS_USART_Inverted_None;
|
||||
break;
|
||||
case HWSETTINGS_SPORTMODE_HALFDUPLEXINVERTED:
|
||||
halfduplex = true;
|
||||
inverted = PIOS_USART_Inverted_RxTx;
|
||||
break;
|
||||
case HWSETTINGS_SPORTMODE_FULLDUPLEXNONINVERTED:
|
||||
halfduplex = false;
|
||||
inverted = PIOS_USART_Inverted_None;
|
||||
break;
|
||||
case HWSETTINGS_SPORTMODE_FULLDUPLEXINVERTED:
|
||||
halfduplex = false;
|
||||
inverted = PIOS_USART_Inverted_RxTx;
|
||||
break;
|
||||
}
|
||||
|
||||
// Port Inversion (Not available on STM32F4, will have no effect)
|
||||
PIOS_COM_Ioctl(frsky->com, PIOS_IOCTL_USART_SET_INVERTED, &inverted);
|
||||
|
||||
// HalfDplex mode (Not available on STM32F0, will have no effect)
|
||||
PIOS_COM_Ioctl(frsky->com, PIOS_IOCTL_USART_SET_HALFDUPLEX, &halfduplex);
|
||||
|
||||
FrSKYSPortTelemetrySettingsConnectCallback(FrSKYSPortTelemetrySettingsUpdatedCb);
|
||||
|
||||
FrSKYSPortTelemetrySettingsUpdatedCb(0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
MODULE_INITCALL(uavoFrSKYSPortBridgeInitialize, uavoFrSKYSPortBridgeStart);
|
||||
|
||||
/**
|
||||
* Main task routine
|
||||
* @param[in] parameters parameter given by PIOS_Thread_Create()
|
||||
*/
|
||||
static void uavoFrSKYSPortBridgeTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
while (1) {
|
||||
uint8_t b = 0;
|
||||
uint16_t count = PIOS_COM_ReceiveBuffer(frsky->com, &b, 1, 0xffffffff);
|
||||
if (count) {
|
||||
frsky_receive_byte(b);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -108,6 +108,10 @@ uint32_t pios_com_debug_id; /* DebugConsole */
|
||||
uint32_t pios_com_hott_id;
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_FRSKY_SPORT
|
||||
uint32_t pios_com_frsky_sport_id;
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_USB
|
||||
|
||||
uint32_t pios_com_telem_usb_id; /* USB telemetry */
|
||||
@ -433,6 +437,13 @@ static const struct uart_function uart_function_map[] = {
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS,
|
||||
},
|
||||
# endif /* PIOS_INCLUDE_SBUS */
|
||||
# ifdef PIOS_INCLUDE_FRSKY_SPORT
|
||||
[PIOS_BOARD_IO_UART_FRSKY_SPORT] = {
|
||||
.com_id = &pios_com_frsky_sport_id,
|
||||
.com_rx_buf_len = PIOS_COM_FRSKY_SPORT_RX_BUF_LEN,
|
||||
.com_tx_buf_len = PIOS_COM_FRSKY_SPORT_TX_BUF_LEN,
|
||||
},
|
||||
# endif
|
||||
#endif /* PIOS_INCLUDE_RCVR */
|
||||
};
|
||||
|
||||
|
||||
@ -173,16 +173,28 @@ extern uint32_t pios_com_frsky_sensorhub_id;
|
||||
# define PIOS_COM_FRSKY_SENSORHUB_TX_BUF_LEN 128
|
||||
#endif
|
||||
|
||||
/* Frsky Sport */
|
||||
#ifdef PIOS_INCLUDE_FRSKY_SPORT
|
||||
extern uint32_t pios_com_frsky_sport_id;
|
||||
# define PIOS_COM_FRSKY_SPORT (pios_com_frsky_sport_id)
|
||||
# ifndef PIOS_COM_FRSKY_SPORT_RX_BUF_LEN
|
||||
# define PIOS_COM_FRSKY_SPORT_RX_BUF_LEN 16
|
||||
# endif
|
||||
# ifndef PIOS_COM_FRSKY_SPORT_TX_BUF_LEN
|
||||
# define PIOS_COM_FRSKY_SPORT_TX_BUF_LEN 16
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/* USB VCP */
|
||||
extern uint32_t pios_com_vcp_id;
|
||||
#define PIOS_COM_VCP (pios_com_vcp_id)
|
||||
#define PIOS_COM_VCP (pios_com_vcp_id)
|
||||
|
||||
|
||||
#ifdef PIOS_INCLUDE_DEBUG_CONSOLE
|
||||
extern uint32_t pios_com_debug_id;
|
||||
#define PIOS_COM_DEBUG (pios_com_debug_id)
|
||||
#define PIOS_COM_DEBUG (pios_com_debug_id)
|
||||
#ifndef PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN
|
||||
# define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40
|
||||
# define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40
|
||||
#endif
|
||||
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
|
||||
|
||||
@ -215,9 +227,9 @@ typedef enum {
|
||||
PIOS_BOARD_IO_UART_SRXL, /* rcvr */
|
||||
PIOS_BOARD_IO_UART_IBUS, /* rcvr */
|
||||
PIOS_BOARD_IO_UART_EXBUS, /* rcvr */
|
||||
// PIOS_BOARD_IO_UART_FRSKY_SPORT_TELEMETRY, /* com */
|
||||
PIOS_BOARD_IO_UART_HOTT_BRIDGE, /* com */
|
||||
PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, /* com */
|
||||
PIOS_BOARD_IO_UART_FRSKY_SPORT, /* com */
|
||||
} PIOS_BOARD_IO_UART_Function;
|
||||
|
||||
|
||||
|
||||
@ -57,6 +57,7 @@ OPTMODULES += AutoTune
|
||||
OPTMODULES += Temperature
|
||||
OPTMODULES += ComUsbBridge
|
||||
OPTMODULES += UAVOHottBridge
|
||||
OPTMODULES += UAVOFrSKYSPortBridge
|
||||
OPTMODULES += UAVOMSPBridge
|
||||
OPTMODULES += UAVOMavlinkBridge
|
||||
OPTMODULES += UAVOFrSKYSensorHubBridge
|
||||
@ -97,6 +98,7 @@ ifndef TESTAPP
|
||||
SRC += $(FLIGHTLIB)/auxmagsupport.c
|
||||
SRC += $(FLIGHTLIB)/lednotification.c
|
||||
SRC += $(FLIGHTLIB)/sha1.c
|
||||
SRC += $(FLIGHTLIB)/frsky_packing.c
|
||||
|
||||
## UAVObjects
|
||||
include ./UAVObjects.inc
|
||||
|
||||
@ -130,6 +130,7 @@ UAVOBJSRCFILENAMES += hottbridgesettings
|
||||
UAVOBJSRCFILENAMES += hottbridgestatus
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
UAVOBJSRCFILENAMES += perfcounter
|
||||
UAVOBJSRCFILENAMES += frskysporttelemetrysettings
|
||||
UAVOBJSRCFILENAMES += systemidentsettings
|
||||
UAVOBJSRCFILENAMES += systemidentstate
|
||||
|
||||
|
||||
@ -163,6 +163,7 @@
|
||||
#define PIOS_INCLUDE_GPS_DJI_PARSER
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
#define PIOS_INCLUDE_FRSKY_SENSORHUB
|
||||
#define PIOS_INCLUDE_FRSKY_SPORT
|
||||
|
||||
/* Stabilization options */
|
||||
#define PIOS_QUATERNION_STABILIZATION
|
||||
|
||||
@ -66,6 +66,7 @@ static const PIOS_BOARD_IO_UART_Function flexiio_function_map[] = {
|
||||
[HWSETTINGS_RM_RCVRPORT_PPMMAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
|
||||
[HWSETTINGS_RM_RCVRPORT_PPMGPS] = PIOS_BOARD_IO_UART_GPS,
|
||||
[HWSETTINGS_RM_RCVRPORT_PPMHOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
|
||||
[HWSETTINGS_RM_RCVRPORT_PPMFRSKYSPORT] = PIOS_BOARD_IO_UART_FRSKY_SPORT,
|
||||
[HWSETTINGS_RM_RCVRPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY,
|
||||
[HWSETTINGS_RM_RCVRPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE,
|
||||
[HWSETTINGS_RM_RCVRPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE,
|
||||
@ -75,6 +76,7 @@ static const PIOS_BOARD_IO_UART_Function flexiio_function_map[] = {
|
||||
[HWSETTINGS_RM_RCVRPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS,
|
||||
[HWSETTINGS_RM_RCVRPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
|
||||
[HWSETTINGS_RM_RCVRPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
|
||||
[HWSETTINGS_RM_RCVRPORT_FRSKYSPORT] = PIOS_BOARD_IO_UART_FRSKY_SPORT,
|
||||
};
|
||||
|
||||
static const PIOS_BOARD_IO_UART_Function main_function_map[] = {
|
||||
@ -89,6 +91,7 @@ static const PIOS_BOARD_IO_UART_Function main_function_map[] = {
|
||||
[HWSETTINGS_RM_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
|
||||
[HWSETTINGS_RM_MAINPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
|
||||
[HWSETTINGS_RM_MAINPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
|
||||
[HWSETTINGS_RM_MAINPORT_FRSKYSPORT] = PIOS_BOARD_IO_UART_FRSKY_SPORT,
|
||||
};
|
||||
|
||||
static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = {
|
||||
@ -107,6 +110,7 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = {
|
||||
[HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
|
||||
[HWSETTINGS_RM_FLEXIPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
|
||||
[HWSETTINGS_RM_FLEXIPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
|
||||
[HWSETTINGS_RM_FLEXIPORT_FRSKYSPORT] = PIOS_BOARD_IO_UART_FRSKY_SPORT,
|
||||
};
|
||||
|
||||
/**
|
||||
@ -295,6 +299,7 @@ void PIOS_Board_Init(void)
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK:
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMGPS:
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMHOTTTELEMETRY:
|
||||
case HWSETTINGS_RM_RCVRPORT_PPMFRSKYSPORT:
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg);
|
||||
|
||||
|
||||
@ -81,6 +81,7 @@ UAVOBJS = \
|
||||
$${UAVOBJ_XML_DIR}/flightplanstatus.xml \
|
||||
$${UAVOBJ_XML_DIR}/flightstatus.xml \
|
||||
$${UAVOBJ_XML_DIR}/flighttelemetrystats.xml \
|
||||
$${UAVOBJ_XML_DIR}/frskysporttelemetrysettings.xml \
|
||||
$${UAVOBJ_XML_DIR}/gcsreceiver.xml \
|
||||
$${UAVOBJ_XML_DIR}/gcstelemetrystats.xml \
|
||||
$${UAVOBJ_XML_DIR}/gpsextendedstatus.xml \
|
||||
|
||||
12
shared/uavobjectdefinition/frskysporttelemetrysettings.xml
Normal file
12
shared/uavobjectdefinition/frskysporttelemetrysettings.xml
Normal file
@ -0,0 +1,12 @@
|
||||
<xml>
|
||||
<object name="FrSKYSPortTelemetrySettings" singleinstance="true" settings="true" category="System">
|
||||
<description>FrSKY S.Port Telemetry configuration.</description>
|
||||
|
||||
<field name="AccelData" units="" type="enum" elements="1" options="Accels,NEDAccels, NEDVelocity, AttitudeAngles" defaultvalue="Accels"/>
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
@ -11,11 +11,11 @@
|
||||
<field name="RV_TelemetryPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,ComAux,ComBridge,MSP,MAVLink" defaultvalue="Telemetry"/>
|
||||
<field name="RV_GPSPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,ComAux,ComBridge,MSP,MAVLink" defaultvalue="GPS"/>
|
||||
|
||||
<field name="RM_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+PWM,PPM+Outputs,PPM+Telemetry,PPM+DebugConsole,PPM+ComBridge,PPM+MSP,PPM+MAVLink,PPM+GPS,PPM+HoTT Telemetry,Outputs,Telemetry,DebugConsole,ComBridge,MSP,MAVLink,GPS,HoTT Telemetry,IBus,FrskySensorHub"
|
||||
<field name="RM_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+PWM,PPM+Outputs,PPM+Telemetry,PPM+DebugConsole,PPM+ComBridge,PPM+MSP,PPM+MAVLink,PPM+GPS,PPM+HoTT Telemetry,PPM+FrSkySPort,Outputs,Telemetry,DebugConsole,ComBridge,MSP,MAVLink,GPS,HoTT Telemetry,IBus,FrskySensorHub,FrSkySPort"
|
||||
defaultvalue="PWM"
|
||||
limits="%0905NE:PPM+PWM:PPM+Telemetry:PPM+DebugConsole:PPM+ComBridge:PPM+MSP:PPM+MAVLink:PPM+GPS:PPM+HoTT Telemetry:Telemetry:DebugConsole:ComBridge:MSP:MAVLink:GPS:HoTT Telemetry:IBus;"/>
|
||||
<field name="RM_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM,HoTT Telemetry,DebugConsole,ComBridge,OsdHk,MSP,MAVLink,FrskySensorHub" defaultvalue="Disabled"/>
|
||||
<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM,EX.Bus,HoTT SUMD,HoTT SUMH,HoTT Telemetry,SRXL,IBus,DebugConsole,ComBridge,OsdHk,MSP,MAVLink,FrskySensorHub" defaultvalue="Disabled"/>
|
||||
limits="%0905NE:PPM+PWM:PPM+Telemetry:PPM+DebugConsole:PPM+ComBridge:PPM+MSP:PPM+MAVLink:PPM+GPS:PPM+HoTT Telemetry:PPM+FrSkySPort:Telemetry:DebugConsole:ComBridge:MSP:MAVLink:GPS:HoTT Telemetry:IBus:FrskySensorHub:FrSkySPort;"/>
|
||||
<field name="RM_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM,HoTT Telemetry,DebugConsole,ComBridge,OsdHk,MSP,MAVLink,FrskySensorHub,FrSkySPort" defaultvalue="Disabled"/>
|
||||
<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM,EX.Bus,HoTT SUMD,HoTT SUMH,HoTT Telemetry,SRXL,IBus,DebugConsole,ComBridge,OsdHk,MSP,MAVLink,FrskySensorHub,FrSkySPort" defaultvalue="Disabled"/>
|
||||
|
||||
<field name="SPK2_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PPM,S.Bus,DSM,SRXL,IBus,EX.Bus,HoTT SUMD,HoTT SUMH" defaultvalue="PPM"/>
|
||||
<field name="SPK2_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,DSM,DebugConsole,ComBridge,OsdHk,MSP,MAVLink,FrskySensorHub" defaultvalue="Disabled"/>
|
||||
@ -35,6 +35,7 @@
|
||||
<field name="ADCRouting" units="" type="enum" elementnames="adc0,adc1,adc2,adc3,adc4,adc5,adc6,adc7" options="Disabled,BatteryVoltage,BatteryCurrent,AnalogAirspeed,NTCTemperature,LM35Temperature,Generic" defaultvalue="Disabled"/>
|
||||
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
|
||||
<field name="SBusMode" units="" type="enum" elements="1" options="Normal,Non Inverted" defaultvalue="Normal"/>
|
||||
<field name="SPortMode" units="" type="enum" elements="1" options="HalfDuplexNonInverted,HalfDuplexInverted,FullDuplexNonInverted,FullDuplexInverted" defaultvalue="HalfDuplexNonInverted"/>
|
||||
<field name="WS2811LED_Out" units="" type="enum" elements="1" options="ServoOut1,ServoOut2,ServoOut3,ServoOut4,ServoOut5,ServoOut6,FlexiIOPin3,FlexiIOPin4,Disabled" defaultvalue="Disabled"
|
||||
limits="%0905NE:ServoOut2:ServoOut3:ServoOut4:ServoOut5:ServoOut6:FlexiIOPin3:FlexiIOPin4;"
|
||||
/>
|
||||
|
||||
@ -35,6 +35,7 @@
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>UAVOFrskySensorHubBridge</elementname>
|
||||
<elementname>UAVOMAVLinkBridge</elementname>
|
||||
<elementname>FrSkySPortTelemetry</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
@ -72,6 +73,7 @@
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>UAVOFrskySensorHubBridge</elementname>
|
||||
<elementname>UAVOMAVLinkBridge</elementname>
|
||||
<elementname>FrSkySPortTelemetry</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
</elementnames>
|
||||
<options>
|
||||
@ -113,6 +115,7 @@
|
||||
<elementname>UAVOMSPBridge</elementname>
|
||||
<elementname>UAVOFrskySensorHubBridge</elementname>
|
||||
<elementname>UAVOMAVLinkBridge</elementname>
|
||||
<elementname>FrSkySPortTelemetry</elementname>
|
||||
<elementname>AutoTune</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user