From 94231327d71adf1f840c7306b189fa6f1fc69bfa Mon Sep 17 00:00:00 2001 From: Eric Price Date: Thu, 4 Jul 2019 00:19:38 +0200 Subject: [PATCH 1/6] LP-117 Rebased Mindnever's FRSky S.Port dev branch as a single commit --- flight/libraries/frsky_packing.c | 671 ++++++++++++++++++ flight/libraries/inc/frsky_packing.h | 108 +++ .../UAVOFrSKYSPortBridge.c | 320 +++++++++ .../boards/revolution/firmware/Makefile | 2 + .../boards/revolution/firmware/UAVObjects.inc | 1 + .../frskysporttelemetrysettings.xml | 13 + shared/uavobjectdefinition/hwsettings.xml | 8 +- shared/uavobjectdefinition/taskinfo.xml | 3 + 8 files changed, 1122 insertions(+), 4 deletions(-) create mode 100644 flight/libraries/frsky_packing.c create mode 100644 flight/libraries/inc/frsky_packing.h create mode 100644 flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c create mode 100644 shared/uavobjectdefinition/frskysporttelemetrysettings.xml diff --git a/flight/libraries/frsky_packing.c b/flight/libraries/frsky_packing.c new file mode 100644 index 000000000..db0c2b308 --- /dev/null +++ b/flight/libraries/frsky_packing.c @@ -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(¤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: + 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; +} + +/** + * @} + * @} + */ diff --git a/flight/libraries/inc/frsky_packing.h b/flight/libraries/inc/frsky_packing.h new file mode 100644 index 000000000..2dd3dee44 --- /dev/null +++ b/flight/libraries/inc/frsky_packing.h @@ -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 */ + +/** + * @} + * @} + */ diff --git a/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c b/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c new file mode 100644 index 000000000..d2be268e1 --- /dev/null +++ b/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c @@ -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(¤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); + + 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); + } + } +} + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 4191203c9..c397006de 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -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 diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 88126753f..edfce7287 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -130,6 +130,7 @@ UAVOBJSRCFILENAMES += hottbridgesettings UAVOBJSRCFILENAMES += hottbridgestatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += frskysporttelemetrysettings UAVOBJSRCFILENAMES += systemidentsettings UAVOBJSRCFILENAMES += systemidentstate diff --git a/shared/uavobjectdefinition/frskysporttelemetrysettings.xml b/shared/uavobjectdefinition/frskysporttelemetrysettings.xml new file mode 100644 index 000000000..61fb54f4a --- /dev/null +++ b/shared/uavobjectdefinition/frskysporttelemetrysettings.xml @@ -0,0 +1,13 @@ + + + FrSKY S.Port Telemetry configuration. + + + + + + + + + + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index ae1098a5a..4a716ca61 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -11,11 +11,11 @@ - - - + 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;"/> + + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 674875421..7b4616d92 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -35,6 +35,7 @@ UAVOMSPBridge UAVOFrskySensorHubBridge UAVOMAVLinkBridge + FrSkySPortTelemetry AutoTune @@ -72,6 +73,7 @@ UAVOMSPBridge UAVOFrskySensorHubBridge UAVOMAVLinkBridge + FrSkySPortTelemetry AutoTune @@ -113,6 +115,7 @@ UAVOMSPBridge UAVOFrskySensorHubBridge UAVOMAVLinkBridge + FrSkySPortTelemetry AutoTune From 19318e88a843000291303de642009d85c37eaf09 Mon Sep 17 00:00:00 2001 From: Eric Price Date: Sat, 6 Jul 2019 13:28:02 +0200 Subject: [PATCH 2/6] LP-117 FRsky SPort: modifications to compile with current "next" --- flight/libraries/frsky_packing.c | 1 + .../UAVOFrSKYSPortBridge.c | 17 +++++++++++++--- flight/pios/common/pios_board_io.c | 11 ++++++++++ flight/pios/inc/pios_board_io.h | 20 +++++++++++++++---- .../revolution/firmware/inc/pios_config.h | 1 + .../boards/revolution/firmware/pios_board.c | 3 +++ 6 files changed, 46 insertions(+), 7 deletions(-) diff --git a/flight/libraries/frsky_packing.c b/flight/libraries/frsky_packing.c index db0c2b308..4c98e2ddf 100644 --- a/flight/libraries/frsky_packing.c +++ b/flight/libraries/frsky_packing.c @@ -216,6 +216,7 @@ bool frsky_encode_t1(struct frsky_settings *frsky, uint32_t *value, bool test_pr t1 = 300; break; case GPSPOSITIONSENSOR_STATUS_FIX3D: + case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS: if (hl_set == HOMELOCATION_SET_TRUE) { t1 = 500; } else { diff --git a/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c b/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c index d2be268e1..cbb9c2c48 100644 --- a/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c +++ b/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c @@ -1,12 +1,13 @@ /** ****************************************************************************** - * @addtogroup TauLabsModules TauLabs Modules + * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup UAVOFrSKYSPortBridge UAVO to FrSKY S.PORT Bridge Module * @{ * * @file uavofrskysportbridge.c - * @author Tau Labs, http://taulabs.org, Copyright (C) 2014 + * @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, @@ -35,6 +36,8 @@ #include "frsky_packing.h" +#include "pios_board_io.h" + #include "barosensor.h" #include "flightbatterysettings.h" #include "flightbatterystate.h" @@ -277,7 +280,12 @@ static int32_t uavoFrSKYSPortBridgeInitialize(void) 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->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; @@ -290,6 +298,9 @@ static int32_t uavoFrSKYSPortBridgeInitialize(void) FrSKYSPortTelemetrySettingsUpdatedCb(0); PIOS_COM_ChangeBaud(frsky->com, FRSKY_SPORT_BAUDRATE); + bool param = true; + PIOS_COM_Ioctl(frsky->com, PIOS_IOCTL_USART_SET_HALFDUPLEX, ¶m); + return 0; } diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index 2e447e15b..5fb3cf22f 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -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 */ }; diff --git a/flight/pios/inc/pios_board_io.h b/flight/pios/inc/pios_board_io.h index 1f8500fd9..fb08bff6b 100644 --- a/flight/pios/inc/pios_board_io.h +++ b/flight/pios/inc/pios_board_io.h @@ -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; diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 6694ff33d..2f567a313 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -162,6 +162,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 diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 595027436..02df0e232 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -75,6 +75,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 +90,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 +109,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, }; /** From d6c0141e012b1bcd8695fb1f3fcb950b473bf36e Mon Sep 17 00:00:00 2001 From: Eric Price Date: Sat, 6 Jul 2019 14:23:20 +0200 Subject: [PATCH 3/6] LP-117 FRSky SPort telemetry added settings to gcs and fixed categorization --- ground/gcs/src/plugins/uavobjects/uavobjects.pro | 1 + shared/uavobjectdefinition/frskysporttelemetrysettings.xml | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ground/gcs/src/plugins/uavobjects/uavobjects.pro b/ground/gcs/src/plugins/uavobjects/uavobjects.pro index 9706ad7ae..aff3e6dd0 100644 --- a/ground/gcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/gcs/src/plugins/uavobjects/uavobjects.pro @@ -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 \ diff --git a/shared/uavobjectdefinition/frskysporttelemetrysettings.xml b/shared/uavobjectdefinition/frskysporttelemetrysettings.xml index 61fb54f4a..00d10fecb 100644 --- a/shared/uavobjectdefinition/frskysporttelemetrysettings.xml +++ b/shared/uavobjectdefinition/frskysporttelemetrysettings.xml @@ -1,5 +1,5 @@ - + FrSKY S.Port Telemetry configuration. From 8aef698b6b7147d9a3b2f1ae001194bb918e11b9 Mon Sep 17 00:00:00 2001 From: Eric Price Date: Sat, 6 Jul 2019 20:10:06 +0200 Subject: [PATCH 4/6] LP-117 Allow USART Port options to be set in hwsettings, to allow suitable configuration based on fc board, presence of ext. inverters and wiring - defaults to Non-Inverted single wire connection. --- .../UAVOFrSKYSPortBridge.c | 42 +++++++++++++++---- .../frskysporttelemetrysettings.xml | 1 - shared/uavobjectdefinition/hwsettings.xml | 1 + 3 files changed, 35 insertions(+), 9 deletions(-) diff --git a/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c b/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c index cbb9c2c48..84c615a4f 100644 --- a/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c +++ b/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c @@ -43,6 +43,7 @@ #include "flightbatterystate.h" #include "gpspositionsensor.h" #include "frskysporttelemetrysettings.h" +#include "hwsettings.h" #include "taskinfo.h" @@ -88,7 +89,6 @@ struct frsky_sport_telemetry { 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; @@ -254,8 +254,6 @@ static void FrSKYSPortTelemetrySettingsUpdatedCb(__attribute__((unused)) UAVObjE FrSKYSPortTelemetrySettingsData settings; FrSKYSPortTelemetrySettingsGet(&settings); - - frsky->master = (settings.PortMaster == FRSKYSPORTTELEMETRYSETTINGS_PORTMASTER_TRUE); } @@ -293,15 +291,43 @@ static int32_t uavoFrSKYSPortBridgeInitialize(void) 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); - PIOS_COM_ChangeBaud(frsky->com, FRSKY_SPORT_BAUDRATE); - bool param = true; - PIOS_COM_Ioctl(frsky->com, PIOS_IOCTL_USART_SET_HALFDUPLEX, ¶m); - - return 0; } } diff --git a/shared/uavobjectdefinition/frskysporttelemetrysettings.xml b/shared/uavobjectdefinition/frskysporttelemetrysettings.xml index 00d10fecb..2cd55c85b 100644 --- a/shared/uavobjectdefinition/frskysporttelemetrysettings.xml +++ b/shared/uavobjectdefinition/frskysporttelemetrysettings.xml @@ -3,7 +3,6 @@ FrSKY S.Port Telemetry configuration. - diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 4a716ca61..399c88e72 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -35,6 +35,7 @@ + From 30332e5923d15e506c91b5250286fda9b14aa869 Mon Sep 17 00:00:00 2001 From: Eric Price Date: Sat, 6 Jul 2019 20:14:45 +0200 Subject: [PATCH 5/6] LP-117 Added Librepilot Copyright header --- flight/libraries/frsky_packing.c | 3 ++- flight/libraries/inc/frsky_packing.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/flight/libraries/frsky_packing.c b/flight/libraries/frsky_packing.c index 4c98e2ddf..59a8a4fcb 100644 --- a/flight/libraries/frsky_packing.c +++ b/flight/libraries/frsky_packing.c @@ -2,7 +2,8 @@ ****************************************************************************** * * @file frsky_packing.c - * @author Tau Labs, http://taulabs.org, Copyright (C) 2015 + * @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, diff --git a/flight/libraries/inc/frsky_packing.h b/flight/libraries/inc/frsky_packing.h index 2dd3dee44..7221a74ba 100644 --- a/flight/libraries/inc/frsky_packing.h +++ b/flight/libraries/inc/frsky_packing.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file frsky_packing.h - * @author Tau Labs, http://taulabs.org, Copyright (C) 2015 + * @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, From f32677af52f6b062f40f32fb4f10ee0119c8486e Mon Sep 17 00:00:00 2001 From: Eric Price Date: Sat, 6 Jul 2019 23:07:06 +0200 Subject: [PATCH 6/6] LP-117 Added PPM+FrSkySPort mode (most port-saving setup, keeps flexi and main free!) --- flight/targets/boards/revolution/firmware/pios_board.c | 2 ++ shared/uavobjectdefinition/hwsettings.xml | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 02df0e232..bc142c7b3 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -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, @@ -298,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); diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 399c88e72..d3305074f 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -11,9 +11,9 @@ - + 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;"/>