rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

LP-117 Rebased Mindnever's FRSky S.Port dev branch as a single commit

Browse Source
This commit is contained in:
Eric Price 2019-07-04 00:19:38 +02:00
parent 7c9f04d87c
commit 94231327d7
8 changed files with 1122 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

671
flight/libraries/frsky_packing.c Normal file
View File

@ -0,0 +1,671 @@
/**
******************************************************************************
*
* @file frsky_packing.c
* @author 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(&current);
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:
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;
}
/**
* @}
* @}
*/

108
flight/libraries/inc/frsky_packing.h Normal file
View File

@ -0,0 +1,108 @@
/**
******************************************************************************
*
* @file frsky_packing.h
* @author 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 */
/**
* @}
* @}
*/

320
flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c Normal file
View File

@ -0,0 +1,320 @@
/**
******************************************************************************
* @addtogroup TauLabsModules TauLabs Modules
* @{
* @addtogroup UAVOFrSKYSPortBridge UAVO to FrSKY S.PORT Bridge Module
* @{
*
* @file uavofrskysportbridge.c
* @author 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 "barosensor.h"
#include "flightbatterysettings.h"
#include "flightbatterystate.h"
#include "gpspositionsensor.h"
#include "frskysporttelemetrysettings.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 master;
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(&currentPin);
// 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);
frsky->master = (settings.PortMaster == FRSKYSPORTTELEMETRYSETTINGS_PORTMASTER_TRUE);
}
/**
* 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.
frsky->schedule_nr = 1;
uint8_t i;
for (i = 0; i < NELEMENTS(frsky_value_items); i++) {
frsky->item_last_triggered[i] = PIOS_DELAY_GetuS();
}
FrSKYSPortTelemetrySettingsConnectCallback(FrSKYSPortTelemetrySettingsUpdatedCb);
FrSKYSPortTelemetrySettingsUpdatedCb(0);
PIOS_COM_ChangeBaud(frsky->com, FRSKY_SPORT_BAUDRATE);
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);
}
}
}
/**
* @}
* @}
*/

2
flight/targets/boards/revolution/firmware/Makefile
View File

@ -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

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -130,6 +130,7 @@ UAVOBJSRCFILENAMES += hottbridgesettings
UAVOBJSRCFILENAMES += hottbridgestatus
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += frskysporttelemetrysettings
UAVOBJSRCFILENAMES += systemidentsettings
UAVOBJSRCFILENAMES += systemidentstate

13
shared/uavobjectdefinition/frskysporttelemetrysettings.xml Normal file
View File

@ -0,0 +1,13 @@
<xml>
<object name="FrSKYSPortTelemetrySettings" singleinstance="true" settings="true">
<description>FrSKY S.Port Telemetry configuration.</description>
<field name="AccelData" units="" type="enum" elements="1" options="Accels,NEDAccels, NEDVelocity, AttitudeAngles" defaultvalue="Accels"/>
<field name="PortMaster" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
<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>

8
shared/uavobjectdefinition/hwsettings.xml
View File

@ -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,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: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"/>

3
shared/uavobjectdefinition/taskinfo.xml
View File

@ -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>