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