diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 7f75b1d65..ead134f14 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -1,11 +1,12 @@ pipelines: default: - step: + image: atlassian/default-image:3 + runs-on: self.hosted script: - - add-apt-repository ppa:librepilot/tools -y - apt-get update -q - - apt-get install -y libc6-i386 libudev-dev libusb-1.0-0-dev libsdl1.2-dev python libopenscenegraph-dev libosgearth-dev qt56-meta-minimal qt56svg qt56script qt56serialport qt56multimedia qt56translations qt56tools + - DEBIAN_FRONTEND=noninteractive apt-get install -y build-essential qtbase5-dev git curl libc6-i386 python2 python2.7-dev libqt5svg5-dev qt5-qmltooling-plugins libqt5webview5-dev qtscript5-dev libqt5serialport5-dev qtmultimedia5-dev libusb-1.0-0-dev libudev-dev pkg-config libsdl-dev libosgearth-dev qttools5-dev-tools - make build_sdk_install - - make all_flight + - make all_flight --jobs=6 - make fw_resource - - make gcs + - make gcs --jobs=6 diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index b9a977a8b..9289611b0 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -283,7 +283,7 @@ SystemAlarmsAlarmOptions AlarmsGetHighestSeverity() } -static const char *const systemalarms_severity_names[] = { +__attribute__((unused)) static const char *const systemalarms_severity_names[] = { [SYSTEMALARMS_ALARM_UNINITIALISED] = "UNINITIALISED", [SYSTEMALARMS_ALARM_OK] = "OK", [SYSTEMALARMS_ALARM_WARNING] = "WARNING", diff --git a/flight/libraries/frsky_packing.c b/flight/libraries/frsky_packing.c new file mode 100644 index 000000000..59a8a4fcb --- /dev/null +++ b/flight/libraries/frsky_packing.c @@ -0,0 +1,673 @@ +/** + ****************************************************************************** + * + * @file frsky_packing.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017-2019 + * Tau Labs, http://taulabs.org, Copyright (C) 2015 + * @brief Packs UAVObjects into FrSKY Smart Port frames + * + * Since there is no public documentation of SmartPort protocol available, + * this was put together by studying OpenTx source code, reading + * tidbits of informations on the Internet and experimenting. + * @see https://github.com/opentx/opentx/tree/next/radio/src/telemetry + * @see https://code.google.com/p/telemetry-convert/wiki/FrSkySPortProtocol + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "frsky_packing.h" +#include "frskysporttelemetrysettings.h" +#include "attitudestate.h" +#include "barosensor.h" +#include "positionstate.h" +#include "velocitystate.h" +#include "flightbatterystate.h" +#include "flightbatterysettings.h" +#include "gpstime.h" +#include "homelocation.h" +#include "accelstate.h" +#include "flightstatus.h" +#include "airspeedstate.h" +#include "nedaccel.h" + +#define GRAVITY 9.805f // [m/s^2] +#define KNOTS2M_PER_SECOND 0.514444444f + + +/** + * Encode baro altitude value + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_altitude(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (!frsky->use_baro_sensor || (PositionStateHandle() == NULL)) { + return false; + } + + if (test_presence_only) { + return true; + } + // instead of encoding baro altitude directly, we will use + // more accurate estimation in PositionState UAVO + float down = 0; + PositionStateDownGet(&down); + int32_t alt = (int32_t)(-down * 100.0f); + *value = (uint32_t)alt; + return true; +} + +/** + * Encode heading value + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_gps_course(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (AttitudeStateHandle() == NULL) { + return false; + } + + if (test_presence_only) { + return true; + } + + AttitudeStateData attitude; + AttitudeStateGet(&attitude); + float hdg = (attitude.Yaw >= 0) ? attitude.Yaw : (attitude.Yaw + 360.0f); + *value = (uint32_t)(hdg * 100.0f); + + return true; +} + +/** + * Encode vertical speed value + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_vario(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (!frsky->use_baro_sensor || VelocityStateHandle() == NULL) { + return false; + } + + if (test_presence_only) { + return true; + } + + float down = 0; + VelocityStateDownGet(&down); + int32_t vspeed = (int32_t)(-down * 100.0f); + *value = (uint32_t)vspeed; + + return true; +} + +/** + * Encode battery current value + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_current(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (!frsky->use_current_sensor) { + return false; + } + if (test_presence_only) { + return true; + } + + float current = 0; + FlightBatteryStateCurrentGet(¤t); + int32_t current_frsky = (int32_t)(current * 10.0f); + *value = (uint32_t)current_frsky; + + return true; +} + +/** + * Encode battery cells voltage + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[], index of battery cell pair + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_cells(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg) +{ + if ((frsky->batt_cell_count == 0) || (frsky->batt_cell_count - 1) < (int)(arg * 2)) { + return false; + } + if (test_presence_only) { + return true; + } + + float voltage = 0; + FlightBatteryStateVoltageGet(&voltage); + + uint32_t cell_voltage = (uint32_t)((voltage * 500.0f) / frsky->batt_cell_count); + *value = ((cell_voltage & 0xfff) << 8) | ((arg * 2) & 0x0f) | ((frsky->batt_cell_count << 4) & 0xf0); + if (((int16_t)frsky->batt_cell_count - 1) >= (int)(arg * 2 + 1)) { + *value |= ((cell_voltage & 0xfff) << 20); + } + + return true; +} + +/** + * Encode GPS status as T1 value + * Right-most two digits means visible satellite count, left-most digit has following meaning: + * 1 - no GPS connected + * 2 - no fix + * 3 - 2D fix + * 4 - 3D fix + * 5 - 3D fix and HomeLocation is SET - should be safe for navigation + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value successfully encoded or presence test passed + */ +bool frsky_encode_t1(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (GPSPositionSensorHandle() == NULL) { + return false; + } + + if (test_presence_only) { + return true; + } + + uint8_t hl_set = HOMELOCATION_SET_FALSE; + + if (HomeLocationHandle()) { + HomeLocationSetGet(&hl_set); + } + + int32_t t1 = 0; + switch (frsky->gps_position.Status) { + case GPSPOSITIONSENSOR_STATUS_NOGPS: + t1 = 100; + break; + case GPSPOSITIONSENSOR_STATUS_NOFIX: + t1 = 200; + break; + case GPSPOSITIONSENSOR_STATUS_FIX2D: + t1 = 300; + break; + case GPSPOSITIONSENSOR_STATUS_FIX3D: + case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS: + if (hl_set == HOMELOCATION_SET_TRUE) { + t1 = 500; + } else { + t1 = 400; + } + break; + } + if (frsky->gps_position.Satellites > 0) { + t1 += frsky->gps_position.Satellites; + } + + *value = (uint32_t)t1; + + return true; +} + +/** + * Encode GPS hDop and vDop as T2 + * Bits 0-7 = hDop * 100, max 255 (hDop = 2.55) + * Bits 8-15 = vDop * 100, max 255 (vDop = 2.55) + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value successfully encoded or presence test passed + */ +bool frsky_encode_t2(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (GPSPositionSensorHandle() == NULL) { + return false; + } + + if (test_presence_only) { + return true; + } + + uint32_t hdop = (uint32_t)(frsky->gps_position.HDOP * 100.0f); + + if (hdop > 255) { + hdop = 255; + } + + uint32_t vdop = (uint32_t)(frsky->gps_position.VDOP * 100.0f); + + if (vdop > 255) { + vdop = 255; + } + + *value = 256 * vdop + hdop; + + return true; +} + +/** + * Encode consumed battery energy as fuel + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_fuel(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (!frsky->use_current_sensor) { + return false; + } + if (test_presence_only) { + return true; + } + + uint32_t capacity = frsky->battery_settings.Capacity; + float consumed_mahs = 0; + FlightBatteryStateConsumedEnergyGet(&consumed_mahs); + + float fuel = (uint32_t)(100.0f * (1.0f - consumed_mahs / capacity)); + fuel = boundf(fuel, 0.0f, 100.0f); + *value = (uint32_t)fuel; + + return true; +} + +/** + * Encode configured values + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[]; 0=X, 1=Y, 2=Z + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_acc(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg) +{ + uint8_t accelDataSettings; + + FrSKYSPortTelemetrySettingsAccelDataGet(&accelDataSettings); + + float acc = 0; + switch (accelDataSettings) { + case FRSKYSPORTTELEMETRYSETTINGS_ACCELDATA_ACCELS: + { + if (AccelStateHandle() == NULL) { + return false; + } else if (test_presence_only) { + return true; + } + + switch (arg) { + case 0: + AccelStatexGet(&acc); + break; + case 1: + AccelStateyGet(&acc); + break; + case 2: + AccelStatezGet(&acc); + break; + } + + acc /= GRAVITY; + acc *= 100.0f; + break; + } + + case FRSKYSPORTTELEMETRYSETTINGS_ACCELDATA_NEDACCELS: + { + if (NedAccelHandle() == NULL) { + return false; + } else if (test_presence_only) { + return true; + } + + switch (arg) { + case 0: + NedAccelNorthGet(&acc); + break; + case 1: + NedAccelEastGet(&acc); + break; + case 2: + NedAccelDownGet(&acc); + break; + } + + acc /= GRAVITY; + acc *= 100.0f; + break; + } + + case FRSKYSPORTTELEMETRYSETTINGS_ACCELDATA_NEDVELOCITY: + { + if (VelocityStateHandle() == NULL) { + return false; + } else if (test_presence_only) { + return true; + } + + switch (arg) { + case 0: + VelocityStateNorthGet(&acc); + break; + case 1: + VelocityStateEastGet(&acc); + break; + case 2: + VelocityStateDownGet(&acc); + break; + } + + acc *= 100.0f; + break; + } + + case FRSKYSPORTTELEMETRYSETTINGS_ACCELDATA_ATTITUDEANGLES: + { + if (AttitudeStateHandle() == NULL) { + return false; + } else if (test_presence_only) { + return true; + } + + switch (arg) { + case 0: + AttitudeStateRollGet(&acc); + break; + case 1: + AttitudeStatePitchGet(&acc); + break; + case 2: + AttitudeStateYawGet(&acc); + break; + } + + acc *= 100.0f; + break; + } + } + + int32_t frsky_acc = (int32_t)acc; + *value = (uint32_t)frsky_acc; + + return true; +} + +/** + * Encode gps coordinates + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[]; 0=lattitude, 1=longitude + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_gps_coord(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg) +{ + if (GPSPositionSensorHandle() == NULL) { + return false; + } + if (frsky->gps_position.Status == GPSPOSITIONSENSOR_STATUS_NOFIX + || frsky->gps_position.Status == GPSPOSITIONSENSOR_STATUS_NOGPS) { + return false; + } + if (test_presence_only) { + return true; + } + + uint32_t frsky_coord = 0; + int32_t coord = 0; + if (arg == 0) { + // lattitude + coord = frsky->gps_position.Latitude; + if (coord >= 0) { + frsky_coord = 0; + } else { + frsky_coord = 1 << 30; + } + } else { + // longitude + coord = frsky->gps_position.Longitude; + if (coord >= 0) { + frsky_coord = 2 << 30; + } else { + frsky_coord = 3 << 30; + } + } + coord = abs(coord); + frsky_coord |= (((uint64_t)coord * 6ull) / 100); + + *value = frsky_coord; + return true; +} + +/** + * Encode gps altitude + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_gps_alt(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (GPSPositionSensorHandle() == NULL) { + return false; + } + if (frsky->gps_position.Status != GPSPOSITIONSENSOR_STATUS_FIX3D) { + return false; + } + if (test_presence_only) { + return true; + } + + int32_t frsky_gps_alt = (int32_t)(frsky->gps_position.Altitude * 100.0f); + *value = (uint32_t)frsky_gps_alt; + + return true; +} + +/** + * Encode gps speed + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_gps_speed(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (GPSPositionSensorHandle() == NULL) { + return false; + } + if (frsky->gps_position.Status != GPSPOSITIONSENSOR_STATUS_FIX3D) { + return false; + } + if (test_presence_only) { + return true; + } + + int32_t frsky_speed = (int32_t)((frsky->gps_position.Groundspeed / KNOTS2M_PER_SECOND) * 1000); + *value = frsky_speed; + return true; +} + +/** + * Encode GPS UTC time + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[]; 0=date, 1=time + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_gps_time(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg) +{ + if (GPSPositionSensorHandle() == NULL || GPSTimeHandle() == NULL) { + return false; + } + if (frsky->gps_position.Status != GPSPOSITIONSENSOR_STATUS_FIX3D) { + return false; + } + if (test_presence_only) { + return true; + } + + GPSTimeData gps_time; + GPSTimeGet(&gps_time); + uint32_t frsky_time = 0; + + if (arg == 0) { + // encode date + frsky_time = 0x000000ff; + frsky_time |= gps_time.Day << 8; + frsky_time |= gps_time.Month << 16; + frsky_time |= (gps_time.Year % 100) << 24; + } else { + frsky_time = 0; + frsky_time |= gps_time.Second << 8; + frsky_time |= gps_time.Minute << 16; + frsky_time |= gps_time.Hour << 24; + } + *value = frsky_time; + return true; +} + +/** + * Encodes ARM status and flight mode number as RPM value + * Since there is no RPM information in any UAVO available, + * we will intentionaly misuse this item to encode another useful information. + * It will encode flight status as three-digit number as follow: + * most left digit encodes arm status (1=armed, 0=disarmed) + * two most right digits encodes flight mode number (see FlightStatus UAVO FlightMode enum) + * To work this propperly on Taranis, you have to set Blades to "1" in telemetry setting + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_rpm(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (FlightStatusHandle() == NULL) { + return false; + } + if (test_presence_only) { + return true; + } + + FlightStatusData flight_status; + FlightStatusGet(&flight_status); + + *value = (flight_status.Armed == FLIGHTSTATUS_ARMED_ARMED) ? 200 : 100; + *value += flight_status.FlightMode; + + return true; +} + +/** + * Encode true airspeed(TAS) + * @param[out] value encoded value + * @param[in] test_presence_only true when function should only test for availability of this value + * @param[in] arg argument specified in frsky_value_items[] + * @returns true when value succesfully encoded or presence test passed + */ +bool frsky_encode_airspeed(__attribute__((unused)) struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, __attribute__((unused)) uint32_t arg) +{ + if (AirspeedStateHandle() == NULL) { + return false; + } + if (test_presence_only) { + return true; + } + + AirspeedStateData airspeed; + AirspeedStateGet(&airspeed); + int32_t frsky_speed = (int32_t)((airspeed.TrueAirspeed / KNOTS2M_PER_SECOND) * 10); + *value = (uint32_t)frsky_speed; + + return true; +} + +/** + * Performs byte stuffing and checksum calculation + * @param[out] obuff buffer where byte stuffed data will came in + * @param[in,out] chk checksum byte to update + * @param[in] byte + * @returns count of bytes inserted to obuff (1 or 2) + */ +uint8_t frsky_insert_byte(uint8_t *obuff, uint16_t *chk, uint8_t byte) +{ + /* checksum calculation is based on data before byte-stuffing */ + *chk += byte; + *chk += (*chk) >> 8; + *chk &= 0x00ff; + *chk += (*chk) >> 8; + *chk &= 0x00ff; + + if (byte == 0x7e || byte == 0x7d) { + obuff[0] = 0x7d; + obuff[1] = byte &= ~0x20; + return 2; + } + + obuff[0] = byte; + return 1; +} + +/** + * Send u32 value dataframe to FrSky SmartPort bus + * @param[in] id FrSky value ID + * @param[in] value value + */ +int32_t frsky_send_frame(uintptr_t com, enum frsky_value_id id, uint32_t value, + bool send_prelude) +{ + /* each call of frsky_insert_byte can add 2 bytes to the buffer at maximum + * and therefore the worst-case is 17 bytes total (the first byte 0x10 won't be + * escaped) */ + uint8_t tx_data[17]; + uint16_t chk = 0; + uint8_t cnt = 0; + + if (send_prelude) { + tx_data[0] = 0x7e; + tx_data[1] = 0x98; + cnt = 2; + } + + cnt += frsky_insert_byte(&tx_data[cnt], &chk, 0x10); + cnt += frsky_insert_byte(&tx_data[cnt], &chk, (uint16_t)id & 0xff); + cnt += frsky_insert_byte(&tx_data[cnt], &chk, ((uint16_t)id >> 8) & 0xff); + cnt += frsky_insert_byte(&tx_data[cnt], &chk, value & 0xff); + cnt += frsky_insert_byte(&tx_data[cnt], &chk, (value >> 8) & 0xff); + cnt += frsky_insert_byte(&tx_data[cnt], &chk, (value >> 16) & 0xff); + cnt += frsky_insert_byte(&tx_data[cnt], &chk, (value >> 24) & 0xff); + cnt += frsky_insert_byte(&tx_data[cnt], &chk, 0xff - chk); + + PIOS_COM_SendBuffer(com, tx_data, cnt); + + return cnt; +} + +/** + * @} + * @} + */ diff --git a/flight/libraries/inc/frsky_packing.h b/flight/libraries/inc/frsky_packing.h new file mode 100644 index 000000000..7221a74ba --- /dev/null +++ b/flight/libraries/inc/frsky_packing.h @@ -0,0 +1,109 @@ +/** + ****************************************************************************** + * + * @file frsky_packing.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017-2019 + * Tau Labs, http://taulabs.org, Copyright (C) 2015 + * @brief Packs UAVObjects into FrSKY Smart Port frames + * + * Since there is no public documentation of SmartPort protocol available, + * this was put together by studying OpenTx source code, reading + * tidbits of informations on the Internet and experimenting. + * @see https://github.com/opentx/opentx/tree/next/radio/src/telemetry + * @see https://code.google.com/p/telemetry-convert/wiki/FrSkySPortProtocol + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef FRSKY_PACKING_H +#define FRSKY_PACKING_H + +#include "pios.h" +#include "openpilot.h" + +#include "flightbatterysettings.h" +#include "gpspositionsensor.h" + +struct frsky_settings { + bool use_current_sensor; + uint8_t batt_cell_count; + bool use_baro_sensor; + FlightBatterySettingsData battery_settings; + GPSPositionSensorData gps_position; +}; + +enum frsky_value_id { + FRSKY_ALT_ID = 0x0100, + FRSKY_VARIO_ID = 0x110, + FRSKY_CURR_ID = 0x0200, + FRSKY_VFAS_ID = 0x0210, + FRSKY_CELLS_ID = 0x0300, + FRSKY_T1_ID = 0x0400, + FRSKY_T2_ID = 0x0410, + FRSKY_RPM_ID = 0x0500, + FRSKY_FUEL_ID = 0x0600, + FRSKY_ACCX_ID = 0x0700, + FRSKY_ACCY_ID = 0x0710, + FRSKY_ACCZ_ID = 0x0720, + FRSKY_GPS_LON_LAT_ID = 0x0800, + FRSKY_GPS_ALT_ID = 0x0820, + FRSKY_GPS_SPEED_ID = 0x0830, + FRSKY_GPS_COURSE_ID = 0x0840, + FRSKY_GPS_TIME_ID = 0x0850, + FRSKY_ADC3_ID = 0x0900, + FRSKY_ADC4_ID = 0x0910, + FRSKY_AIR_SPEED_ID = 0x0a00, + FRSKY_RSSI_ID = 0xf101, + FRSKY_ADC1_ID = 0xf102, + FRSKY_ADC2_ID = 0xf103, + FRSKY_BATT_ID = 0xf104, + FRSKY_SWR_ID = 0xf105, +}; + +struct frsky_value_item { + enum frsky_value_id id; + uint16_t period_ms; + bool (*encode_value)(struct frsky_settings *sport, uint32_t *value, bool test_presence_only, uint32_t arg); + uint32_t fn_arg; +}; + +bool frsky_encode_gps_course(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_altitude(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_vario(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_current(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_cells(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_t1(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_t2(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_fuel(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_acc(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_gps_coord(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_gps_alt(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_gps_speed(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_gps_time(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_rpm(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +bool frsky_encode_airspeed(struct frsky_settings *frsky, uint32_t *value, bool test_presence_only, uint32_t arg); +uint8_t frsky_insert_byte(uint8_t *obuff, uint16_t *chk, uint8_t byte); +int32_t frsky_send_frame(uintptr_t com, enum frsky_value_id id, uint32_t value, + bool send_prelude); + +#endif /* FRSKY_PACKING_H */ + +/** + * @} + * @} + */ diff --git a/flight/libraries/math/mathmisc.c b/flight/libraries/math/mathmisc.c index f1bff5751..3f951794d 100644 --- a/flight/libraries/math/mathmisc.c +++ b/flight/libraries/math/mathmisc.c @@ -29,6 +29,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#include #include void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size) diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index f586d65ea..dcd70e5df 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -107,7 +107,7 @@ static inline void vector_normalizef(float *vector, const uint8_t dim) { float length = vector_lengthf(vector, dim); - if (length <= 0.0f || isnan(length)) { + if (length <= 0.0f || !IS_REAL(length)) { return; } for (int t = 0; t < dim; t++) { diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index f861c953c..ebfccc20d 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -148,6 +148,7 @@ int32_t configuration_check() { ADDSEVERITY(!gps_assisted); } + // fall through case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYROAM: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND: @@ -195,11 +196,11 @@ int32_t configuration_check() ManualControlSettingsChannelMaxGet(&channelMax); switch (thrustType) { case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: - ADDSEVERITY(fabsf(channelMax.Throttle - channelMin.Throttle) > 300.0f); + ADDSEVERITY(fabsf((float)(channelMax.Throttle - channelMin.Throttle)) > 300.0f); ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0); break; case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: - ADDSEVERITY(fabsf(channelMax.Collective - channelMin.Collective) > 300.0f); + ADDSEVERITY(fabsf((float)(channelMax.Collective - channelMin.Collective)) > 300.0f); ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0); break; default: diff --git a/flight/make/apps-defs.mk b/flight/make/apps-defs.mk index 77620dabf..628447ba8 100644 --- a/flight/make/apps-defs.mk +++ b/flight/make/apps-defs.mk @@ -74,10 +74,12 @@ SRC += $(PIOSCOMMON)/pios_adxl345.c SRC += $(PIOSCOMMON)/pios_bma180.c SRC += $(PIOSCOMMON)/pios_bmp085.c SRC += $(PIOSCOMMON)/pios_etasv3.c +SRC += $(PIOSCOMMON)/pios_sdp3x.c SRC += $(PIOSCOMMON)/pios_gcsrcvr.c SRC += $(PIOSCOMMON)/pios_hcsr04.c SRC += $(PIOSCOMMON)/pios_hmc5843.c SRC += $(PIOSCOMMON)/pios_hmc5x83.c +SRC += $(PIOSCOMMON)/pios_qmc5883l.c SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_l3gd20.c SRC += $(PIOSCOMMON)/pios_mpu6000.c diff --git a/flight/make/common-defs.mk b/flight/make/common-defs.mk index 933aedc8f..061be4af9 100644 --- a/flight/make/common-defs.mk +++ b/flight/make/common-defs.mk @@ -128,7 +128,7 @@ CFLAGS += -fomit-frame-pointer CFLAGS += -Wall -Wextra CFLAGS += -Wfloat-equal -Wdouble-promotion CFLAGS += -Wshadow -CFLAGS += -Werror +CFLAGS += -Werror -Wno-address-of-packed-member CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<)))) diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index c8f7c40f1..765c637d4 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -43,6 +43,7 @@ #include "airspeedsensor.h" // object that will be updated by the module #include "baro_airspeed_ms4525do.h" #include "baro_airspeed_etasv3.h" +#include "baro_airspeed_sdp3x.h" #include "baro_airspeed_mpxv.h" #include "imu_airspeed.h" #include "airspeedalarm.h" @@ -186,6 +187,12 @@ static void airspeedTask(__attribute__((unused)) void *parameters) baro_airspeedGetETASV3(&airspeedData, &airspeedSettings); break; #endif +#if defined(PIOS_INCLUDE_SDP3X) + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_SDP3X: + // SDP3X + baro_airspeedGetSDP3X(&airspeedData, &airspeedSettings); + break; +#endif #if defined(PIOS_INCLUDE_MS4525DO) case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO: // PixHawk Airpeed based on MS4525DO diff --git a/flight/modules/Airspeed/baro_airspeed_sdp3x.c b/flight/modules/Airspeed/baro_airspeed_sdp3x.c new file mode 100644 index 000000000..71bb4930d --- /dev/null +++ b/flight/modules/Airspeed/baro_airspeed_sdp3x.c @@ -0,0 +1,97 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Communicate with airspeed sensors and return values + * @{ + * + * @file baro_airspeed.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Airspeed module, handles temperature and pressure readings from BMP085 + * + * @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 + */ + +/** + * Output object: BaroAirspeed + * + * This module will periodically update the value of the BaroAirspeed object. + * + */ + +#include "openpilot.h" +#include "hwsettings.h" +#include "airspeedsettings.h" +#include "airspeedsensor.h" // object that will be updated by the module +#include "airspeedalarm.h" + +#if defined(PIOS_INCLUDE_SDP3X) + +#define CALIBRATION_IDLE_MS 2000 // Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 2000 // Time to spend calibrating, in [ms] + +#define P0 101325.0f // standard pressure +#define CCEXPONENT 0.2857142857f // exponent of compressibility correction 2/7 +#define CASFACTOR 760.8802669f // sqrt(5) * speed of sound at standard +#define TASFACTOR 0.05891022589f // 1/sqrt(T0) + +// Private types + +// Private variables + +// Private functions + +static volatile struct PIOS_SDP3X_STATE state = { .status = SDP3X_STATUS_NOTFOUND, .pressure = 0, .temperature = 0 }; + +void baro_airspeedGetSDP3X(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) +{ + // Check to see if airspeed sensor is returning airspeedSensor + PIOS_SDP3X_ReadAirspeed(&state); + airspeedSensor->SensorValue = state.pressure; + airspeedSensor->SensorValueTemperature = state.temperature; + + if (state.status != SDP3X_STATUS_READY) { + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + airspeedSensor->CalibratedAirspeed = 0; + AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); + // At boot time, sensor needs time for self calibration during which it may not be disturbed. Wait 2 seconds before next attempt + vTaskDelay(2000 / portTICK_RATE_MS); + return; + } + + // No calibration, sensor comes factory calibrated! + + // Compute airspeed + airspeedSensor->Temperature = ((float)state.temperature / 200.0f) + 273.15f; // convert to kelvin + airspeedSensor->DifferentialPressure = (1.0f / (((float)state.scale) + 1e-9f)) * ((float)(state.pressure) - (float)((int16_t)(airspeedSettings->ZeroPoint))); + airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf(powf(fabsf(airspeedSensor->DifferentialPressure) / P0 + 1.0f, CCEXPONENT) - 1.0f); + airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(airspeedSensor->Temperature); + + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + AirspeedAlarm(SYSTEMALARMS_ALARM_OK); +} + + +#endif /* if defined(PIOS_INCLUDE_SDP3X) */ + +/** + * @} + * @} + */ diff --git a/flight/modules/Airspeed/inc/baro_airspeed_sdp3x.h b/flight/modules/Airspeed/inc/baro_airspeed_sdp3x.h new file mode 100644 index 000000000..658dae560 --- /dev/null +++ b/flight/modules/Airspeed/inc/baro_airspeed_sdp3x.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements + * @{ + * + * @file baro_airspeed_etasv3.h + * @author The LibrePilot Team, http://www.librepilot.org Copyright (C) 2021. + * @brief Airspeed module, reads temperature and pressure from SDP3X + * + * @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 BARO_AIRSPEED_SDP3X_H +#define BARO_AIRSPEED_SDP3X_H +#if defined(PIOS_INCLUDE_SDP3X) + +void baro_airspeedGetSDP3X(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings); + +#endif +#endif // BARO_AIRSPEED_SDP3X_H + +/** + * @} + * @} + */ diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 8a9719efc..561bc03a7 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -538,7 +538,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData * accels[2] *= accel_scale.Z * invcount; temp *= invcount; - if (isnan(temperature)) { + if (!IS_REAL(temperature)) { temperature = temp; } temperature = temp_alpha * (temp - temperature) + temperature; @@ -740,7 +740,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN - if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) { + if ((fabsf(inv_qmag) > 1e3f) || !IS_REAL(inv_qmag)) { q[0] = 1; q[1] = 0; q[2] = 0; diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index f03f72063..9a239c4de 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -707,14 +707,14 @@ static void InitSystemIdent(bool loadDefaults) systemIdentSettings.Tau = u.systemIdentState.Tau; systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); - systemIdentSettings.Complete = u.systemIdentState.Complete; + systemIdentSettings.Complete = (SystemIdentSettingsCompleteOptions)u.systemIdentState.Complete; } else { // Tau, GyroReadTimeAverage, Beta, and the Complete flag get stored values // so the user can fly another battery to select and test PIDs with the slider/knob u.systemIdentState.Tau = systemIdentSettings.Tau; u.systemIdentState.GyroReadTimeAverage = systemIdentSettings.GyroReadTimeAverage; memcpy(&u.systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData)); - u.systemIdentState.Complete = systemIdentSettings.Complete; + u.systemIdentState.Complete = (SystemIdentStateCompleteOptions)systemIdentSettings.Complete; } SystemIdentStateSet(&u.systemIdentState); diff --git a/flight/modules/GPS/DJI.c b/flight/modules/GPS/DJI.c index a178e1cf2..07e8d85d6 100644 --- a/flight/modules/GPS/DJI.c +++ b/flight/modules/GPS/DJI.c @@ -382,6 +382,8 @@ uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *gpsPosi gpsPosition->HDOP = 99.99f; gpsPosition->PDOP = 99.99f; gpsPosition->VDOP = 99.99f; + // clear out satellite data because DJI doesn't provide it + GPSSatellitesSetDefaults(GPSSatellitesHandle(), 0); djiInitialized = true; } diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index fd7eb9a05..32e68ebb3 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -51,6 +51,9 @@ #include "NMEA.h" #include "UBX.h" #include "DJI.h" +#ifdef PIOS_INCLUDE_SENSORS_AUXMAG +#include "sensors.h" +#endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #include "inc/ubx_autoconfig.h" #define FULL_UBX_PARSER @@ -72,7 +75,7 @@ PERF_DEFINE_COUNTER(counterParse); #if defined(ANY_GPS_PARSER) && !defined(PIOS_GPS_MINIMAL) #define ANY_FULL_GPS_PARSER #endif -#if (defined(PIOS_INCLUDE_HMC5X83) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) +#if (defined(PIOS_INCLUDE_SENSORS_AUXMAG) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) #define ANY_FULL_MAG_PARSER #endif @@ -97,8 +100,9 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev); // **************** // Private constants -// GPS timeout is greater than 1000ms so that a stock GPS configuration can be used without timeout errors -#define GPS_TIMEOUT_MS 1250 +// GPS timeout is greater than 1000ms so that a stock GPS configuration +// or 2400bds can be used without timeout errors +#define GPS_TIMEOUT_MS 1800 // delay from detecting HomeLocation.Set == False before setting new homelocation // this prevent that a save with homelocation.Set = false triggered by gps ends saving @@ -126,7 +130,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev); // are run often enough. // GPS_LOOP_DELAY_MS on the other hand, should be less then 5.55 ms. A value set too high will cause data to be dropped. -#define GPS_LOOP_DELAY_MS 5 +#define GPS_LOOP_DELAY_MS 4 #define GPS_BLOCK_ON_NO_DATA_MS 20 #ifdef PIOS_GPS_SETS_HOMELOCATION @@ -552,11 +556,12 @@ void gps_set_fc_baud_from_arg(uint8_t baud) // can the code stand having two tasks/threads do an XyzSet() call at the same time? if (__sync_fetch_and_add(&mutex, 1) == 0) { // don't bother doing the baud change if it is actually the same - // might drop less data if (previous_baud != baud) { previous_baud = baud; // Set Revo port hwsettings_baud PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud)); + // clear Rx buffer + PIOS_COM_ClearRxBuffer(PIOS_COM_GPS); GPSPositionSensorBaudRateSet(&baud); } } @@ -571,15 +576,14 @@ static void gps_set_fc_baud_from_settings() uint8_t speed; // Retrieve settings + HwSettingsGPSSpeedGet(&speed); + #if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL) if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) { speed = HWSETTINGS_GPSSPEED_115200; - } else { -#endif - HwSettingsGPSSpeedGet(&speed); -#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL) -} + } #endif + // set fc baud gps_set_fc_baud_from_arg(speed); } @@ -653,8 +657,8 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) #if defined(PIOS_INCLUDE_GPS_DJI_PARSER) dji_load_mag_settings(); #endif -#if defined(PIOS_INCLUDE_HMC5X83) - aux_hmc5x83_load_mag_settings(); +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) + sensors_auxmag_load_mag_settings(); #endif } #endif /* defined(ANY_FULL_MAG_PARSER) */ @@ -663,16 +667,19 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) #if defined(ANY_FULL_GPS_PARSER) void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) { + GPSSettingsDataProtocolOptions previous_data_protocol = gpsSettings.DataProtocol; ubx_autoconfig_settings_t newconfig; GPSSettingsGet(&gpsSettings); #if defined(PIOS_INCLUDE_GPS_DJI_PARSER) - // each time there is a protocol change, set the baud rate - // so that DJI can be forced to 115200, but changing to another protocol will change the baud rate to the user specified value // note that changes to HwSettings GPS baud rate are detected in the HwSettings callback, - // but changing to/from DJI is effectively a baud rate change because DJI is forced to be 115200 - gps_set_fc_baud_from_settings(); // knows to force 115200 for DJI + // but changing to/from DJI in GPSSettings is effectively a baud rate change because DJI is forced to be 115200 + if (((gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) && (previous_data_protocol != GPSSETTINGS_DATAPROTOCOL_DJI)) || + ((gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_DJI) && (previous_data_protocol == GPSSETTINGS_DATAPROTOCOL_DJI))) { + // knows to force 115200 for DJI + gps_set_fc_baud_from_settings(); + } #endif // it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running another protocol @@ -791,12 +798,6 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) gps_ubx_autoconfig_set(&newconfig); } #endif -#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) - if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) { - // clear out satellite data because DJI doesn't provide it - GPSSatellitesSetDefaults(GPSSatellitesHandle(), 0); - } -#endif } #endif /* defined(ANY_FULL_GPS_PARSER) */ /** diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index f622ea87a..1583f188e 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -677,6 +677,5 @@ uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *); int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *); void op_gpsv9_load_mag_settings(); -void aux_hmc5x83_load_mag_settings(); #endif /* UBX_H */ diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 44045840c..0ea7ebbca 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -57,7 +57,7 @@ // timeout for a settings save, in case it has to erase flash? #define UBX_SAVE_WAIT_TIME (1000 * 1000) // max retries in case of timeout -#define UBX_MAX_RETRIES 5 +#define UBX_MAX_RETRIES 10 // max time for ubx parser to respond to MON_VER #define UBX_PARSER_TIMEOUT (950 * 1000) // pause between each unverifiable (no ack/nak) configuration step diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 1f692b6d1..1494af571 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -44,6 +44,8 @@ typedef enum { INIT_STEP_WAIT_MON_VER_ACK, INIT_STEP_RESET_GPS, INIT_STEP_REVO_9600_BAUD, + INIT_STEP_SET_LOWRATE, + INIT_STEP_SET_LOWRATE_WAIT_ACK, INIT_STEP_GPS_BAUD, INIT_STEP_REVO_BAUD, INIT_STEP_ENABLE_SENTENCES, @@ -93,6 +95,7 @@ ubx_cfg_msg_t msg_config_ubx6[] = { { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SBAS, .rate = 0 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEGPS, .rate = 0 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELECEF, .rate = 0 }, + { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 0 }, { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW, .rate = 0 }, { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW2, .rate = 0 }, @@ -104,11 +107,10 @@ ubx_cfg_msg_t msg_config_ubx6[] = { { .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI, .rate = 0 }, - // message to enable + // message to enable, keep SVINFO at end { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .rate = 1 }, - { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 }, @@ -139,12 +141,16 @@ ubx_cfg_msg_t msg_config_ubx7[] = { { .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI, .rate = 0 }, - // message to enable + // message to enable, keep SVINFO at end { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 }, }; +ubx_cfg_msg_t msg_config_ubx_disable_svinfo[] = { + { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 0 }, +}; + // private defines #define LAST_CONFIG_SENT_START (-1) @@ -169,7 +175,8 @@ static volatile bool current_step_touched = false; // both the pointer and what it points to are volatile. Yuk. static volatile status_t *volatile status = 0; static uint8_t hwsettings_baud; -static uint8_t baud_to_try_index = 255; +static uint8_t baud_to_try = 0; +static uint8_t new_gps_speed = 255; // functions @@ -238,14 +245,10 @@ void gps_ubx_reset_sensor_type() // is this needed? // what happens if two tasks / threads try to do an XyzSet() at the same time? if (__sync_fetch_and_add(&mutex, 1) == 0) { - ubxHwVersion = -1; - baud_to_try_index -= 1; // undo postincrement and start with the one that was most recently successful - ubxSensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; + ubxHwVersion = -1; + baud_to_try = new_gps_speed; + ubxSensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; GPSPositionSensorSensorTypeSet(&ubxSensorType); - // make the sensor type / autobaud code time out immediately to send the request immediately - if (status) { - status->lastStepTimestampRaw += 0x8000000UL; - } } --mutex; } @@ -283,14 +286,18 @@ static void config_gps_baud(uint16_t *bytes_to_send) // Ask GPS to change it's speed status->working_packet.message.payload.cfg_prt.baudRate = hwsettings_gpsspeed_enum_to_baud(hwsettings_baud); *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_PRT, sizeof(ubx_cfg_prt_t)); + + // GPS will be found later at this new_gps_speed + new_gps_speed = hwsettings_baud; } -static void config_rate(uint16_t *bytes_to_send) +static void config_rate(uint16_t *bytes_to_send, bool min_rate) { memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t)); // if rate is less than 1 uses the highest rate for current hardware - uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99; + // force rate to 1Hz if min_rate = true + uint16_t rate = (min_rate) ? 1 : (status->currentSettings.navRate > 0) ? status->currentSettings.navRate : 99; if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) { rate = UBX_MAX_RATE; } else if (ubxHwVersion < UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER7) { @@ -304,6 +311,11 @@ static void config_rate(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); + + if (min_rate) { + // used for INIT_STEP_SET_LOWRATE step + status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; + } } @@ -428,11 +440,16 @@ static void config_save(uint16_t *bytes_to_send) static void configure(uint16_t *bytes_to_send) { + // for low baudrates, keep rate to 1Hz and fall to config_nav + // apply UbxRate from GPS settings for higher baudrates + bool min_rate = (new_gps_speed <= HWSETTINGS_GPSSPEED_9600) ? true : false; + switch (status->lastConfigSent) { case LAST_CONFIG_SENT_START: - // increase message rates to 5 fixes per second - config_rate(bytes_to_send); - break; + if (!min_rate) { + config_rate(bytes_to_send, min_rate); + break; + } case LAST_CONFIG_SENT_START + 1: config_nav(bytes_to_send); @@ -446,7 +463,7 @@ static void configure(uint16_t *bytes_to_send) // Skip and fall through to next step status->lastConfigSent++; } - + // fall through case LAST_CONFIG_SENT_START + 3: if (status->currentSettings.enableGLONASS || status->currentSettings.enableGPS) { config_gnss(bytes_to_send); @@ -456,7 +473,7 @@ static void configure(uint16_t *bytes_to_send) status->lastConfigSent++; } // in the else case we must fall through because we must send something each time because successful send is tested externally - + // fall through case LAST_CONFIG_SENT_START + 4: config_sbas(bytes_to_send); break; @@ -468,7 +485,7 @@ static void configure(uint16_t *bytes_to_send) } -static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) +static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send, bool disable_svinfo) { int8_t msg = status->lastConfigSent + 1; uint8_t msg_count = (ubxHwVersion >= UBX_HW_VERSION_7) ? @@ -477,17 +494,23 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) &msg_config_ubx7[0] : &msg_config_ubx6[0]; if (msg >= 0 && msg < msg_count) { - status->working_packet.message.payload.cfg_msg = msg_config[msg]; + // last message is SVINFO, disable if needed + status->working_packet.message.payload.cfg_msg = (msg == (msg_count - 1) && disable_svinfo) ? msg_config_ubx_disable_svinfo[0] : msg_config[msg]; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t)); } else { status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; } + + // Reset GPSSatellites UAVO + if (disable_svinfo && (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) && (GPSSatellitesHandle() != NULL)) { + GPSSatellitesSetDefaults(GPSSatellitesHandle(), 0); + } } #if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) // permanently store our version of GPSSettings.UbxAutoConfig -// we use this to disable after AbConfigStoreAndDisable is complete +// we use this to disable after AutoBaudConfigureStoreAndDisable is complete static void setGpsSettings() { // trying to do this as perfectly as possible we must realize that they may have pressed Send on some fields @@ -526,8 +549,6 @@ static void setGpsSettings() #endif /* if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) */ -// 9600 baud and lower are not usable, and are best left at factory default -// if the user selects 9600 void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) { *bytes_to_send = 0; @@ -548,55 +569,38 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // at low baud rates and high data rates the ubx gps simply must drop some outgoing data // this isn't really an error // and when a lot of data is being dropped, the MON VER reply often gets dropped - // on the other hand, uBlox documents that some versions discard data that is over 1 second old - // implying a 1 second send buffer and that it could be over 1 second before a reply is received - // later uBlox versions dropped this 1 second constraint and drop data when the send buffer is full - // and that could be even longer than 1 second - // send this more quickly and it will get a reply more quickly if a fixed percentage of replies are being dropped + uint32_t time_to_wait = UBX_PARSER_TIMEOUT; + // Add extra time to detect ubxHwVersion in case detection + // becomes more difficult due to a lower transmission rate + if (baud_to_try < HWSETTINGS_GPSSPEED_9600) { + time_to_wait += UBX_PARSER_TIMEOUT; + } // wait for the normal reply timeout before sending it over and over - if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_PARSER_TIMEOUT) { + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < time_to_wait) { return; } - // at this point we have already waited for the MON_VER reply to time out (except the first time where it times out without being sent) + // at this point we have already waited for the MON_VER reply to time out // and the fact we are here says that ubxHwVersion has not been set (it is set externally) // so this try at this baud rate has failed - // if we get here - // select the next baud rate, skipping ahead if new baud rate is HwSettings.GPSSpeed - // set Revo baud rate to current++ value (immediate change so we can send right after that) and send the MON_VER request - // baud rate search order are most likely matches first + // if we get here set Revo baud rate to the previous incremented value and send the MON_VER request // if AutoBaud or higher, do AutoBaud if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUD) { - uint8_t baud_to_try; - static uint8_t baud_array[] = { - HWSETTINGS_GPSSPEED_57600, - HWSETTINGS_GPSSPEED_9600, - HWSETTINGS_GPSSPEED_115200, - HWSETTINGS_GPSSPEED_38400, - HWSETTINGS_GPSSPEED_19200, - HWSETTINGS_GPSSPEED_230400, - HWSETTINGS_GPSSPEED_4800, - HWSETTINGS_GPSSPEED_2400 - }; - // first try HwSettings.GPSSpeed and then - // get the next baud rate to try from the table, but skip over the value of HwSettings.GPSSpeed - do { - // index is inited to be out of bounds, which is interpreted as "currently defined baud rate" (= HwSettings.GPSSpeed) - if (baud_to_try_index >= sizeof(baud_array) / sizeof(baud_array[0])) { - HwSettingsGPSSpeedGet(&hwsettings_baud); - baud_to_try = hwsettings_baud; - baud_to_try_index = 0; - break; - } else { - baud_to_try = baud_array[baud_to_try_index++]; - } - // skip HwSettings.GPSSpeed when you run across it in the list - } while (baud_to_try == hwsettings_baud); + // at startup new_gps_speed is not initialized yet + if (new_gps_speed > HWSETTINGS_GPSSPEED_230400) { + // try current speed set in HwSettings.GPSSpeed + HwSettingsGPSSpeedGet(&hwsettings_baud); + baud_to_try = hwsettings_baud; + new_gps_speed = baud_to_try; + } // set the FC (Revo) baud rate gps_set_fc_baud_from_arg(baud_to_try); + + // increase baud_to_try for next try, in case ubxHwVersion is not set + baud_to_try = (baud_to_try < HWSETTINGS_GPSSPEED_230400) ? baud_to_try + 1 : HWSETTINGS_GPSSPEED_2400; } // this code is executed even if ubxautoconfig is disabled @@ -628,6 +632,7 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // we can do that if we choose because we haven't sent any data in this state // break; + // fall through case INIT_STEP_SEND_MON_VER: build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); // keep timeouts running properly, we (will have) just sent a packet that generates a reply @@ -649,6 +654,7 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // break; // if here, we have just verified that the baud rates are in sync (again) + // fall through case INIT_STEP_RESET_GPS: // make sure we don't change the baud rate too soon and garble the packet being sent // even after pios says the buffer is empty, the serial port buffer still has data in it @@ -686,8 +692,6 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // GPS was just reset, so GPS is running 9600 baud, and Revo is running whatever baud it was before case INIT_STEP_REVO_9600_BAUD: #if !defined(ALWAYS_RESET) - // if user requests a low baud rate then we just reset and leave it set to NMEA - // because low baud and high OP data rate doesn't play nice // if user requests that settings be saved, we will reset here too // that makes sure that all strange settings are reset to factory default // else these strange settings may persist because we don't reset all settings by hand @@ -711,12 +715,30 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // at most, we just set Revo baud and that doesn't send any data // fall through to next state // we can do that if we choose because we haven't sent any data in this state - // set_current_step_if_untouched(INIT_STEP_GPS_BAUD); + // set_current_step_if_untouched(INIT_STEP_SET_LOWRATE); // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); // break; + case INIT_STEP_SET_LOWRATE: + // Here we set minimal baudrate *before* changing gps baudrate + config_rate(bytes_to_send, true); + if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { + status->lastConfigSent = LAST_CONFIG_SENT_START; + status->retryCount = 0; + set_current_step_if_untouched(INIT_STEP_GPS_BAUD); + } else { + set_current_step_if_untouched(INIT_STEP_SET_LOWRATE_WAIT_ACK); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + break; + // Revo and GPS are both at 9600 baud + // fall through case INIT_STEP_GPS_BAUD: + // wait for previous step (INIT_STEP_SET_LOWRATE) + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { + return; + } // https://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf // It is possible to change the current communications port settings using a UBX-CFG-CFG message. This could // affect baud rate and other transmission parameters. Because there may be messages queued for transmission @@ -745,51 +767,48 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) } // set the Revo GPS port baud rate to the (same) user configured value gps_set_fc_baud_from_arg(hwsettings_baud); - status->lastConfigSent = LAST_CONFIG_SENT_START; - // zero the retries for the first "enable sentence" - status->retryCount = 0; - // skip enabling UBX sentences for low baud rates - // low baud rates are not usable, and higher data rates just makes it harder for this code to change the configuration - if (hwsettings_baud <= HWSETTINGS_GPSSPEED_9600) { - set_current_step_if_untouched(INIT_STEP_SAVE); - } else { - set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); - } + // enable UBX sentences + set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); break; case INIT_STEP_ENABLE_SENTENCES: - case INIT_STEP_CONFIGURE: { - bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE); - if (step_configure) { - configure(bytes_to_send); - } else { - enable_sentences(bytes_to_send); - } - - // for some branches, allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + // for low baudrates, disable SVINFO + bool disable_svinfo = (new_gps_speed <= HWSETTINGS_GPSSPEED_9600) ? true : false; + enable_sentences(bytes_to_send, disable_svinfo); if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { - if (step_configure) { - // zero retries for the next state that needs it (INIT_STEP_SAVE) - status->retryCount = 0; - set_current_step_if_untouched(INIT_STEP_SAVE); - } else { - // finished enabling sentences, now configure() needs to start at the beginning - status->lastConfigSent = LAST_CONFIG_SENT_START; - set_current_step_if_untouched(INIT_STEP_CONFIGURE); - } + // finished enabling sentences, now configure() needs to start at the beginning + status->lastConfigSent = LAST_CONFIG_SENT_START; + // zero the retries for the first "configure" + status->retryCount = 0; + set_current_step_if_untouched(INIT_STEP_CONFIGURE); } else { - set_current_step_if_untouched(step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK); + set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES_WAIT_ACK); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); } break; } + case INIT_STEP_CONFIGURE: + { + configure(bytes_to_send); + if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { + status->retryCount = 0; + set_current_step_if_untouched(INIT_STEP_SAVE); + } else { + set_current_step_if_untouched(INIT_STEP_CONFIGURE_WAIT_ACK); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + break; + } + + case INIT_STEP_SET_LOWRATE_WAIT_ACK: case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK: case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS { - bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK); + bool step_setlowrate = (status->currentStep == INIT_STEP_SET_LOWRATE_WAIT_ACK); + bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK); if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) { // Continue with next configuration option // start retries over for the next setting to be sent @@ -811,6 +830,8 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // success or failure here, retries are handled elsewhere if (step_configure) { set_current_step_if_untouched(INIT_STEP_CONFIGURE); + } else if (step_setlowrate) { + set_current_step_if_untouched(INIT_STEP_SET_LOWRATE); } else { set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); } @@ -847,6 +868,7 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // break; // the autoconfig has completed normally + // fall through case INIT_STEP_PRE_DONE: #if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) // determine if we need to disable autoconfig via the autoconfig==AUTOBAUDCONFIGSTOREANDDISABLE setting diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 4e89f10e4..92d5889f5 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -597,6 +597,7 @@ void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettin *position = modeSettings->BatteryFailsafeSwitchPositions.Critical; break; } + // fall through case BATTERYFAILSAFE_WARNING: if (modeSettings->BatteryFailsafeSwitchPositions.Warning != -1) { *position = modeSettings->BatteryFailsafeSwitchPositions.Warning; @@ -646,37 +647,38 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST: { // default to cruise control. - FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL; + typedef FlightModeSettingsStabilization1SettingsOptions _tm; + _tm thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL; switch (flightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - thrustMode = modeSettings->Stabilization1Settings.Thrust; + thrustMode = (_tm)modeSettings->Stabilization1Settings.Thrust; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - thrustMode = modeSettings->Stabilization2Settings.Thrust; + thrustMode = (_tm)modeSettings->Stabilization2Settings.Thrust; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - thrustMode = modeSettings->Stabilization3Settings.Thrust; + thrustMode = (_tm)modeSettings->Stabilization3Settings.Thrust; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: - thrustMode = modeSettings->Stabilization4Settings.Thrust; + thrustMode = (_tm)modeSettings->Stabilization4Settings.Thrust; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: - thrustMode = modeSettings->Stabilization5Settings.Thrust; + thrustMode = (_tm)modeSettings->Stabilization5Settings.Thrust; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: - thrustMode = modeSettings->Stabilization6Settings.Thrust; + thrustMode = (_tm)modeSettings->Stabilization6Settings.Thrust; break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM: // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which // is a more appropriate throttle mode. "GPSAssist" adds braking // and a better throttle management to the standard Position Hold. - thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO; + thrustMode = (_tm)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO; break; case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF: - thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL; + thrustMode = (_tm)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL; break; // other modes will use cruisecontrol as default diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index 65dbdbe05..d302b8f57 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -805,7 +805,7 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode) { // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm - unsigned int steep = abs(y1 - y0) > abs(x1 - x0); + unsigned int steep = abs((int)(y1 - y0)) > abs((int)(x1 - x0)); if (steep) { SWAP(x0, y0); @@ -816,7 +816,7 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1 SWAP(y0, y1); } int deltax = x1 - x0; - unsigned int deltay = abs(y1 - y0); + unsigned int deltay = abs((int)(y1 - y0)); int error = deltax / 2; int ystep; unsigned int y = y0; @@ -884,7 +884,7 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi omode = 1; imode = 0; } - int steep = abs(y1 - y0) > abs(x1 - x0); + int steep = abs((int)(y1 - y0)) > abs((int)(x1 - x0)); if (steep) { SWAP(x0, y0); SWAP(x1, y1); @@ -894,7 +894,7 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi SWAP(y0, y1); } int deltax = x1 - x0; - unsigned int deltay = abs(y1 - y0); + unsigned int deltay = abs((int)(y1 - y0)); int error = deltax / 2; int ystep; unsigned int y = y0; @@ -1583,7 +1583,7 @@ void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size) void printTime(uint16_t x, uint16_t y) { - char temp[9] = + char temp[12] = { 0 }; sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec); diff --git a/flight/modules/PathFollower/inc/fixedwingautotakeoffcontroller.h b/flight/modules/PathFollower/inc/fixedwingautotakeoffcontroller.h index 6930bd7db..e2a6a9b3e 100644 --- a/flight/modules/PathFollower/inc/fixedwingautotakeoffcontroller.h +++ b/flight/modules/PathFollower/inc/fixedwingautotakeoffcontroller.h @@ -51,7 +51,7 @@ public: static FixedWingFlyController *instance() { if (!p_inst) { - p_inst = new FixedWingAutoTakeoffController(); + p_inst = new FixedWingAutoTakeoffController; } return p_inst; } diff --git a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp index d4e6240f5..517435d0a 100644 --- a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp +++ b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp @@ -94,14 +94,14 @@ void VtolAutoTakeoffController::Activate(void) autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED; // We only allow takeoff if the state transition of disarmed to armed occurs // whilst in the autotake flight mode - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); + FlightStatusData _flightStatus; + FlightStatusGet(&_flightStatus); StabilizationDesiredData stabiDesired; StabilizationDesiredGet(&stabiDesired); - if (flightStatus.Armed) { + if (_flightStatus.Armed) { // Are we inflight? - if (stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT || flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + if (stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT || _flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { // ok assume already in flight and just enter position hold // if we are not actually inflight this will just be a violent autotakeoff autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD; @@ -327,9 +327,9 @@ void VtolAutoTakeoffController::UpdateAutoPilot() switch (autotakeoffState) { case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST: { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - if (!flightStatus.Armed) { + FlightStatusData _flightStatus; + FlightStatusGet(&_flightStatus); + if (!_flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED; fsm->setControlState(autotakeoffState); } @@ -337,9 +337,9 @@ void VtolAutoTakeoffController::UpdateAutoPilot() break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED: { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - if (flightStatus.Armed) { + FlightStatusData _flightStatus; + FlightStatusGet(&_flightStatus); + if (_flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE; fsm->setControlState(autotakeoffState); } @@ -360,11 +360,11 @@ void VtolAutoTakeoffController::UpdateAutoPilot() { ManualControlCommandData cmd; ManualControlCommandGet(&cmd); - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); + FlightStatusData _flightStatus; + FlightStatusGet(&_flightStatus); // we do not do a takeoff abort in pathplanner mode - if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE && + if (_flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE && cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT; fsm->setControlState(autotakeoffState); @@ -373,9 +373,9 @@ void VtolAutoTakeoffController::UpdateAutoPilot() break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT: { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - if (!flightStatus.Armed) { + FlightStatusData _flightStatus; + FlightStatusGet(&_flightStatus); + if (!_flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED; fsm->setControlState(autotakeoffState); } diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 833c208c8..ef890d20d 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -260,6 +260,7 @@ static void pathPlannerTask() switch (pathAction.Command) { case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT: endCondition = !endCondition; + // fall through case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: if (endCondition) { setWaypoint(waypointActive.Index + 1); @@ -267,6 +268,7 @@ static void pathPlannerTask() break; case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: endCondition = !endCondition; + // fall through case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT: if (endCondition) { if (pathAction.JumpDestination < 0) { @@ -312,7 +314,7 @@ void updatePathDesired() pathDesired.End.East = waypoint.Position.East; pathDesired.End.Down = waypoint.Position.Down; pathDesired.EndingVelocity = waypoint.Velocity; - pathDesired.Mode = pathAction.Mode; + pathDesired.Mode = (PathDesiredModeOptions)pathAction.Mode; pathDesired.ModeParameters[0] = pathAction.ModeParameters[0]; pathDesired.ModeParameters[1] = pathAction.ModeParameters[1]; pathDesired.ModeParameters[2] = pathAction.ModeParameters[2]; @@ -339,10 +341,23 @@ void updatePathDesired() WaypointData waypointPrev; WaypointInstGet(waypointActive.Index - 1, &waypointPrev); - pathDesired.Start.North = waypointPrev.Position.North; - pathDesired.Start.East = waypointPrev.Position.East; - pathDesired.Start.Down = waypointPrev.Position.Down; - pathDesired.StartingVelocity = waypointPrev.Velocity; + // When exiting CIRCLE, use current UAV position as a start point for PathDesired vector to the next waypoint + // instead of previous waypoint location (that represents the center of the circle) + PathActionData prevAction; + PathActionInstGet(waypointPrev.Action, &prevAction); + if ((prevAction.Mode == PATHACTION_MODE_CIRCLERIGHT) || (prevAction.Mode == PATHACTION_MODE_CIRCLELEFT)) { + PositionStateData positionState; + PositionStateGet(&positionState); + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.StartingVelocity = waypointPrev.Velocity; + } else { + pathDesired.Start.North = waypointPrev.Position.North; + pathDesired.Start.East = waypointPrev.Position.East; + pathDesired.Start.Down = waypointPrev.Position.Down; + pathDesired.StartingVelocity = waypointPrev.Velocity; + } } PathDesiredSet(&pathDesired); @@ -662,7 +677,15 @@ static uint8_t conditionPointingTowardsNext() WaypointData nextWaypoint; WaypointInstGet(nextWaypointId, &nextWaypoint); - float angle1 = atan2f((nextWaypoint.Position.North - waypoint.Position.North), (nextWaypoint.Position.East - waypoint.Position.East)); + PositionStateData positionState; + PositionStateGet(&positionState); + + // check if current position exactly matches nextWaipoint (in 2D space) + if ((fabsf(nextWaypoint.Position.North - positionState.North) < 1e-6f) && (fabsf(nextWaypoint.Position.East - positionState.East) < 1e-6f)) { + return true; + } + + float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East)); VelocityStateData velocity; VelocityStateGet(&velocity); diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 8b5643c5b..903ec2e6d 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -914,6 +914,23 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES static uint8_t isAssistedFlightMode(uint8_t position) { + // Since VelocityRoam is by all means an "assisted" mode, + // here we do explicitly recognize it as "assisted", no matter + // if it has "GPSAssist" set in FlightModeAssistMap or not, thus + // always applying the "Assisted Control stick deadband" when + // VelocityRoam is active. + FlightModeSettingsData modeSettings; + FlightModeSettingsGet(&modeSettings); + + uint8_t thisMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; + if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) { + thisMode = modeSettings.FlightModePosition[position]; + } + + if (thisMode == FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM) { + return STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST; + } + uint8_t isAssistedFlag = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE; uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM]; diff --git a/flight/modules/Sensors/inc/sensors.h b/flight/modules/Sensors/inc/sensors.h index 4676ef8eb..1c98c2732 100644 --- a/flight/modules/Sensors/inc/sensors.h +++ b/flight/modules/Sensors/inc/sensors.h @@ -34,5 +34,6 @@ #include "openpilot.h" int32_t SensorsInitialize(void); +void sensors_auxmag_load_mag_settings(void); #endif // SENSORS_H diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 2d16307e4..13cca640d 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -71,6 +71,7 @@ #include #include #include +#include "sensors.h" // Private constants #define STACK_SIZE_BYTES 1000 @@ -131,10 +132,6 @@ PERF_DEFINE_COUNTER(counterBaroPeriod); PERF_DEFINE_COUNTER(counterSensorPeriod); PERF_DEFINE_COUNTER(counterSensorResets); -#if defined(PIOS_INCLUDE_HMC5X83) -void aux_hmc5x83_load_settings(); -#endif - // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent *objEv); @@ -148,7 +145,7 @@ static void clearContext(sensor_fetch_context *sensor_context); static void handleAccel(float *samples, float temperature); static void handleGyro(float *samples, float temperature, uint32_t timestamp); static void handleMag(float *samples, float temperature); -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) static void handleAuxMag(float *samples); #endif static void handleBaro(float sample, float temperature); @@ -193,7 +190,7 @@ static float baro_temp_bias = 0; static float baro_temperature = NAN; static uint8_t baro_temp_calibration_count = 0; -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) // Allow AuxMag to be disabled without reboot // because the other mags are that way static bool useAuxMag = false; @@ -211,7 +208,7 @@ int32_t SensorsInitialize(void) MagSensorInitialize(); BaroSensorInitialize(); -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) // for auxmagsupport.c helpers AuxMagSensorInitialize(); #endif @@ -388,7 +385,7 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE float inv_count = 1.0f / (float)sensor_context->count; if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL) || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_AUXMAG) #endif ) { @@ -403,7 +400,7 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE PERF_MEASURE_PERIOD(counterMagPeriod); return; -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) case PIOS_SENSORS_TYPE_3AXIS_AUXMAG: handleAuxMag(samples); PERF_MEASURE_PERIOD(counterMagPeriod); @@ -502,7 +499,7 @@ static void handleMag(float *samples, float temperature) MagSensorSet(&mag); } -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) static void handleAuxMag(float *samples) { if (useAuxMag) { @@ -518,7 +515,7 @@ static void handleBaro(float sample, float temperature) float altitude = 44330.0f * (1.0f - powf((sample) / PIOS_CONST_MKS_STD_ATMOSPHERE_F, (1.0f / 5.255f))); - if (!isnan(altitude)) { + if (IS_REAL(altitude)) { BaroSensorData data; data.Altitude = altitude; data.Temperature = temperature; @@ -530,7 +527,7 @@ static void handleBaro(float sample, float temperature) static void updateAccelTempBias(float temperature) { - if (isnan(accel_temperature)) { + if (!IS_REAL(accel_temperature)) { accel_temperature = temperature; } accel_temperature = temp_alpha_gyro_accel * (temperature - accel_temperature) + accel_temperature; @@ -552,7 +549,7 @@ static void updateAccelTempBias(float temperature) static void updateGyroTempBias(float temperature) { - if (isnan(gyro_temperature)) { + if (!IS_REAL(gyro_temperature)) { gyro_temperature = temperature; } @@ -573,7 +570,7 @@ static void updateGyroTempBias(float temperature) static void updateBaroTempBias(float temperature) { - if (isnan(baro_temperature)) { + if (!IS_REAL(baro_temperature)) { baro_temperature = temperature; } @@ -641,8 +638,8 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) fabsf(baroCorrection.d) > 1e-9f)); } -#if defined(PIOS_INCLUDE_HMC5X83) -void aux_hmc5x83_load_mag_settings() +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) +void sensors_auxmag_load_mag_settings() { uint8_t magType = auxmagsupport_get_type(); diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 58228728c..dfac06cde 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -309,6 +309,7 @@ static void stabilizationInnerloopTask() } // IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication! // keep order as it is, RATE must follow! + // fall through case STABILIZATIONSTATUS_INNERLOOP_RATE: { // limit rate to maximum configured limits (once here instead of 5 times in outer loop) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 0e67fb99a..61482893e 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -259,16 +259,10 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float // During initialization and if (this->first_run) { -#if defined(PIOS_INCLUDE_HMC5X83) // wait until mags have been updated if (!this->magUpdated && this->useMag) { return FILTERRESULT_ERROR; } -#else - mag[0] = 100.0f; - mag[1] = 0.0f; - mag[2] = 0.0f; -#endif pseudo_windowed_variance_init(&this->gyro_var[0], VARIANCE_WINDOW_SIZE); pseudo_windowed_variance_init(&this->gyro_var[1], VARIANCE_WINDOW_SIZE); @@ -535,7 +529,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN - if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) { + if ((fabsf(inv_qmag) > 1e3f) || !IS_REAL(inv_qmag)) { this->first_run = 1; return FILTERRESULT_WARNING; } diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 92f3f7364..08f0ef32a 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -481,7 +481,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // check for invalid variance values static inline bool invalid_var(float data) { - if (isnan(data) || isinf(data)) { + if (!IS_REAL(data)) { return true; } if (data < 1e-15f) { // var should not be close to zero. And not negative either. diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 299af3d30..5fe73d20e 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -282,9 +282,9 @@ void magOffsetEstimation(struct data *this, float mag[3]) delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); delta[2] = -rate * (Rz - B_e[2]); - if (!isnan(delta[0]) && !isinf(delta[0]) && - !isnan(delta[1]) && !isinf(delta[1]) && - !isnan(delta[2]) && !isinf(delta[2])) { + if (IS_REAL(delta[0]) && + IS_REAL(delta[1]) && + IS_REAL(delta[2])) { this->magBias[0] += delta[0]; this->magBias[1] += delta[1]; this->magBias[2] += delta[2]; diff --git a/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c b/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c new file mode 100644 index 000000000..84c615a4f --- /dev/null +++ b/flight/modules/UAVOFrSKYSPortBridge/UAVOFrSKYSPortBridge.c @@ -0,0 +1,357 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup UAVOFrSKYSPortBridge UAVO to FrSKY S.PORT Bridge Module + * @{ + * + * @file uavofrskysportbridge.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017-2019 + * Tau Labs, http://taulabs.org, Copyright (C) 2014 + * @brief Bridges selected UAVObjects to FrSKY Smart Port bus + * + * Since there is no public documentation of SmartPort protocol available, + * this was put together by studying OpenTx source code, reading + * tidbits of informations on the Internet and experimenting. + * @see https://github.com/opentx/opentx/tree/next/radio/src/telemetry + * @see https://code.google.com/p/telemetry-convert/wiki/FrSkySPortProtocol + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "frsky_packing.h" + +#include "pios_board_io.h" + +#include "barosensor.h" +#include "flightbatterysettings.h" +#include "flightbatterystate.h" +#include "gpspositionsensor.h" +#include "frskysporttelemetrysettings.h" +#include "hwsettings.h" +#include "taskinfo.h" + + +#define FRSKY_POLL_REQUEST 0x7e +#define FRSKY_MINIMUM_POLL_INTERVAL 10000 + +enum frsky_state { + FRSKY_STATE_WAIT_POLL_REQUEST, + FRSKY_STATE_WAIT_SENSOR_ID, + FRSKY_STATE_WAIT_TX_DONE, +}; + +// Set of objects sent by this module +static const struct frsky_value_item frsky_value_items[] = { + { FRSKY_GPS_COURSE_ID, 100, frsky_encode_gps_course, 0 }, // attitude yaw estimate + { FRSKY_ALT_ID, 100, frsky_encode_altitude, 0 }, // altitude estimate + { FRSKY_VARIO_ID, 100, frsky_encode_vario, 0 }, // vertical speed + { FRSKY_CURR_ID, 300, frsky_encode_current, 0 }, // battery current + { FRSKY_CELLS_ID, 850, frsky_encode_cells, 0 }, // battery cells 1-2 + { FRSKY_CELLS_ID, 850, frsky_encode_cells, 1 }, // battery cells 3-4 + { FRSKY_CELLS_ID, 850, frsky_encode_cells, 2 }, // battery cells 5-6 + { FRSKY_CELLS_ID, 850, frsky_encode_cells, 3 }, // battery cells 7-8 + { FRSKY_CELLS_ID, 850, frsky_encode_cells, 4 }, // battery cells 9-10 + { FRSKY_CELLS_ID, 850, frsky_encode_cells, 5 }, // battery cells 11-12 + { FRSKY_T1_ID, 2000, frsky_encode_t1, 0 }, // baro temperature + { FRSKY_T2_ID, 1500, frsky_encode_t2, 0 }, // encodes GPS status! + { FRSKY_FUEL_ID, 600, frsky_encode_fuel, 0 }, // consumed battery energy + { FRSKY_ACCX_ID, 100, frsky_encode_acc, 0 }, // accX + { FRSKY_ACCY_ID, 100, frsky_encode_acc, 1 }, // accY + { FRSKY_ACCZ_ID, 100, frsky_encode_acc, 2 }, // accZ + { FRSKY_GPS_LON_LAT_ID, 100, frsky_encode_gps_coord, 0 }, // lattitude + { FRSKY_GPS_LON_LAT_ID, 100, frsky_encode_gps_coord, 1 }, // longitude + { FRSKY_GPS_ALT_ID, 750, frsky_encode_gps_alt, 0 }, // gps altitude + { FRSKY_GPS_SPEED_ID, 200, frsky_encode_gps_speed, 0 }, // gps speed + { FRSKY_GPS_TIME_ID, 8000, frsky_encode_gps_time, 0 }, // gps date + { FRSKY_GPS_TIME_ID, 2000, frsky_encode_gps_time, 1 }, // gps time + { FRSKY_RPM_ID, 1500, frsky_encode_rpm, 0 }, // encodes flight status! + { FRSKY_AIR_SPEED_ID, 100, frsky_encode_airspeed, 0 }, // airspeed +}; + +struct frsky_sport_telemetry { + enum frsky_state state; + int32_t scheduled_item; + uint32_t last_poll_time; + uintptr_t com; + bool ignore_echo; + uint8_t ignore_rx_chars; + struct frsky_settings frsky_settings; + uint16_t schedule_nr; + uint32_t item_last_triggered[NELEMENTS(frsky_value_items)]; + uint16_t item_schedule_nr[NELEMENTS(frsky_value_items)]; +}; + +static const uint8_t frsky_sensor_ids[] = { 0x1b }; + +#define FRSKY_SPORT_BAUDRATE 57600 + +#if defined(PIOS_FRSKY_SPORT_TELEMETRY_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_FRSKY_SPORT_TELEMETRY_STACK_SIZE +#else +#define STACK_SIZE_BYTES 672 +#endif +#define TASK_PRIORITY (tskIDLE_PRIORITY) + +static struct frsky_sport_telemetry *frsky = 0; +static int32_t uavoFrSKYSPortBridgeInitialize(void); +static void uavoFrSKYSPortBridgeTask(void *parameters); + +/** + * Scan for value item with the longest expired time and schedule it to send in next poll turn + * + */ + +static void frsky_schedule_next_item(void) +{ + frsky->scheduled_item = -1; + + for (uint32_t i = 0; i < NELEMENTS(frsky_value_items); i++) { + if (frsky_value_items[i].encode_value(&frsky->frsky_settings, 0, true, frsky_value_items[i].fn_arg)) { + if (frsky->item_schedule_nr[i] == frsky->schedule_nr) { + continue; + } + if (PIOS_DELAY_GetuSSince(frsky->item_last_triggered[i]) > (frsky_value_items[i].period_ms * 1000)) { + frsky->scheduled_item = i; + frsky->item_schedule_nr[i] = frsky->schedule_nr; + break; + } + } + } + + if (frsky->scheduled_item < 0) { + frsky->schedule_nr++; + } +} + + +/** + * Send value item previously scheduled by frsky_schedule_next_itme() + * @returns true when item value was sended + */ +static bool frsky_send_scheduled_item(void) +{ + int32_t item = frsky->scheduled_item; + + if ((item >= 0) && (item < (int32_t)NELEMENTS(frsky_value_items))) { + frsky->item_last_triggered[item] = PIOS_DELAY_GetuS(); + uint32_t value = 0; + if (frsky_value_items[item].encode_value(&frsky->frsky_settings, &value, false, + frsky_value_items[item].fn_arg)) { + frsky->ignore_rx_chars += frsky_send_frame(frsky->com, (uint16_t)(frsky_value_items[item].id), value, false); + + return true; + } + } + + return false; +} + +/** + * Process incoming bytes from FrSky S.PORT bus + * @param[in] b received byte + */ +static void frsky_receive_byte(uint8_t b) +{ + uint32_t i = 0; + + switch (frsky->state) { + case FRSKY_STATE_WAIT_TX_DONE: + // because RX and TX are connected, we need to ignore bytes + // transmited by us + if (--frsky->ignore_rx_chars == 0) { + frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST; + } + break; + + case FRSKY_STATE_WAIT_POLL_REQUEST: + if (b == FRSKY_POLL_REQUEST) { + // X8R is polling us every 13ms + if (PIOS_DELAY_GetuSSince(frsky->last_poll_time) > FRSKY_MINIMUM_POLL_INTERVAL) { + frsky->last_poll_time = PIOS_DELAY_GetuS(); + frsky->state = FRSKY_STATE_WAIT_SENSOR_ID; + } + } + break; + + case FRSKY_STATE_WAIT_SENSOR_ID: + + frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST; + + for (i = 0; i < sizeof(frsky_sensor_ids); i++) { + if (frsky_sensor_ids[i] == b) { + // get GPSPositionData once here to be more efficient, since + // GPS position data are very often used by encode() handlers + if (GPSPositionSensorHandle() != NULL) { + GPSPositionSensorGet(&frsky->frsky_settings.gps_position); + } + // send item previously scheduled + frsky_send_scheduled_item(); + + if (frsky->ignore_echo && frsky->ignore_rx_chars) { + frsky->state = FRSKY_STATE_WAIT_TX_DONE; + } + + // schedule item for next poll turn + frsky_schedule_next_item(); + break; + } + } + break; + } +} + +/** + * Module start routine automatically called after initialization routine + * @return 0 when was successful + */ +static int32_t uavoFrSKYSPortBridgeStart(void) +{ + if (!frsky) { + return -1; + } + + if (FlightBatterySettingsHandle() != NULL + && FlightBatteryStateHandle() != NULL) { +// TODO: maybe get this setting from somewhere else? +// uint8_t currentPin; +// FlightBatterySettingsCurrentPinGet(¤tPin); +// if (currentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE) + frsky->frsky_settings.use_current_sensor = true; + FlightBatterySettingsGet(&frsky->frsky_settings.battery_settings); + frsky->frsky_settings.batt_cell_count = frsky->frsky_settings.battery_settings.NbCells; + } +// This is just to check if barometer is enabled. +// if (BaroSensorHandle() != NULL +// && PIOS_SENSORS_GetQueue(PIOS_SENSOR_BARO) != NULL) + frsky->frsky_settings.use_baro_sensor = true; + + + xTaskHandle task; + xTaskCreate(uavoFrSKYSPortBridgeTask, "FrSky SPort Telemetry", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &task); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FRSKYSPORTTELEMETRY, task); + + return 0; +} + +static void FrSKYSPortTelemetrySettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + FrSKYSPortTelemetrySettingsData settings; + + FrSKYSPortTelemetrySettingsGet(&settings); +} + + +/** + * Module initialization routine + * @return 0 when initialization was successful + */ +static int32_t uavoFrSKYSPortBridgeInitialize(void) +{ + FrSKYSPortTelemetrySettingsInitialize(); + + if (PIOS_COM_FRSKY_SPORT) { + frsky = pios_malloc(sizeof(struct frsky_sport_telemetry)); + + if (frsky != NULL) { + memset(frsky, 0x00, sizeof(struct frsky_sport_telemetry)); + + frsky->frsky_settings.use_current_sensor = false; + frsky->frsky_settings.batt_cell_count = 0; + frsky->frsky_settings.use_baro_sensor = false; + frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST; + frsky->last_poll_time = PIOS_DELAY_GetuS(); + frsky->scheduled_item = -1; + frsky->com = PIOS_COM_FRSKY_SPORT; + frsky->ignore_echo = true; // This has to be true when RX & TX hw serial lines are connected. Otherwise false. (enforced below by setting half duplex. this makes internal connection between rx and tx + // connect TX pin of flight controller to UNINVERTED SPort + // (F4 based controllers do not have TX inversion capability. + // Use external inverter or solder to uninverted receiver pin) + // TODO: Add code/PIOS driver to enable inversion for F3 based convertes + // TODO: implement FPORT driver + frsky->schedule_nr = 1; + + uint8_t i; + for (i = 0; i < NELEMENTS(frsky_value_items); i++) { + frsky->item_last_triggered[i] = PIOS_DELAY_GetuS(); + } + + // Set Port options: + // BAUD rate + PIOS_COM_ChangeBaud(frsky->com, FRSKY_SPORT_BAUDRATE); + + HwSettingsSPortModeOptions options; + HwSettingsSPortModeGet(&options); + bool halfduplex; + enum PIOS_USART_Inverted inverted; + switch (options) { + case HWSETTINGS_SPORTMODE_HALFDUPLEXNONINVERTED: + halfduplex = true; + inverted = PIOS_USART_Inverted_None; + break; + case HWSETTINGS_SPORTMODE_HALFDUPLEXINVERTED: + halfduplex = true; + inverted = PIOS_USART_Inverted_RxTx; + break; + case HWSETTINGS_SPORTMODE_FULLDUPLEXNONINVERTED: + halfduplex = false; + inverted = PIOS_USART_Inverted_None; + break; + case HWSETTINGS_SPORTMODE_FULLDUPLEXINVERTED: + halfduplex = false; + inverted = PIOS_USART_Inverted_RxTx; + break; + } + + // Port Inversion (Not available on STM32F4, will have no effect) + PIOS_COM_Ioctl(frsky->com, PIOS_IOCTL_USART_SET_INVERTED, &inverted); + + // HalfDplex mode (Not available on STM32F0, will have no effect) + PIOS_COM_Ioctl(frsky->com, PIOS_IOCTL_USART_SET_HALFDUPLEX, &halfduplex); + + FrSKYSPortTelemetrySettingsConnectCallback(FrSKYSPortTelemetrySettingsUpdatedCb); + + FrSKYSPortTelemetrySettingsUpdatedCb(0); + + return 0; + } + } + + return -1; +} +MODULE_INITCALL(uavoFrSKYSPortBridgeInitialize, uavoFrSKYSPortBridgeStart); + +/** + * Main task routine + * @param[in] parameters parameter given by PIOS_Thread_Create() + */ +static void uavoFrSKYSPortBridgeTask(__attribute__((unused)) void *parameters) +{ + while (1) { + uint8_t b = 0; + uint16_t count = PIOS_COM_ReceiveBuffer(frsky->com, &b, 1, 0xffffffff); + if (count) { + frsky_receive_byte(b); + } + } +} + +/** + * @} + * @} + */ diff --git a/flight/modules/UAVOFrSKYSensorHubBridge/UAVOFrSKYSensorHubBridge.c b/flight/modules/UAVOFrSKYSensorHubBridge/UAVOFrSKYSensorHubBridge.c index 964697d8c..3b1e37c33 100644 --- a/flight/modules/UAVOFrSKYSensorHubBridge/UAVOFrSKYSensorHubBridge.c +++ b/flight/modules/UAVOFrSKYSensorHubBridge/UAVOFrSKYSensorHubBridge.c @@ -610,7 +610,7 @@ static uint16_t frsky_pack_fuel( { uint8_t index = 0; - uint16_t level = lroundf(abs(fuel_level * 100)); + uint16_t level = lroundf(fabsf(fuel_level * 100)); // Use fixed levels here because documentation says so. if (level > 94) { diff --git a/flight/modules/UAVOHottBridge/uavohottbridge.c b/flight/modules/UAVOHottBridge/uavohottbridge.c index 32641e48f..e8c31608f 100644 --- a/flight/modules/UAVOHottBridge/uavohottbridge.c +++ b/flight/modules/UAVOHottBridge/uavohottbridge.c @@ -1944,7 +1944,7 @@ void update_telemetrydata() // use climbrate pointer for alternate display, without GPS every 5s or 3.3s with speed/dist msg uint8_t statusmsg_num = (telestate->Settings.Sensor.GPS == HOTTBRIDGESETTINGS_SENSOR_ENABLED) ? 3 : 2; if (telestate->climbrate_pointer < (climbratesize / statusmsg_num)) { - snprintf(telestate->statusline, sizeof(telestate->statusline), "%12s,%8s", txt_flightmode, txt_armstate); + snprintf(telestate->statusline, sizeof(telestate->statusline), "%12s,%7s", txt_flightmode, txt_armstate); } else if (telestate->climbrate_pointer < (climbratesize / statusmsg_num) * 2) { snprintf(telestate->statusline, sizeof(telestate->statusline), "MaxG %2d.%d MinG %2d.%d", (int8_t)(telestate->max_G), (int8_t)(telestate->max_G * 10) % 10, diff --git a/flight/pios/common/libraries/CMSIS/Include/core_cmFunc.h b/flight/pios/common/libraries/CMSIS/Include/core_cmFunc.h index 0a18fafc3..595f671ff 100644 --- a/flight/pios/common/libraries/CMSIS/Include/core_cmFunc.h +++ b/flight/pios/common/libraries/CMSIS/Include/core_cmFunc.h @@ -438,7 +438,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void) */ __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack) { - __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp"); + __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : ); } @@ -465,7 +465,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void) */ __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack) { - __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp"); + __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : ); } diff --git a/flight/pios/common/pios_adxl345.c b/flight/pios/common/pios_adxl345.c index a31034e81..6f454ad6e 100644 --- a/flight/pios/common/pios_adxl345.c +++ b/flight/pios/common/pios_adxl345.c @@ -355,7 +355,7 @@ int8_t PIOS_ADXL345_ReadAndAccumulateSamples(struct pios_adxl345_data *data, uin PIOS_Instrumentation_TimeStart(counterUpd); #endif for (uint8_t i = 0; i < samples; i++) { - uint8_t buf[7] = { 0 }; + uint8_t buf[8] = { 0 }; uint8_t *rec = &buf[1]; PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT; // Multibyte read starting at X0 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/common/pios_board_sensors.c b/flight/pios/common/pios_board_sensors.c index 1357c1798..43c7226e9 100644 --- a/flight/pios/common/pios_board_sensors.c +++ b/flight/pios/common/pios_board_sensors.c @@ -60,6 +60,10 @@ pios_hmc5x83_dev_t pios_hmc5x83_internal_id; # endif #endif +#ifdef PIOS_INCLUDE_QMC5883L +# include "pios_qmc5883l.h" +#endif + #ifdef PIOS_INCLUDE_L3GD20 # include "pios_l3gd20.h" #endif @@ -146,35 +150,36 @@ void PIOS_BOARD_Sensors_Configure() # endif /* PIOS_INCLUDE_HMC5X83_INTERNAL */ -# ifdef PIOS_INCLUDE_HMC5X83 +# ifdef PIOS_INCLUDE_SENSORS_AUXMAG + AuxMagSettingsInitialize(); AuxMagSettingsTypeOptions option; AuxMagSettingsTypeGet(&option); - const struct pios_hmc5x83_cfg *hmc5x83_external_cfg = PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(pios_board_info_blob.board_rev); + uint32_t auxmag_i2c_id = 0; - if (hmc5x83_external_cfg) { - uint32_t i2c_id = 0; - - if (option == AUXMAGSETTINGS_TYPE_FLEXI) { - // i2c_external + if (option == AUXMAGSETTINGS_TYPE_FLEXI) { + // i2c_external #ifdef PIOS_I2C_FLEXI_ADAPTER - i2c_id = PIOS_I2C_FLEXI_ADAPTER; + auxmag_i2c_id = PIOS_I2C_FLEXI_ADAPTER; #endif - } else if (option == AUXMAGSETTINGS_TYPE_I2C) { - // i2c_internal (or Sparky2/F3 dedicated I2C port) + } else if (option == AUXMAGSETTINGS_TYPE_I2C) { + // i2c_internal (or Sparky2/F3 dedicated I2C port) #ifdef PIOS_I2C_EXTERNAL_ADAPTER - i2c_id = PIOS_I2C_EXTERNAL_ADAPTER; + auxmag_i2c_id = PIOS_I2C_EXTERNAL_ADAPTER; #endif - } + } - if (i2c_id) { - uint32_t external_mag = PIOS_HMC5x83_Init(hmc5x83_external_cfg, i2c_id, 0); -# ifdef PIOS_INCLUDE_WDG + if (auxmag_i2c_id) { +# ifdef PIOS_INCLUDE_HMC5X83 + const struct pios_hmc5x83_cfg *hmc5x83_external_cfg = PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(pios_board_info_blob.board_rev); + if (hmc5x83_external_cfg) { + uint32_t external_mag = PIOS_HMC5x83_Init(hmc5x83_external_cfg, auxmag_i2c_id, 0); +# ifdef PIOS_INCLUDE_WDG // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed // this is not in a loop, so it is safe PIOS_WDG_Clear(); -# endif /* PIOS_INCLUDE_WDG */ +# endif /* PIOS_INCLUDE_WDG */ // add this sensor to the sensor task's list // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor // and before registering some other fast and important sensor @@ -183,8 +188,16 @@ void PIOS_BOARD_Sensors_Configure() // mag alarm is cleared later, so use I2C AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); } +# endif /* PIOS_INCLUDE_HMC5X83 */ +# ifdef PIOS_INCLUDE_QMC5883L + pios_qmc5883l_dev_t qmc5883l_dev = PIOS_QMC5883L_Init(auxmag_i2c_id); +# ifdef PIOS_INCLUDE_WDG + PIOS_WDG_Clear(); +# endif /* PIOS_INCLUDE_WDG */ + AlarmsSet(SYSTEMALARMS_ALARM_I2C, (qmc5883l_dev) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); +# endif /* PIOS_INCLUDE_QMC5883L */ } -# endif /* PIOS_INCLUDE_HMC5X83 */ +# endif /* PIOS_INCLUDE_SENSORS_AUXMAG */ // internal ms5611 baro #ifdef PIOS_INCLUDE_MS56XX diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index 6bef10454..97ed79ff8 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -295,6 +295,32 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) return 0; } +void PIOS_COM_ChangeBaud_VoidWrapper(uint32_t com_id, uint32_t baud) +{ + PIOS_COM_ChangeBaud(com_id, baud); +} + + +/** + * Clear Rx buffer + * \param[in] port COM port + * \return -1 if port not available + * \return 0 on success + */ +int32_t PIOS_COM_ClearRxBuffer(uint32_t com_id) +{ + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } + + fifoBuf_clearData(&com_dev->rx); + + return 0; +} + int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate) { @@ -337,6 +363,10 @@ int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state) return 0; } +void PIOS_COM_SetCtrlLine_VoidWrapper(uint32_t com_id, uint32_t mask, uint32_t state) +{ + PIOS_COM_SetCtrlLine(com_id, mask, state); +} /** * Set control lines associated with the port @@ -827,12 +857,12 @@ void PIOS_COM_LinkComPair(uint32_t com1_id, uint32_t com2_id, bool link_ctrl_lin PIOS_COM_ASYNC_RegisterRxCallback(com2_id, PIOS_COM_LinkComPairRxCallback, com1_id); // Optionally link the control like and baudrate changes between the two. if (link_ctrl_line) { - PIOS_COM_RegisterCtrlLineCallback(com1_id, (pios_com_callback_ctrl_line)PIOS_COM_SetCtrlLine, com2_id); - PIOS_COM_RegisterCtrlLineCallback(com2_id, (pios_com_callback_ctrl_line)PIOS_COM_SetCtrlLine, com1_id); + PIOS_COM_RegisterCtrlLineCallback(com1_id, (pios_com_callback_ctrl_line)PIOS_COM_SetCtrlLine_VoidWrapper, com2_id); + PIOS_COM_RegisterCtrlLineCallback(com2_id, (pios_com_callback_ctrl_line)PIOS_COM_SetCtrlLine_VoidWrapper, com1_id); } if (link_baud_rate) { - PIOS_COM_RegisterBaudRateCallback(com1_id, (pios_com_callback_baud_rate)PIOS_COM_ChangeBaud, com2_id); - PIOS_COM_RegisterBaudRateCallback(com2_id, (pios_com_callback_baud_rate)PIOS_COM_ChangeBaud, com1_id); + PIOS_COM_RegisterBaudRateCallback(com1_id, (pios_com_callback_baud_rate)PIOS_COM_ChangeBaud_VoidWrapper, com2_id); + PIOS_COM_RegisterBaudRateCallback(com2_id, (pios_com_callback_baud_rate)PIOS_COM_ChangeBaud_VoidWrapper, com1_id); } } diff --git a/flight/pios/common/pios_ms56xx.c b/flight/pios/common/pios_ms56xx.c index bf0237b12..c2c7b604f 100644 --- a/flight/pios/common/pios_ms56xx.c +++ b/flight/pios/common/pios_ms56xx.c @@ -474,7 +474,7 @@ bool PIOS_MS56xx_driver_poll(__attribute__((unused)) uintptr_t context) case MS56XX_FSM_CALIBRATION: PIOS_MS56xx_ReadCalibrationData(); /* fall through to MS56XX_FSM_TEMPERATURE */ - + // fall through case MS56XX_FSM_TEMPERATURE: PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_TemperatureConv); next_state = MS56XX_FSM_PRESSURE; diff --git a/flight/pios/common/pios_qmc5883l.c b/flight/pios/common/pios_qmc5883l.c new file mode 100644 index 000000000..5f871d58b --- /dev/null +++ b/flight/pios/common/pios_qmc5883l.c @@ -0,0 +1,319 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_QMC5883L QMC5883L Functions + * @brief Deals with the hardware interface to the QMC5883L magnetometer + * @{ + * + * @file pios_qmc5883l.c + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (C) 2018 + * @brief QMC5883L functions implementation. + * @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 "pios.h" +#ifdef PIOS_INCLUDE_QMC5883L + +#include +#include "pios_qmc5883l.h" + +#define PIOS_QMC5883L_I2C_ADDR 0x0D +#define PIOS_QMC5883L_CHIP_ID 0xFF + +#define PIOS_QMC5883L_REG_DATA 0x00 +#define PIOS_QMC5883L_REG_STATUS 0x06 +#define PIOS_QMC5883L_REG_TEMPL 0x07 +#define PIOS_QMC5883L_REG_TEMPH 0x08 +#define PIOS_QMC5883L_REG_CTRL1 0x09 +#define PIOS_QMC5883L_REG_CTRL2 0x0A +#define PIOS_QMC5883L_REG_FBR 0x0B +#define PIOS_QMC5883L_REG_CHIP_ID 0x0D + + +#define PIOS_QMC5883L_CTRL1_ODR_10HZ (0 << 2) +#define PIOS_QMC5883L_CTRL1_ODR_50HZ (1 << 2) +#define PIOS_QMC5883L_CTRL1_ODR_100HZ (2 << 2) +#define PIOS_QMC5883L_CTRL1_ODR_200HZ (3 << 2) + +#define PIOS_QMC5883L_CTRL1_OSR_512 (0 << 6) +#define PIOS_QMC5883L_CTRL1_OSR_256 (1 << 6) +#define PIOS_QMC5883L_CTRL1_OSR_128 (2 << 6) +#define PIOS_QMC5883L_CTRL1_OSR_64 (3 << 6) + +#define PIOS_QMC5883L_CTRL1_RNG_2G (0 << 4) +#define PIOS_QMC5883L_CTRL1_RNG_8G (1 << 4) + +#define PIOS_QMC5883L_CTRL1_MODE_STANDBY (0 << 0) +#define PIOS_QMC5883L_CTRL1_MODE_CONTINUOUS (1 << 0) + +#define PIOS_QMC5883L_CTRL2_INT_ENB (1 << 0) +#define PIOS_QMC5883L_CTRL2_ROL_PNT (1 << 6) +#define PIOS_QMC5883L_CTRL2_SOFT_RST (1 << 7) + +#define PIOS_QMC5883L_FBR_RECOMMENDED 0x01 + +#define PIOS_QMC5883L_STATUS_DOR (1 << 2) +#define PIOS_QMC5883L_STATUS_OVL (1 << 1) +#define PIOS_QMC5883L_STATUS_DRDY (1 << 0) + + +#define PIOS_QMC5883L_I2C_CONFIG_RETRY_DELAY 1000000 +#define PIOS_QMC5883L_I2C_CONFIG_DATA_DELAY 10000 +#define PIOS_QMC5883L_I2C_CONFIG_DATA_TIMEOUT (PIOS_QMC5883L_I2C_CONFIG_DATA_DELAY * 10) +#define PIOS_QMC5883L_I2C_RETRIES 2 + +enum pios_qmc5883l_dev_magic { + PIOS_QMC5883L_MAGIC = 0xF8D3262A +}; + +struct pios_qmc5883l_dev { + enum pios_qmc5883l_dev_magic magic; + uint32_t i2c_id; + + bool sensorIsAlive; + uint32_t configTime; + uint32_t readTime; + + int16_t data_X, data_Y, data_Z; +}; + + +static int32_t PIOS_QMC5883L_Read(uintptr_t i2c_id, uint8_t address, uint8_t *buffer, uint8_t len); +static int32_t PIOS_QMC5883L_Write(uintptr_t i2c_id, uint8_t address, uint8_t buffer); +static int32_t PIOS_QMC5883L_Configure(struct pios_qmc5883l_dev *dev); + +// sensor driver interface +static bool PIOS_QMC5883L_driver_Test(uintptr_t context); +static void PIOS_QMC5883L_driver_Reset(uintptr_t context); +static void PIOS_QMC5883L_driver_get_scale(float *scales, uint8_t size, uintptr_t context); +static void PIOS_QMC5883L_driver_fetch(void *, uint8_t size, uintptr_t context); +static bool PIOS_QMC5883L_driver_poll(uintptr_t context); + +const PIOS_SENSORS_Driver PIOS_QMC5883L_Driver = { + .test = PIOS_QMC5883L_driver_Test, + .poll = PIOS_QMC5883L_driver_poll, + .fetch = PIOS_QMC5883L_driver_fetch, + .reset = PIOS_QMC5883L_driver_Reset, + .get_queue = NULL, + .get_scale = PIOS_QMC5883L_driver_get_scale, + .is_polled = true, +}; + +static bool PIOS_QMC5883L_Validate(struct pios_qmc5883l_dev *dev) +{ + return dev && (dev->magic == PIOS_QMC5883L_MAGIC); +} + +pios_qmc5883l_dev_t PIOS_QMC5883L_Init(uint32_t i2c_device) +{ + struct pios_qmc5883l_dev *dev = (struct pios_qmc5883l_dev *)pios_malloc(sizeof(*dev)); + + memset(dev, 0, sizeof(*dev)); + + dev->i2c_id = i2c_device; + dev->magic = PIOS_QMC5883L_MAGIC; + + PIOS_SENSORS_Register(&PIOS_QMC5883L_Driver, PIOS_SENSORS_TYPE_3AXIS_AUXMAG, (uintptr_t)dev); + + return dev; +} + +static int32_t PIOS_QMC5883L_Configure(struct pios_qmc5883l_dev *dev) +{ + // read chip id? + uint8_t chip_id; + + if (dev->sensorIsAlive) { + return 0; + } + + if (PIOS_DELAY_DiffuS(dev->configTime) < PIOS_QMC5883L_I2C_CONFIG_RETRY_DELAY) { // Do not reinitialize too often + return -1; + } + + dev->configTime = PIOS_DELAY_GetRaw(); + // Reset chip + dev->sensorIsAlive = (PIOS_QMC5883L_Write(dev->i2c_id, PIOS_QMC5883L_REG_CTRL2, PIOS_QMC5883L_CTRL2_SOFT_RST) == 0); + if (!dev->sensorIsAlive) { + return -1; + } + + // Read ID + dev->sensorIsAlive = (PIOS_QMC5883L_Read(dev->i2c_id, PIOS_QMC5883L_REG_CHIP_ID, &chip_id, sizeof(chip_id)) == 0); + if (!dev->sensorIsAlive) { + return -1; + } + + if (chip_id != PIOS_QMC5883L_CHIP_ID) { + return -2; + } + + // Set FBR + dev->sensorIsAlive = (PIOS_QMC5883L_Write(dev->i2c_id, PIOS_QMC5883L_REG_FBR, PIOS_QMC5883L_FBR_RECOMMENDED) == 0); + if (!dev->sensorIsAlive) { + return -1; + } + + // Set control registers + dev->sensorIsAlive = (PIOS_QMC5883L_Write(dev->i2c_id, PIOS_QMC5883L_REG_CTRL1, PIOS_QMC5883L_CTRL1_MODE_CONTINUOUS | PIOS_QMC5883L_CTRL1_ODR_200HZ | PIOS_QMC5883L_CTRL1_OSR_512 | PIOS_QMC5883L_CTRL1_RNG_8G) == 0); + if (!dev->sensorIsAlive) { + return -1; + } + + return 0; +} + +/** + * Reads one or more bytes into a buffer + * \param[in] the command indicating the address to read + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + */ +static int32_t PIOS_QMC5883L_Read(uintptr_t i2c_id, uint8_t address, uint8_t *buffer, uint8_t len) +{ + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_QMC5883L_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = 1, + .buf = &address, + } + , + { + .info = __func__, + .addr = PIOS_QMC5883L_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + for (uint8_t retry = PIOS_QMC5883L_I2C_RETRIES; retry > 0; --retry) { + if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) { + return 0; + } + } + + return -1; +} + +static int32_t PIOS_QMC5883L_Write(uintptr_t i2c_id, uint8_t address, uint8_t value) +{ + uint8_t data[] = { address, value }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_QMC5883L_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + }; + + for (uint8_t retry = PIOS_QMC5883L_I2C_RETRIES; retry > 0; --retry) { + if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) { + return 0; + } + } + + return -1; +} + +bool PIOS_QMC5883L_driver_Test(__attribute__((unused)) uintptr_t context) +{ + return true; +} + +static void PIOS_QMC5883L_driver_Reset(__attribute__((unused)) uintptr_t context) +{} + +static void PIOS_QMC5883L_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context) +{ + PIOS_Assert(size > 0); + scales[0] = 0.35f; +} + +static void PIOS_QMC5883L_driver_fetch(void *data, __attribute__((unused)) uint8_t size, uintptr_t context) +{ + struct pios_qmc5883l_dev *dev = (struct pios_qmc5883l_dev *)context; + + PIOS_Assert(PIOS_QMC5883L_Validate(dev)); + PIOS_Assert(data); + + PIOS_SENSORS_3Axis_SensorsWithTemp *tmp = data; + + tmp->count = 1; + tmp->sample[0].x = dev->data_X; + tmp->sample[0].y = dev->data_Y; + tmp->sample[0].z = dev->data_Z; + tmp->temperature = 0; +} + +static bool PIOS_QMC5883L_driver_poll(uintptr_t context) +{ + struct pios_qmc5883l_dev *dev = (struct pios_qmc5883l_dev *)context; + + PIOS_Assert(PIOS_QMC5883L_Validate(dev)); + + if (!dev->sensorIsAlive) { + if (PIOS_QMC5883L_Configure(dev) < 0) { + return false; + } + } + + if (PIOS_DELAY_DiffuS(dev->readTime) < PIOS_QMC5883L_I2C_CONFIG_DATA_DELAY) { + return false; + } + + // Read Status + uint8_t status; + dev->sensorIsAlive = (PIOS_QMC5883L_Read(dev->i2c_id, PIOS_QMC5883L_REG_STATUS, &status, sizeof(status)) == 0); + if (!dev->sensorIsAlive) { + return false; + } + + // Is it ready? + if (!(status & PIOS_QMC5883L_STATUS_DRDY)) { + if (PIOS_DELAY_DiffuS(dev->readTime) > PIOS_QMC5883L_I2C_CONFIG_DATA_TIMEOUT) { // the sensor has reset and it is not in continuous mode anymore + dev->sensorIsAlive = false; + } + return false; + } + + uint8_t data[6]; + dev->sensorIsAlive = (PIOS_QMC5883L_Read(dev->i2c_id, PIOS_QMC5883L_REG_DATA, data, sizeof(data)) == 0); + if (!dev->sensorIsAlive) { + return false; + } + + dev->readTime = PIOS_DELAY_GetRaw(); + + dev->data_X = (int16_t)(data[1] << 8 | data[0]); + dev->data_Y = (int16_t)(data[3] << 8 | data[2]); + dev->data_Z = (int16_t)(data[5] << 8 | data[4]); + + return true; +} + + +#endif /* PIOS_INCLUDE_QMC5883L */ diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index f160b034d..e26a0d880 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -139,7 +139,7 @@ struct pios_rfm22b_transition { }; // Must ensure these prefilled arrays match the define sizes -static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = { +__attribute__((unused)) static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = { PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, @@ -155,7 +155,7 @@ static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = { PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE }; // 64 bytes -static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1) / 2 + 2] = { +__attribute__((unused)) static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1) / 2 + 2] = { PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2 }; @@ -388,7 +388,7 @@ static const uint8_t reg_72[] = { 0x30, 0x48, 0x48, 0x48, 0x48, 0x60, 0x90, 0xCD static const uint8_t packet_time[] = { 80, 40, 25, 15, 13, 10, 8, 6, 5 }; static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 13, 10, 8, 6, 5 }; -static const uint8_t num_channels[] = { 32, 32, 32, 32, 32, 32, 32, 32, 32 }; +__attribute__((unused)) static const uint8_t num_channels[] = { 32, 32, 32, 32, 32, 32, 32, 32, 32 }; static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; diff --git a/flight/pios/common/pios_sdp3x.c b/flight/pios/common/pios_sdp3x.c new file mode 100644 index 000000000..9cd93a3cf --- /dev/null +++ b/flight/pios/common/pios_sdp3x.c @@ -0,0 +1,126 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SDP3x SDP3x Differential pressure sensors + * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 + * @{ + * + * @file pios_stp3x.c + * @author The LibrePilot Team, http://www.librepilot.org Copyright (C) 2021. + * @brief SDP3x Airspeed Sensor Driver + * @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 "pios.h" + +#ifdef PIOS_INCLUDE_SDP3X + +static bool PIOS_SDP3X_WriteCommand(uint16_t command) +{ + uint8_t commandbuffer[2]; + + commandbuffer[0] = (uint8_t)((uint16_t)(command >> 8)); + commandbuffer[1] = (uint8_t)((uint16_t)(command & 0xff)); + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = SDP3X_I2C_ADDR1, + .rw = PIOS_I2C_TXN_WRITE, + .len = 2, + .buf = commandbuffer, + } + }; + + return PIOS_I2C_Transfer(PIOS_I2C_SDP3X_ADAPTER, txn_list, NELEMENTS(txn_list)); +} + +static bool PIOS_SDP3X_Read(uint8_t *buffer, uint8_t len) +{ + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = SDP3X_I2C_ADDR1, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + for (uint8_t retry = PIOS_SDP3X_RETRY_LIMIT; retry > 0; --retry) { + if (PIOS_I2C_Transfer(PIOS_I2C_SDP3X_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0) { + return 0; + } + } + return -1; +} + +uint8_t PIOS_SDP3X_crc(const uint8_t buffer[], unsigned len, uint8_t crc) +{ + uint8_t c = SDP3X_CRC_INIT; + + for (uint8_t i = 0; i < len; i++) { + c ^= buffer[i]; + for (uint8_t b = 8; b > 0; --b) { + if (c & 0x80) { + c = (c << 1) ^ SDP3X_CRC_POLY; + } else { + c = c << 1; + } + } + } + return c == crc; +} + +void PIOS_SDP3X_ReadAirspeed(volatile struct PIOS_SDP3X_STATE *state) +{ + PIOS_Assert(state); + + if (state->status == SDP3X_STATUS_NOTFOUND) { + if (PIOS_SDP3X_WriteCommand(SDP3X_CONT_MODE_STOP) == 0) { + PIOS_DELAY_WaituS(500); + if (PIOS_SDP3X_WriteCommand(SDP3X_CONT_MEAS_AVG_MODE) == 0) { + state->status = SDP3X_STATUS_CONFIGURED; + } + } + return; + } + if (state->status == SDP3X_STATUS_CONFIGURED || state->status == SDP3X_STATUS_READY) { + state->status = SDP3X_STATUS_READY; + uint8_t airspeed_raw[9]; + if (PIOS_SDP3X_Read(airspeed_raw, sizeof(airspeed_raw)) == 0) { + // check checksum + if (!PIOS_SDP3X_crc(&airspeed_raw[0], 2, airspeed_raw[2]) || + !PIOS_SDP3X_crc(&airspeed_raw[0], 2, airspeed_raw[2]) || + !PIOS_SDP3X_crc(&airspeed_raw[0], 2, airspeed_raw[2])) { +// CRC error + state->status = SDP3X_STATUS_CONFIGURED; + return; + } + + state->pressure = (int16_t)((((uint16_t)airspeed_raw[0]) << 8) + (uint16_t)airspeed_raw[1]); + state->temperature = (int16_t)((((uint16_t)airspeed_raw[3]) << 8) + (uint16_t)airspeed_raw[4]); + state->scale = (int16_t)((((uint16_t)airspeed_raw[6]) << 8) + (uint16_t)airspeed_raw[7]); + } else { + state->status = SDP3X_STATUS_NOTFOUND; + } + } +} + +#endif /* PIOS_INCLUDE_SDP3X */ diff --git a/flight/pios/common/pios_video.c b/flight/pios/common/pios_video.c index 3fcea2070..8fa12ff29 100644 --- a/flight/pios/common/pios_video.c +++ b/flight/pios/common/pios_video.c @@ -292,48 +292,48 @@ void PIOS_Video_Init(const struct pios_video_cfg *cfg) GPIO_Init(GPIOC, &initStruct); /* SPI3 - MASKBUFFER */ - GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->mask.sclk.init)); - GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef *)&(cfg->mask.miso.init)); + GPIO_Init(cfg->mask->sclk.gpio, (GPIO_InitTypeDef *)&(cfg->mask->sclk.init)); + GPIO_Init(cfg->mask->miso.gpio, (GPIO_InitTypeDef *)&(cfg->mask->miso.init)); /* SPI1 SLAVE FRAMEBUFFER */ - GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->level.sclk.init)); - GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef *)&(cfg->level.miso.init)); + GPIO_Init(cfg->level->sclk.gpio, (GPIO_InitTypeDef *)&(cfg->level->sclk.init)); + GPIO_Init(cfg->level->miso.gpio, (GPIO_InitTypeDef *)&(cfg->level->miso.init)); - if (cfg->mask.remap) { - GPIO_PinAFConfig(cfg->mask.sclk.gpio, - __builtin_ctz(cfg->mask.sclk.init.GPIO_Pin), - cfg->mask.remap); - GPIO_PinAFConfig(cfg->mask.miso.gpio, - __builtin_ctz(cfg->mask.miso.init.GPIO_Pin), - cfg->mask.remap); + if (cfg->mask->remap) { + GPIO_PinAFConfig(cfg->mask->sclk.gpio, + __builtin_ctz(cfg->mask->sclk.init.GPIO_Pin), + cfg->mask->remap); + GPIO_PinAFConfig(cfg->mask->miso.gpio, + __builtin_ctz(cfg->mask->miso.init.GPIO_Pin), + cfg->mask->remap); } - if (cfg->level.remap) { - GPIO_PinAFConfig(cfg->level.sclk.gpio, - __builtin_ctz(cfg->level.sclk.init.GPIO_Pin), - cfg->level.remap); - GPIO_PinAFConfig(cfg->level.miso.gpio, - __builtin_ctz(cfg->level.miso.init.GPIO_Pin), - cfg->level.remap); + if (cfg->level->remap) { + GPIO_PinAFConfig(cfg->level->sclk.gpio, + __builtin_ctz(cfg->level->sclk.init.GPIO_Pin), + cfg->level->remap); + GPIO_PinAFConfig(cfg->level->miso.gpio, + __builtin_ctz(cfg->level->miso.init.GPIO_Pin), + cfg->level->remap); } /* Initialize the SPI block */ - SPI_Init(cfg->level.regs, (SPI_InitTypeDef *)&(cfg->level.init)); - SPI_Init(cfg->mask.regs, (SPI_InitTypeDef *)&(cfg->mask.init)); + SPI_Init(cfg->level->regs, (SPI_InitTypeDef *)&(cfg->level->init)); + SPI_Init(cfg->mask->regs, (SPI_InitTypeDef *)&(cfg->mask->init)); /* Enable SPI */ - SPI_Cmd(cfg->level.regs, ENABLE); - SPI_Cmd(cfg->mask.regs, ENABLE); + SPI_Cmd(cfg->level->regs, ENABLE); + SPI_Cmd(cfg->mask->regs, ENABLE); /* Configure DMA for SPI Tx SLAVE Maskbuffer */ - DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE); - DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->mask.dma.tx.init)); + DMA_Cmd(cfg->mask->dma.tx.channel, DISABLE); + DMA_Init(cfg->mask->dma.tx.channel, (DMA_InitTypeDef *)&(cfg->mask->dma.tx.init)); /* Configure DMA for SPI Tx SLAVE Framebuffer*/ - DMA_Cmd(cfg->level.dma.tx.channel, DISABLE); - DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->level.dma.tx.init)); + DMA_Cmd(cfg->level->dma.tx.channel, DISABLE); + DMA_Init(cfg->level->dma.tx.channel, (DMA_InitTypeDef *)&(cfg->level->dma.tx.init)); /* Trigger interrupt when for half conversions too to indicate double buffer */ - DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE); + DMA_ITConfig(cfg->level->dma.tx.channel, DMA_IT_TC, ENABLE); /* Configure and clear buffers */ draw_buffer_level = buffer0_level; @@ -347,16 +347,16 @@ void PIOS_Video_Init(const struct pios_video_cfg *cfg) /* Configure DMA interrupt */ - NVIC_Init(&cfg->level.dma.irq.init); + NVIC_Init(&cfg->level->dma.irq.init); /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE); - SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE); + SPI_I2S_DMACmd(cfg->mask->regs, SPI_I2S_DMAReq_Tx, ENABLE); + SPI_I2S_DMACmd(cfg->level->regs, SPI_I2S_DMAReq_Tx, ENABLE); mask_dma = DMA1; main_dma = DMA2; - main_stream = cfg->level.dma.tx.channel; - mask_stream = cfg->mask.dma.tx.channel; + main_stream = cfg->level->dma.tx.channel; + mask_stream = cfg->mask->dma.tx.channel; /* Configure the Video Line interrupt */ PIOS_EXTI_Init(cfg->hsync); PIOS_EXTI_Init(cfg->vsync); @@ -377,26 +377,26 @@ static void prepare_line(uint32_t line_num) dev_cfg->pixel_timer.timer->CNT = dc; - DMA_ClearFlag(dev_cfg->mask.dma.tx.channel, DMA_FLAG_TCIF7 | DMA_FLAG_HTIF7 | DMA_FLAG_FEIF7 | DMA_FLAG_TEIF7); - DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_FEIF5 | DMA_FLAG_TEIF5); + DMA_ClearFlag(dev_cfg->mask->dma.tx.channel, DMA_FLAG_TCIF7 | DMA_FLAG_HTIF7 | DMA_FLAG_FEIF7 | DMA_FLAG_TEIF7); + DMA_ClearFlag(dev_cfg->level->dma.tx.channel, DMA_FLAG_FEIF5 | DMA_FLAG_TEIF5); // Load new line - DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel, (uint32_t)&disp_buffer_level[buf_offset], DMA_Memory_0); - DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel, (uint32_t)&disp_buffer_mask[buf_offset], DMA_Memory_0); + DMA_MemoryTargetConfig(dev_cfg->level->dma.tx.channel, (uint32_t)&disp_buffer_level[buf_offset], DMA_Memory_0); + DMA_MemoryTargetConfig(dev_cfg->mask->dma.tx.channel, (uint32_t)&disp_buffer_mask[buf_offset], DMA_Memory_0); // Enable DMA, Slave first - DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel, BUFFER_LINE_LENGTH); - DMA_SetCurrDataCounter(dev_cfg->mask.dma.tx.channel, BUFFER_LINE_LENGTH); + DMA_SetCurrDataCounter(dev_cfg->level->dma.tx.channel, BUFFER_LINE_LENGTH); + DMA_SetCurrDataCounter(dev_cfg->mask->dma.tx.channel, BUFFER_LINE_LENGTH); - SPI_Cmd(dev_cfg->level.regs, ENABLE); - SPI_Cmd(dev_cfg->mask.regs, ENABLE); + SPI_Cmd(dev_cfg->level->regs, ENABLE); + SPI_Cmd(dev_cfg->mask->regs, ENABLE); /* Enable SPI interrupts to DMA */ - SPI_I2S_DMACmd(dev_cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE); - SPI_I2S_DMACmd(dev_cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE); + SPI_I2S_DMACmd(dev_cfg->mask->regs, SPI_I2S_DMAReq_Tx, ENABLE); + SPI_I2S_DMACmd(dev_cfg->level->regs, SPI_I2S_DMAReq_Tx, ENABLE); - DMA_Cmd(dev_cfg->level.dma.tx.channel, ENABLE); - DMA_Cmd(dev_cfg->mask.dma.tx.channel, ENABLE); + DMA_Cmd(dev_cfg->level->dma.tx.channel, ENABLE); + DMA_Cmd(dev_cfg->mask->dma.tx.channel, ENABLE); } reset_hsync_timers(); } @@ -417,27 +417,27 @@ static void flush_spi() // Can't flush if clock not running while ((dev_cfg->pixel_timer.timer->CR1 & 0x0001) && (!level_stopped | !mask_stopped)) { - level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level.regs, SPI_I2S_FLAG_TXE) == SET; - mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask.regs, SPI_I2S_FLAG_TXE) == SET; + level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level->regs, SPI_I2S_FLAG_TXE) == SET; + mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask->regs, SPI_I2S_FLAG_TXE) == SET; - if (level_empty && !level_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == RESET) { - SPI_Cmd(dev_cfg->level.regs, DISABLE); + if (level_empty && !level_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->level->regs ,SPI_I2S_FLAG_BSY) == RESET) { + SPI_Cmd(dev_cfg->level->regs, DISABLE); level_stopped = true; } - if (mask_empty && !mask_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == RESET) { - SPI_Cmd(dev_cfg->mask.regs, DISABLE); + if (mask_empty && !mask_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->mask->regs ,SPI_I2S_FLAG_BSY) == RESET) { + SPI_Cmd(dev_cfg->mask->regs, DISABLE); mask_stopped = true; } } /* uint32_t i = 0; - while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++; - while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/ - SPI_Cmd(dev_cfg->mask.regs, DISABLE); - SPI_Cmd(dev_cfg->level.regs, DISABLE); + while(SPI_I2S_GetFlagStatus(dev_cfg->level->regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->mask->regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->level->regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++; + while(SPI_I2S_GetFlagStatus(dev_cfg->mask->regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/ + SPI_Cmd(dev_cfg->mask->regs, DISABLE); + SPI_Cmd(dev_cfg->level->regs, DISABLE); } /** @@ -446,8 +446,8 @@ static void flush_spi() void PIOS_VIDEO_DMA_Handler(void) { // Handle flags from stream channel - if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel, DMA_FLAG_TCIF5)) { // whole double buffer filled - DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_TCIF5); + if (DMA_GetFlagStatus(dev_cfg->level->dma.tx.channel, DMA_FLAG_TCIF5)) { // whole double buffer filled + DMA_ClearFlag(dev_cfg->level->dma.tx.channel, DMA_FLAG_TCIF5); if (gActiveLine < GRAPHICS_HEIGHT) { flush_spi(); stop_hsync_timers(); @@ -461,12 +461,12 @@ void PIOS_VIDEO_DMA_Handler(void) stop_hsync_timers(); // STOP DMA, master first - DMA_Cmd(dev_cfg->mask.dma.tx.channel, DISABLE); - DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE); + DMA_Cmd(dev_cfg->mask->dma.tx.channel, DISABLE); + DMA_Cmd(dev_cfg->level->dma.tx.channel, DISABLE); } gActiveLine++; - } else if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel, DMA_FLAG_HTIF5)) { - DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_HTIF5); + } else if (DMA_GetFlagStatus(dev_cfg->level->dma.tx.channel, DMA_FLAG_HTIF5)) { + DMA_ClearFlag(dev_cfg->level->dma.tx.channel, DMA_FLAG_HTIF5); } else {} } 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/pios/inc/pios_com.h b/flight/pios/inc/pios_com.h index 3e33775da..439df28c3 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -84,6 +84,7 @@ struct pios_com_driver { /* Public Functions */ extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len); extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); +extern int32_t PIOS_COM_ClearRxBuffer(uint32_t com_id); extern int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate); extern int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state); extern int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t usart_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context); diff --git a/flight/pios/inc/pios_flash_jedec_catalog.h b/flight/pios/inc/pios_flash_jedec_catalog.h index bc932a209..a71eab5b9 100644 --- a/flight/pios/inc/pios_flash_jedec_catalog.h +++ b/flight/pios/inc/pios_flash_jedec_catalog.h @@ -39,7 +39,7 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = { // m25p16 .expect_manufacturer = JEDEC_MANUFACTURER_ST, .expect_memorytype = 0x20, - .expect_capacity = 0x15, + .expect_capacity = 0x15, // these chips all have 2^expect_capacity bytes .sector_erase = 0xD8, .chip_erase = 0xC7, .fast_read = 0x0B, @@ -48,7 +48,7 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = { // m25px16 .expect_manufacturer = JEDEC_MANUFACTURER_ST, .expect_memorytype = 0x71, - .expect_capacity = 0x15, + .expect_capacity = 0x15, // these chips all have 2^expect_capacity bytes .sector_erase = 0xD8, .chip_erase = 0xC7, .fast_read = 0x0B, @@ -57,43 +57,43 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = { // w25x .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, .expect_memorytype = 0x30, - .expect_capacity = 0x13, - .sector_erase = 0x20, - .chip_erase = 0x60, + .expect_capacity = 0x13, // these chips all have 2^expect_capacity bytes + .sector_erase = 0xD8, // 0xD8 is the 64k sector erase command, 0x20 erases 4k sectors, flashfs_m25p_cfg has fixed 64k sectors + .chip_erase = 0x60, // chip_erase is 0x60 or 0xC7 .fast_read = 0x0B, .fast_read_dummy_bytes = 1, }, { // 25q16 .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, .expect_memorytype = 0x40, - .expect_capacity = 0x15, - .sector_erase = 0x20, - .chip_erase = 0x60, + .expect_capacity = 0x15, // these chips all have 2^expect_capacity bytes + .sector_erase = 0xD8, // 0xD8 is the 64k sector erase command, 0x20 erases 4k sectors, flashfs_m25p_cfg has fixed 64k sectors + .chip_erase = 0x60, // chip_erase is 0x60 or 0xC7 .fast_read = 0x0B, .fast_read_dummy_bytes = 1, }, { // 25q64 .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, .expect_memorytype = 0x40, - .expect_capacity = 0x17, - .sector_erase = 0x20, - .chip_erase = 0x60, + .expect_capacity = 0x17, // these chips all have 2^expect_capacity bytes + .sector_erase = 0xD8, // 0xD8 is the 64k sector erase command, 0x20 erases 4k sectors, flashfs_m25p_cfg has fixed 64k sectors + .chip_erase = 0x60, // chip_erase is 0x60 or 0xC7 .fast_read = 0x0B, .fast_read_dummy_bytes = 1, }, - { // 25q512 + { // 25q512, note that devices over 128Mb=16MB probably require recoding to use 4 byte addresses .expect_manufacturer = JEDEC_MANUFACTURER_MICRON, .expect_memorytype = 0xBA, - .expect_capacity = 0x20, - .sector_erase = 0xD8, + .expect_capacity = 0x1A, // pretty sure this should be 0x1A and not 0x20 (author counted 0x19 then 0x20, should have been 0x1A) + .sector_erase = 0xD8, // (these chips all have 2^expect_capacity bytes) .chip_erase = 0xC7, .fast_read = 0x0B, .fast_read_dummy_bytes = 1, }, - { // 25q256 + { // 25q256, note that devices over 128Mb=16MB probably require recoding to use 4 byte addresses .expect_manufacturer = JEDEC_MANUFACTURER_NUMORIX, .expect_memorytype = 0xBA, - .expect_capacity = 0x19, + .expect_capacity = 0x19, // these chips all have 2^expect_capacity bytes .sector_erase = 0xD8, .chip_erase = 0xC7, .fast_read = 0x0B, @@ -102,7 +102,7 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = { // 25q128 .expect_manufacturer = JEDEC_MANUFACTURER_MICRON, .expect_memorytype = 0xBA, - .expect_capacity = 0x18, + .expect_capacity = 0x18, // these chips all have 2^expect_capacity bytes .sector_erase = 0xD8, .chip_erase = 0xC7, .fast_read = 0x0B, diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index 8c89b13c5..e812c850b 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -26,6 +26,9 @@ #ifndef PIOS_MATH_H #define PIOS_MATH_H + +#include + // Generic float math constants #define M_E_F 2.71828182845904523536028747135f /* e */ #define M_LOG2E_F 1.44269504088896340735992468100f /* log_2 (e) */ @@ -66,20 +69,23 @@ #define M_EULER_D 0.57721566490153286060651209008d /* Euler constant */ // Conversion macro -#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F)) -#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f)) +#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F)) +#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f)) -#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D)) -#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d)) +#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D)) +#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d)) // helper macros for LPFs -#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc))) +#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc))) // Useful math macros -#define MAX(a, b) ((a) > (b) ? (a) : (b)) -#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) -#define IS_REAL(f) (isfinite(f)) +__attribute__((optimize("no-fast-math"))) static inline int IS_REAL(float f) +{ + return !(isnan(f) || isinf(f)); +} // Bitfield access #define IS_SET(field, mask) (((field) & (mask)) == (mask)) diff --git a/flight/pios/inc/pios_qmc5883l.h b/flight/pios/inc/pios_qmc5883l.h new file mode 100644 index 000000000..8b1bb5509 --- /dev/null +++ b/flight/pios/inc/pios_qmc5883l.h @@ -0,0 +1,40 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_QMC5883L QMC5883L Functions + * @brief Deals with the hardware interface to the QMC5883L magnetometer + * @{ + * + * @file pios_qmc5883l.h + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (C) 2018 + * @brief QMC5883L functions header. + * @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 PIOS_QMC5883L_H +#define PIOS_QMC5883L_H + +struct pios_qmc5883l_dev; +typedef struct pios_qmc5883l_dev *pios_qmc5883l_dev_t; + +extern pios_qmc5883l_dev_t PIOS_QMC5883L_Init(uint32_t i2c_device); + + +#endif /* PIOS_QMC5883L_H */ diff --git a/flight/pios/inc/pios_sdp3x.h b/flight/pios/inc/pios_sdp3x.h new file mode 100644 index 000000000..9b11452c8 --- /dev/null +++ b/flight/pios/inc/pios_sdp3x.h @@ -0,0 +1,62 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SDP3x SDP3x Differential pressure sensors + * @brief Hardware functions to deal with the Eagle Tree Airspeed MicroSensor V3 + * @{ + * + * @file pios_sdp3x.h + * @author The LibrePilot Team, http://www.librepilot.org Copyright (C) 2021. + * @brief SDP3x Airspeed Sensor Driver + * @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 PIOS_SDP3X_H +#define PIOS_SDP3X_H + +#define SDP3X_I2C_ADDR1 0x21 +#define SDP3X_I2C_ADDR2 0x22 +#define SDP3X_I2C_ADDR3 0x23 + +#define SDP3X_SCALE_TEMPERATURE 200.0f +#define SDP3X_RESET_ADDR 0x00 +#define SDP3X_RESET_CMD 0x06 +#define SDP3X_CONT_MEAS_AVG_MODE 0x3615 +#define SDP3X_CONT_MODE_STOP 0x3FF9 + +#define SDP3X_SCALE_PRESSURE_SDP31 60 +#define SDP3X_SCALE_PRESSURE_SDP32 240 +#define SDP3X_SCALE_PRESSURE_SDP33 20 +#define SDP3X_CRC_INIT 0xff +#define SDP3X_CRC_POLY 0x31 + +#define SDP3X_CONVERSION_INTERVAL (1000000 / SDP3X_MEAS_RATE) /* microseconds */ + +#define PIOS_SDP3X_RETRY_LIMIT 3 +struct PIOS_SDP3X_STATE { + enum { SDP3X_STATUS_NOTFOUND, SDP3X_STATUS_CONFIGURED, SDP3X_STATUS_READY } status; + int16_t pressure; + int16_t temperature; + int16_t scale; +}; + +void PIOS_SDP3X_ReadAirspeed(volatile struct PIOS_SDP3X_STATE *state); + +#endif /* PIOS_SDP3X_H */ diff --git a/flight/pios/inc/pios_video.h b/flight/pios/inc/pios_video.h index 8781aa911..7d3c31f6b 100644 --- a/flight/pios/inc/pios_video.h +++ b/flight/pios/inc/pios_video.h @@ -36,8 +36,8 @@ #include struct pios_video_cfg { - const struct pios_spi_cfg mask; - const struct pios_spi_cfg level; + const struct pios_spi_cfg *mask; + const struct pios_spi_cfg *level; const struct pios_exti_cfg *hsync; const struct pios_exti_cfg *vsync; diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 8d37b0852..3857b362c 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -199,6 +199,11 @@ extern "C" { #include #endif +#ifdef PIOS_INCLUDE_SDP3X +/* SDP3X Airspeed MicroSensor*/ +#include +#endif + #ifdef PIOS_INCLUDE_MS4525DO /* PixHawk Airspeed Sensor based on MS4525DO */ #include @@ -339,6 +344,10 @@ extern "C" { /* Performance counters */ /* #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 */ +#if defined(PIOS_INCLUDE_HMC5X83) || defined(PIOS_INCLUDE_QMC5883L) +#define PIOS_INCLUDE_SENSORS_AUXMAG +#endif + #endif /* USE_SIM_POSIX */ diff --git a/flight/pios/stm32f0x/libraries/CMSIS/Include/core_cmFunc.h b/flight/pios/stm32f0x/libraries/CMSIS/Include/core_cmFunc.h index 0a18fafc3..595f671ff 100644 --- a/flight/pios/stm32f0x/libraries/CMSIS/Include/core_cmFunc.h +++ b/flight/pios/stm32f0x/libraries/CMSIS/Include/core_cmFunc.h @@ -438,7 +438,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void) */ __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack) { - __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp"); + __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : ); } @@ -465,7 +465,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void) */ __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack) { - __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp"); + __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : ); } diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index 0f664429e..09c3d42a2 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -539,6 +539,7 @@ static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) return -2; /* do not allow dsm bind on port with inverter */ } #endif /* otherwise, return RXGPIO */ + // fall through case PIOS_IOCTL_USART_GET_RXGPIO: *(struct stm32_gpio *)param = usart_dev->cfg->rx; break; diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_hrtim.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_hrtim.c index 40e3bd7a6..bf746d7d8 100755 --- a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_hrtim.c +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_hrtim.c @@ -2838,6 +2838,7 @@ void HRTIM_ADCTriggerConfig(HRTIM_TypeDef * HRTIMx, /* Set the ADC trigger 3 source */ HRTIMx->HRTIM_COMMON.ADC3R = pADCTriggerCfg->Trigger; } + break; case HRTIM_ADCTRIGGER_4: { HRTIM_cr1 &= ~(HRTIM_CR1_ADC4USRC); diff --git a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rcc.c b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rcc.c index f8568fa30..c364cfe8b 100755 --- a/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rcc.c +++ b/flight/pios/stm32f30x/libraries/STM32F30x_StdPeriph_Driver/src/stm32f30x_rcc.c @@ -1386,8 +1386,10 @@ void RCC_TIMCLKConfig(uint32_t RCC_TIMCLK) break; case 0x05: RCC->CFGR3 &= ~RCC_CFGR3_TIM20SW; + break; case 0x06: RCC->CFGR3 &= ~RCC_CFGR3_TIM2SW; + break; case 0x07: RCC->CFGR3 &= ~RCC_CFGR3_TIM3SW; break; diff --git a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/discoveryf4bare/system_stm32f4xx.c b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/discoveryf4bare/system_stm32f4xx.c index cbc36d6e3..abd71303c 100644 --- a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/discoveryf4bare/system_stm32f4xx.c +++ b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/discoveryf4bare/system_stm32f4xx.c @@ -405,7 +405,7 @@ static void SetSysClock(void) RCC->CFGR |= RCC_CFGR_SW_PLL; /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL) { } } diff --git a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c index aa5928619..bc49755fe 100644 --- a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c +++ b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c @@ -405,7 +405,7 @@ static void SetSysClock(void) RCC->CFGR |= RCC_CFGR_SW_PLL; /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL) { } } diff --git a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revolution/system_stm32f4xx.c b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revolution/system_stm32f4xx.c index e6ffcd777..d0ee79e5c 100644 --- a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revolution/system_stm32f4xx.c +++ b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revolution/system_stm32f4xx.c @@ -405,7 +405,7 @@ static void SetSysClock(void) RCC->CFGR |= RCC_CFGR_SW_PLL; /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL) { } } diff --git a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revonano/system_stm32f4xx.c b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revonano/system_stm32f4xx.c index fe1149ddb..b99ef0bc6 100644 --- a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revonano/system_stm32f4xx.c +++ b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revonano/system_stm32f4xx.c @@ -404,7 +404,7 @@ static void SetSysClock(void) RCC->CFGR |= RCC_CFGR_SW_PLL; /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL) { } } diff --git a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revoproto/system_stm32f4xx.c b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revoproto/system_stm32f4xx.c index a5589ead7..dfe884ca0 100644 --- a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revoproto/system_stm32f4xx.c +++ b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/revoproto/system_stm32f4xx.c @@ -405,7 +405,7 @@ static void SetSysClock(void) RCC->CFGR |= RCC_CFGR_SW_PLL; /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL) { } } diff --git a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/sparky2/system_stm32f4xx.c b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/sparky2/system_stm32f4xx.c index e6ffcd777..d0ee79e5c 100644 --- a/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/sparky2/system_stm32f4xx.c +++ b/flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/sparky2/system_stm32f4xx.c @@ -405,7 +405,7 @@ static void SetSysClock(void) RCC->CFGR |= RCC_CFGR_SW_PLL; /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL) { } } diff --git a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c index 18fcd2abb..4ab030b8f 100644 --- a/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c +++ b/flight/pios/stm32f4xx/libraries/STM32_USB_OTG_Driver/src/usb_core.c @@ -180,7 +180,7 @@ USB_OTG_STS USB_OTG_WritePacket(USB_OTG_CORE_HANDLE *pdev, fifo = pdev->regs.DFIFO[ch_ep_num]; for (i = 0; i < count32b; i++, src+=4) { - USB_OTG_WRITE_REG32( fifo, *((__packed uint32_t *)src) ); + USB_OTG_WRITE_REG32( fifo, *((uint32_t *)src) ); } } return status; @@ -205,7 +205,7 @@ void *USB_OTG_ReadPacket(USB_OTG_CORE_HANDLE *pdev, for ( i = 0; i < count32b; i++, dest += 4 ) { - *(__packed uint32_t *)dest = USB_OTG_READ_REG32(fifo); + *(uint32_t *)dest = USB_OTG_READ_REG32(fifo); } return ((void *)dest); diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index fef3165dd..bb6fa4e7f 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -595,6 +595,7 @@ static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) return -2; /* do not allow dsm bind on port with inverter */ } #endif /* otherwise, return RXGPIO */ + // fall through case PIOS_IOCTL_USART_GET_RXGPIO: *(struct stm32_gpio *)param = usart_dev->cfg->rx; break; diff --git a/flight/pios/stm32f4xx/startup.c b/flight/pios/stm32f4xx/startup.c index b11f3a0cc..b545eed0d 100644 --- a/flight/pios/stm32f4xx/startup.c +++ b/flight/pios/stm32f4xx/startup.c @@ -112,7 +112,7 @@ static void default_cpu_handler(void) } /** Prototype for optional exception vector handlers */ -#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_cpu_handler"))) +#define HANDLER(_name) extern vector _name __attribute__((weak, alias("default_cpu_handler"), noreturn)) /* standard CMSIS vector names */ HANDLER(NMI_Handler); diff --git a/flight/targets/boards/ccf3d/pios_board.h b/flight/targets/boards/ccf3d/pios_board.h index 6d0f67d46..012c9ed33 100644 --- a/flight/targets/boards/ccf3d/pios_board.h +++ b/flight/targets/boards/ccf3d/pios_board.h @@ -251,7 +251,7 @@ extern uint32_t pios_ws2811_id; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 370955e2a..154695fd5 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -275,7 +275,7 @@ extern uint32_t pios_spi_flash_accel_adapter_id; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h index 577c10ec6..cb8958086 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h +++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h @@ -87,6 +87,7 @@ /* #define PIOS_INCLUDE_HMC5843 */ #define PIOS_INCLUDE_HMC5X83 #define PIOS_INCLUDE_HMC5X83_INTERNAL +#define PIOS_INCLUDE_QMC5883L // #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ /* #define PIOS_INCLUDE_MS56XX */ diff --git a/flight/targets/boards/discoveryf4bare/pios_board.h b/flight/targets/boards/discoveryf4bare/pios_board.h index 048e2fe53..4c012326c 100644 --- a/flight/targets/boards/discoveryf4bare/pios_board.h +++ b/flight/targets/boards/discoveryf4bare/pios_board.h @@ -112,6 +112,7 @@ extern uint32_t pios_i2c_mag_pressure_adapter_id; extern uint32_t pios_i2c_flexiport_adapter_id; #define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) #define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) #define PIOS_I2C_EXTERNAL_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) // ------------------------- diff --git a/flight/targets/boards/nucleof303re/firmware/inc/pios_config.h b/flight/targets/boards/nucleof303re/firmware/inc/pios_config.h index abe4f0eaf..9123f8d8f 100644 --- a/flight/targets/boards/nucleof303re/firmware/inc/pios_config.h +++ b/flight/targets/boards/nucleof303re/firmware/inc/pios_config.h @@ -86,6 +86,7 @@ /* #define PIOS_MPU6000_ACCEL */ /* #define PIOS_INCLUDE_HMC5843 */ #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_QMC5883L /* #define PIOS_HMC5883_HAS_GPIOS */ /* #define PIOS_INCLUDE_MPU9250 */ /* #define PIOS_MPU9250_ACCEL */ diff --git a/flight/targets/boards/nucleof303re/pios_board.h b/flight/targets/boards/nucleof303re/pios_board.h index 65d4272bb..fb1f1fd59 100644 --- a/flight/targets/boards/nucleof303re/pios_board.h +++ b/flight/targets/boards/nucleof303re/pios_board.h @@ -210,7 +210,7 @@ extern uint32_t pios_i2c_id; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs diff --git a/flight/targets/boards/osd/board_hw_defs.c b/flight/targets/boards/osd/board_hw_defs.c index d38d509ce..445ad947f 100644 --- a/flight/targets/boards/osd/board_hw_defs.c +++ b/flight/targets/boards/osd/board_hw_defs.c @@ -607,154 +607,157 @@ static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = { }, }; +static const struct pios_spi_cfg mask = { + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_1Line_Tx, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case RX) + .flags = (DMA_IT_TCIF7), + .init = { + .NVIC_IRQChannel = DMA1_Stream7_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + /*.rx = {},*/ + .tx = { + .channel = DMA1_Stream7, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_BufferSize = BUFFER_LINE_LENGTH, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_VeryHigh, + .DMA_FIFOMode = DMA_FIFOMode_Enable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + /*.mosi = {},*/ + .slave_count = 1, +}; + +static const struct pios_spi_cfg level = { + .regs = SPI1, + .remap = GPIO_AF_SPI1, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_1Line_Tx, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = false, + .dma = { + .irq = { + .flags = (DMA_IT_TCIF5), + .init = { + .NVIC_IRQChannel = DMA2_Stream5_IRQn, + .NVIC_IRQChannelPreemptionPriority = 0, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + /*.rx = {},*/ + .tx = { + .channel = DMA2_Stream5, + .init = { + .DMA_Channel = DMA_Channel_3, + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_BufferSize = BUFFER_LINE_LENGTH, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_VeryHigh, + .DMA_FIFOMode = DMA_FIFOMode_Enable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + /*.mosi = {},*/ + .slave_count = 1, +}; static const struct pios_video_cfg pios_video_cfg = { - .mask = { - .regs = SPI3, - .remap = GPIO_AF_SPI3, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_1Line_Tx, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - // Note this is the stream ID that triggers interrupts (in this case RX) - .flags = (DMA_IT_TCIF7), - .init = { - .NVIC_IRQChannel = DMA1_Stream7_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - /*.rx = {},*/ - .tx = { - .channel = DMA1_Stream7, - .init = { - .DMA_Channel = DMA_Channel_0, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI3->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_BufferSize = BUFFER_LINE_LENGTH, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_VeryHigh, - .DMA_FIFOMode = DMA_FIFOMode_Enable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - .miso = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_NOPULL - }, - }, - /*.mosi = {},*/ - .slave_count = 1, - }, - .level = { - .regs = SPI1, - .remap = GPIO_AF_SPI1, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_1Line_Tx, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_Low, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = false, - .dma = { - .irq = { - .flags = (DMA_IT_TCIF5), - .init = { - .NVIC_IRQChannel = DMA2_Stream5_IRQn, - .NVIC_IRQChannelPreemptionPriority = 0, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - /*.rx = {},*/ - .tx = { - .channel = DMA2_Stream5, - .init = { - .DMA_Channel = DMA_Channel_3, - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_MemoryToPeripheral, - .DMA_BufferSize = BUFFER_LINE_LENGTH, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_VeryHigh, - .DMA_FIFOMode = DMA_FIFOMode_Enable, - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, - }, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_100MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - /*.mosi = {},*/ - .slave_count = 1, - }, + .mask = &mask, + .level = &level, .hsync = &pios_exti_hsync_cfg, .vsync = &pios_exti_vsync_cfg, - .pixel_timer = { + .pixel_timer = { .timer = TIM4, - .timer_chan = TIM_Channel_1, + .timer_chan = TIM_Channel_1, .pin = { .gpio = GPIOB, .init = { @@ -764,13 +767,13 @@ static const struct pios_video_cfg pios_video_cfg = { .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }, - .pin_source = GPIO_PinSource6, + .pin_source = GPIO_PinSource6, }, - .remap = GPIO_AF_TIM4, + .remap = GPIO_AF_TIM4, }, - .hsync_capture = { + .hsync_capture = { .timer = TIM4, - .timer_chan = TIM_Channel_2, + .timer_chan = TIM_Channel_2, .pin = { .gpio = GPIOB, .init = { @@ -780,11 +783,11 @@ static const struct pios_video_cfg pios_video_cfg = { .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_UP }, - .pin_source = GPIO_PinSource7, + .pin_source = GPIO_PinSource7, }, - .remap = GPIO_AF_TIM4, + .remap = GPIO_AF_TIM4, }, - .tim_oc_init = { + .tim_oc_init = { .TIM_OCMode = TIM_OCMode_PWM1, .TIM_OutputState = TIM_OutputState_Enable, .TIM_OutputNState = TIM_OutputNState_Disable, diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c index aebfe97a2..92c1afbee 100644 --- a/flight/targets/boards/osd/firmware/pios_board.c +++ b/flight/targets/boards/osd/firmware/pios_board.c @@ -118,7 +118,7 @@ static const TIM_TimeBaseInitTypeDef tim_4_time_base = { .TIM_RepetitionCounter = 0x0000, }; -static const struct pios_tim_clock_cfg pios_tim4_cfg = { +__attribute__((unused)) static const struct pios_tim_clock_cfg pios_tim4_cfg = { .timer = TIM4, .time_base_init = &tim_4_time_base, .irq = { diff --git a/flight/targets/boards/pikoblx/pios_board.h b/flight/targets/boards/pikoblx/pios_board.h index f3654b0b0..d60fd1f0c 100644 --- a/flight/targets/boards/pikoblx/pios_board.h +++ b/flight/targets/boards/pikoblx/pios_board.h @@ -208,7 +208,7 @@ extern uint32_t pios_ws2811_id; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs 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/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 6694ff33d..170a026a1 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -88,10 +88,12 @@ #define PIOS_INCLUDE_HMC5X83 #define PIOS_INCLUDE_HMC5X83_INTERNAL #define PIOS_HMC5X83_HAS_GPIOS +#define PIOS_INCLUDE_QMC5883L /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS56XX #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 +#define PIOS_INCLUDE_SDP3X #define PIOS_INCLUDE_MS4525DO /* #define PIOS_INCLUDE_HCSR04 */ @@ -162,6 +164,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..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, @@ -75,6 +76,7 @@ static const PIOS_BOARD_IO_UART_Function flexiio_function_map[] = { [HWSETTINGS_RM_RCVRPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, [HWSETTINGS_RM_RCVRPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, [HWSETTINGS_RM_RCVRPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, + [HWSETTINGS_RM_RCVRPORT_FRSKYSPORT] = PIOS_BOARD_IO_UART_FRSKY_SPORT, }; static const PIOS_BOARD_IO_UART_Function main_function_map[] = { @@ -89,6 +91,7 @@ static const PIOS_BOARD_IO_UART_Function main_function_map[] = { [HWSETTINGS_RM_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, [HWSETTINGS_RM_MAINPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, [HWSETTINGS_RM_MAINPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, + [HWSETTINGS_RM_MAINPORT_FRSKYSPORT] = PIOS_BOARD_IO_UART_FRSKY_SPORT, }; static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { @@ -107,6 +110,7 @@ static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { [HWSETTINGS_RM_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, [HWSETTINGS_RM_FLEXIPORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB, [HWSETTINGS_RM_FLEXIPORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE, + [HWSETTINGS_RM_FLEXIPORT_FRSKYSPORT] = PIOS_BOARD_IO_UART_FRSKY_SPORT, }; /** @@ -295,6 +299,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: case HWSETTINGS_RM_RCVRPORT_PPMGPS: case HWSETTINGS_RM_RCVRPORT_PPMHOTTTELEMETRY: + case HWSETTINGS_RM_RCVRPORT_PPMFRSKYSPORT: #if defined(PIOS_INCLUDE_PPM) PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 811b8aad9..453af26a3 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -131,6 +131,7 @@ extern uint32_t pios_i2c_mag_pressure_adapter_id; extern uint32_t pios_i2c_flexiport_adapter_id; #define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) #define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) #define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) #define PIOS_I2C_EXTERNAL_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) @@ -276,7 +277,7 @@ extern uint32_t pios_packet_handler; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index d784db783..2cac9a2df 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -86,11 +86,13 @@ // #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_QMC5883L // #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS56XX #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 +#define PIOS_INCLUDE_SDP3X #define PIOS_INCLUDE_MS4525DO #define PIOS_INCLUDE_MPU9250 #define PIOS_MPU9250_ACCEL diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 73c3e10bd..ed57cde73 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -127,6 +127,7 @@ extern uint32_t pios_i2c_eeprom_pressure_adapter_id; extern uint32_t pios_i2c_flexiport_adapter_id; #define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) #define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) #define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) #define PIOS_I2C_EXTERNAL_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) @@ -271,7 +272,7 @@ extern uint32_t pios_packet_handler; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 7238d5885..58b99a991 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -85,10 +85,12 @@ #define PIOS_INCLUDE_HMC5X83 #define PIOS_INCLUDE_HMC5X83_INTERNAL #define PIOS_HMC5X83_HAS_GPIOS +#define PIOS_INCLUDE_QMC5883L /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS56XX #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 +#define PIOS_INCLUDE_SDP3X /* #define PIOS_INCLUDE_HCSR04 */ #define PIOS_SENSOR_RATE 500.0f diff --git a/flight/targets/boards/revoproto/pios_board.h b/flight/targets/boards/revoproto/pios_board.h index 960334acf..fa5bdeef6 100644 --- a/flight/targets/boards/revoproto/pios_board.h +++ b/flight/targets/boards/revoproto/pios_board.h @@ -114,6 +114,7 @@ extern uint32_t pios_i2c_pressure_adapter_id; extern uint32_t pios_i2c_flexiport_adapter_id; #define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) #define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) #define PIOS_I2C_EXTERNAL_ADAPTER (pios_i2c_flexiport_adapter_id) diff --git a/flight/targets/boards/sparky2/firmware/inc/pios_config.h b/flight/targets/boards/sparky2/firmware/inc/pios_config.h index c3dc8fcaf..31416b42f 100644 --- a/flight/targets/boards/sparky2/firmware/inc/pios_config.h +++ b/flight/targets/boards/sparky2/firmware/inc/pios_config.h @@ -86,11 +86,13 @@ /* seems to be completely unused #define PIOS_MPU6000_ACCEL */ /* #define PIOS_INCLUDE_HMC5843 */ #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_QMC5883L /* Sparky2 5X83s are all external and thus don't have GPIOs #define PIOS_HMC5X83_HAS_GPIOS */ /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS56XX #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 +#define PIOS_INCLUDE_SDP3X #define PIOS_INCLUDE_MS4525DO #define PIOS_INCLUDE_MPU9250 #define PIOS_MPU9250_ACCEL diff --git a/flight/targets/boards/sparky2/pios_board.h b/flight/targets/boards/sparky2/pios_board.h index 825cf18d7..8356fc22f 100644 --- a/flight/targets/boards/sparky2/pios_board.h +++ b/flight/targets/boards/sparky2/pios_board.h @@ -133,6 +133,7 @@ extern uint32_t pios_i2c_pressure_adapter_id; extern uint32_t pios_i2c_flexiport_adapter_id; #define PIOS_I2C_FLEXI_ADAPTER (pios_i2c_flexiport_adapter_id) #define PIOS_I2C_ETASV3_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) +#define PIOS_I2C_SDP3X_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) #define PIOS_I2C_MS4525DO_ADAPTER (PIOS_I2C_FLEXI_ADAPTER) // ------------------------- @@ -278,7 +279,7 @@ extern uint32_t pios_packet_handler; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs diff --git a/flight/targets/boards/spracingf3/firmware/inc/pios_config.h b/flight/targets/boards/spracingf3/firmware/inc/pios_config.h index b4f856a2b..36aa4826c 100644 --- a/flight/targets/boards/spracingf3/firmware/inc/pios_config.h +++ b/flight/targets/boards/spracingf3/firmware/inc/pios_config.h @@ -88,6 +88,7 @@ #define PIOS_INCLUDE_HMC5X83 #define PIOS_INCLUDE_HMC5X83_INTERNAL #define PIOS_HMC5883_HAS_GPIOS +#define PIOS_INCLUDE_QMC5883L /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS56XX /* #define PIOS_INCLUDE_MPXV */ diff --git a/flight/targets/boards/spracingf3/pios_board.h b/flight/targets/boards/spracingf3/pios_board.h index 988cdb661..953aa8cba 100644 --- a/flight/targets/boards/spracingf3/pios_board.h +++ b/flight/targets/boards/spracingf3/pios_board.h @@ -214,7 +214,7 @@ extern uint32_t pios_ws2811_id; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs diff --git a/flight/targets/boards/spracingf3evo/firmware/inc/pios_config.h b/flight/targets/boards/spracingf3evo/firmware/inc/pios_config.h index 89bd45e3b..0d8830f61 100644 --- a/flight/targets/boards/spracingf3evo/firmware/inc/pios_config.h +++ b/flight/targets/boards/spracingf3evo/firmware/inc/pios_config.h @@ -86,6 +86,7 @@ /* #define PIOS_MPU6000_ACCEL */ /* #define PIOS_INCLUDE_HMC5843 */ #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_QMC5883L /* #define PIOS_HMC5883_HAS_GPIOS */ #define PIOS_INCLUDE_MPU9250 #define PIOS_MPU9250_ACCEL diff --git a/flight/targets/boards/spracingf3evo/pios_board.h b/flight/targets/boards/spracingf3evo/pios_board.h index 629b33783..0f631f190 100644 --- a/flight/targets/boards/spracingf3evo/pios_board.h +++ b/flight/targets/boards/spracingf3evo/pios_board.h @@ -219,7 +219,7 @@ extern uint32_t pios_ws2811_id; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs diff --git a/flight/targets/boards/tinyfish/pios_board.h b/flight/targets/boards/tinyfish/pios_board.h index 45e4f69e4..33ed376ef 100644 --- a/flight/targets/boards/tinyfish/pios_board.h +++ b/flight/targets/boards/tinyfish/pios_board.h @@ -182,7 +182,7 @@ extern uint32_t pios_ws2811_id; // Receiver FlySky IBus input // ------------------------- #define PIOS_IBUS_MAX_DEVS 1 -#define PIOS_IBUS_NUM_INPUTS 10 +#define PIOS_IBUS_NUM_INPUTS 14 // ------------------------- // Servo outputs diff --git a/flight/uavobjects/inc/uavobjectprivate.h b/flight/uavobjects/inc/uavobjectprivate.h index 2177539fd..501e62420 100644 --- a/flight/uavobjects/inc/uavobjectprivate.h +++ b/flight/uavobjects/inc/uavobjectprivate.h @@ -102,13 +102,13 @@ struct UAVOBase { bool isSettings : 1; bool isPriority : 1; } flags; -} __attribute__((packed)); +} __attribute__((packed, aligned(4))); /* Augmented type for Meta UAVO */ struct UAVOMeta { struct UAVOBase base; UAVObjMetadata instance0; -} __attribute__((packed)); +} __attribute__((packed, aligned(4))); /* Shared data structure for all data-carrying UAVObjects (UAVOSingle and UAVOMulti) */ struct UAVOData { @@ -130,7 +130,7 @@ struct UAVOSingle { * Additional space will be malloc'd here to hold the * the data for this instance. */ -} __attribute__((packed)); +} __attribute__((packed, aligned(4))); /* Part of a linked list of instances chained off of a multi instance UAVO. */ struct UAVOMultiInst { @@ -151,7 +151,7 @@ struct UAVOMulti { * Additional space will be malloc'd here to hold the * the data for instance 0. */ -} __attribute__((packed)); +} __attribute__((packed, aligned(4))); /** all information about a metaobject are hardcoded constants **/ #define MetaNumBytes sizeof(UAVObjMetadata) diff --git a/ground/gcs/src/plugins/config/configcamerastabilizationwidget.cpp b/ground/gcs/src/plugins/config/configcamerastabilizationwidget.cpp index ce0c51332..8c7fae4fa 100644 --- a/ground/gcs/src/plugins/config/configcamerastabilizationwidget.cpp +++ b/ground/gcs/src/plugins/config/configcamerastabilizationwidget.cpp @@ -128,6 +128,8 @@ void ConfigCameraStabilizationWidget::refreshWidgetsValuesImpl(UAVObject *obj) &mixerSettingsData.Mixer8Type, &mixerSettingsData.Mixer9Type, &mixerSettingsData.Mixer10Type, + &mixerSettingsData.Mixer11Type, + &mixerSettingsData.Mixer12Type, }; const int NUM_MIXERS = sizeof(mixerTypes) / sizeof(mixerTypes[0]); @@ -183,6 +185,8 @@ void ConfigCameraStabilizationWidget::updateObjectsFromWidgetsImpl() &mixerSettingsData.Mixer8Type, &mixerSettingsData.Mixer9Type, &mixerSettingsData.Mixer10Type, + &mixerSettingsData.Mixer11Type, + &mixerSettingsData.Mixer12Type, }; const int NUM_MIXERS = sizeof(mixerTypes) / sizeof(mixerTypes[0]); diff --git a/ground/gcs/src/plugins/config/inputchannelform.cpp b/ground/gcs/src/plugins/config/inputchannelform.cpp index 75265878f..85ccf8004 100644 --- a/ground/gcs/src/plugins/config/inputchannelform.cpp +++ b/ground/gcs/src/plugins/config/inputchannelform.cpp @@ -147,7 +147,7 @@ void InputChannelForm::groupUpdated() count = 8; // Need to make this 6 for CC break; case ManualControlSettings::CHANNELGROUPS_IBUS: - count = 10; + count = 14; break; case ManualControlSettings::CHANNELGROUPS_DSMMAINPORT: case ManualControlSettings::CHANNELGROUPS_DSMFLEXIPORT: diff --git a/ground/gcs/src/plugins/flightlog/flightlogmanager.cpp b/ground/gcs/src/plugins/flightlog/flightlogmanager.cpp index 02a3b0f55..b216e49f3 100644 --- a/ground/gcs/src/plugins/flightlog/flightlogmanager.cpp +++ b/ground/gcs/src/plugins/flightlog/flightlogmanager.cpp @@ -229,13 +229,16 @@ void FlightLogManager::retrieveLogs(int flightToRetrieve) // cycle until there is space for another object while (start + header_len + 1 < data_len) { memset(&fields, 0xFF, total_len); - memcpy(&fields, &logEntry->getData().Data[start], header_len); + ExtendedDebugLogEntry::DataFields tmp = logEntry->getData(); + memcpy(&fields, &tmp.Data[start], header_len); + // check wether a packed object is found // note that empty data blocks are set as 0xFF in flight side to minimize flash wearing // thus as soon as this read outside of used area, the test will fail as lenght would be 0xFFFF quint32 toread = header_len + fields.Size; if (!(toread + start > data_len)) { - memcpy(&fields, &logEntry->getData().Data[start], toread); + tmp = logEntry->getData(); + memcpy(&fields, &tmp.Data[start], toread); ExtendedDebugLogEntry *subEntry = new ExtendedDebugLogEntry(); subEntry->setData(fields, m_objectManager); m_logEntries << subEntry; 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/make/tool_install/gcc-arm-none-eabi.sh b/make/tool_install/gcc-arm-none-eabi.sh index 807709175..6e19cd4d0 100644 --- a/make/tool_install/gcc-arm-none-eabi.sh +++ b/make/tool_install/gcc-arm-none-eabi.sh @@ -1,24 +1,26 @@ if [ "$uname" = Linux ] then - url_ext="linux.tar.bz2" + url_ext="x86_64-linux.tar.bz2" + tool_md5="2383e4eb4ea23f248d33adc70dc3227e" elif [ "$uname" = Darwin ] then url_ext="mac.tar.bz2" + tool_md5="7f2a7b7b23797302a9d6182c6e482449" elif [ "$uname" = Windows ] then url_ext="win32.zip" + tool_md5="2bc8f0c4c4659f8259c8176223eeafc1" depends=(7z) fi -pkgver=4.9_2015_q2_update -pkgdate=20150609 +pkgver=10.3-2021.10 _pkgver=${pkgver//_/-} _pkgvershort=${_pkgver%-*} _pkgvershort=${_pkgvershort/-q/q} -tool_url="https://launchpad.net/gcc-arm-embedded/${pkgver%%_*}/${_pkgver}/+download/${tool}-${_pkgvershort/./_}-${pkgdate}-${url_ext}" -tool_md5_url="${tool_url}/+md5" -tool_install_name="${tool}-${_pkgvershort/./_}" +tool_url="https://developer.arm.com/-/media/Files/downloads/gnu-rm/${pkgver}/${tool}-${pkgver}-${url_ext}" +#tool_install_name="${tool}-${_pkgvershort/./_}" +tool_install_name="${tool}-${pkgver}" if [ "$uname" = Windows ] then tool_extract_dir=$tools_dir/$tool_install_name diff --git a/make/tools.mk b/make/tools.mk index c626fcb3e..a9cdb2526 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -92,6 +92,7 @@ TOOLS_URL := http://librepilot.github.io/tools QT_SHORT_VERSION := 5.9 QT_VERSION := 5.9.0 +QT_FOLDER := archive OSG_VERSION := 3.5.5 OSGEARTH_VERSION := 2.8 @@ -99,8 +100,8 @@ OSGEARTH_VERSION := 2.8 ifeq ($(UNAME), Linux) ifeq ($(ARCH), x86_64) QT_SDK_ARCH := gcc_64 - QT_SDK_URL := http://download.qt.io/official_releases/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/qt-opensource-linux-x64-$(QT_VERSION).run - QT_SDK_MD5_URL := http://download.qt.io/official_releases/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/md5sums.txt + QT_SDK_URL := http://download.qt.io/$(QT_FOLDER)/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/qt-opensource-linux-x64-$(QT_VERSION).run + QT_SDK_MD5_URL := http://download.qt.io/$(QT_FOLDER)/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/md5sums.txt OSG_URL := $(TOOLS_URL)/osg-$(OSG_VERSION)-linux-x64.tar.gz OSGEARTH_URL := $(TOOLS_URL)/osgearth-$(OSGEARTH_VERSION)-linux-x64.tar.gz else @@ -110,8 +111,8 @@ ifeq ($(UNAME), Linux) DOXYGEN_URL := $(TOOLS_URL)/doxygen-1.8.3.1.src.tar.gz else ifeq ($(UNAME), Darwin) QT_SDK_ARCH := clang_64 - QT_SDK_URL := http://download.qt.io/official_releases/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/qt-opensource-mac-x64-$(QT_VERSION).dmg - QT_SDK_MD5_URL := http://download.qt.io/official_releases/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/md5sums.txt + QT_SDK_URL := http://download.qt.io/$(QT_FOLDER)/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/qt-opensource-mac-x64-$(QT_VERSION).dmg + QT_SDK_MD5_URL := http://download.qt.io/$(QT_FOLDER)/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/md5sums.txt QT_SDK_MOUNT_DIR := /Volumes/qt-opensource-mac-x64-$(QT_VERSION) QT_SDK_MAINTENANCE_TOOL := /Volumes/qt-opensource-mac-x64-$(QT_VERSION)/qt-opensource-mac-x64-$(QT_VERSION).app/Contents/MacOS/qt-opensource-mac-x64-$(QT_VERSION) UNCRUSTIFY_URL := $(TOOLS_URL)/uncrustify-0.60.tar.gz @@ -120,8 +121,8 @@ else ifeq ($(UNAME), Darwin) OSGEARTH_URL := $(TOOLS_URL)/osgearth-$(OSGEARTH_VERSION)-clang_64.tar.gz else ifeq ($(UNAME), Windows) QT_SDK_ARCH := mingw53_32 - QT_SDK_URL := http://download.qt.io/official_releases/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/qt-opensource-windows-x86-mingw530-$(QT_VERSION).exe - QT_SDK_MD5_URL := http://download.qt.io/official_releases/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/md5sums.txt + QT_SDK_URL := http://download.qt.io/$(QT_FOLDER)/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/qt-opensource-windows-x86-mingw530-$(QT_VERSION).exe + QT_SDK_MD5_URL := http://download.qt.io/$(QT_FOLDER)/qt/$(QT_SHORT_VERSION)/$(QT_VERSION)/md5sums.txt NSIS_URL := $(TOOLS_URL)/nsis-2.46-unicode.tar.bz2 MESAWIN_URL := $(TOOLS_URL)/mesawin.tar.gz UNCRUSTIFY_URL := $(TOOLS_URL)/uncrustify-0.60-windows.tar.bz2 diff --git a/shared/uavobjectdefinition/airspeedsettings.xml b/shared/uavobjectdefinition/airspeedsettings.xml index 8f52ac9fa..94b45e492 100644 --- a/shared/uavobjectdefinition/airspeedsettings.xml +++ b/shared/uavobjectdefinition/airspeedsettings.xml @@ -4,7 +4,7 @@ - + diff --git a/shared/uavobjectdefinition/frskysporttelemetrysettings.xml b/shared/uavobjectdefinition/frskysporttelemetrysettings.xml new file mode 100644 index 000000000..2cd55c85b --- /dev/null +++ b/shared/uavobjectdefinition/frskysporttelemetrysettings.xml @@ -0,0 +1,12 @@ + + + FrSKY S.Port Telemetry configuration. + + + + + + + + + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index ae1098a5a..d3305074f 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:PPM+FrSkySPort:Telemetry:DebugConsole:ComBridge:MSP:MAVLink:GPS:HoTT Telemetry:IBus:FrskySensorHub:FrSkySPort;"/> + + @@ -35,6 +35,7 @@ + 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 diff --git a/tool_install.sh b/tool_install.sh index 670b35730..3a41fddbc 100755 --- a/tool_install.sh +++ b/tool_install.sh @@ -191,7 +191,7 @@ function download_and_verify fi elif [ -n "${tool_md5:-}" ] then - if [[ "$tool_md5"* != "$(cd "$downloads_dir" && md5sum "$downloaded_file")" ]] + if [[ "${tool_md5:-} -" != "$(cd "$downloads_dir" && md5sum <"$downloaded_file")" ]] then mv -f "$downloaded_file"{,.rej} && \ verified=false