From 1e32ccb1d0af96afc42d9849e657215ef3d71ac5 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 8 Apr 2016 14:04:33 +0200 Subject: [PATCH 01/23] LP-291 Initial port --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 846 ++++++++++++++++++ .../boards/revolution/firmware/Makefile | 1 + .../boards/revolution/firmware/pios_board.c | 10 + flight/targets/boards/revolution/pios_board.h | 2 + shared/uavobjectdefinition/hwsettings.xml | 5 +- 5 files changed, 862 insertions(+), 2 deletions(-) create mode 100644 flight/modules/UAVOMSPBridge/UAVOMSPBridge.c diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c new file mode 100644 index 000000000..1c660e32c --- /dev/null +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -0,0 +1,846 @@ +/** + ****************************************************************************** + * @addtogroup TauLabsModules TauLabs Modules + * @{ + * @addtogroup UAVOMSPBridge UAVO to MSP Bridge Module + * @{ + * + * @file uavomspbridge.c + * @author Tau Labs, http://taulabs.org, Copyright (C) 2015 + * @author dRonin, http://dronin.org Copyright (C) 2015-2016 + * @brief Bridges selected UAVObjects to MSP for MWOSD and the like. + * + * @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 + * + * Additional note on redistribution: The copyright and license notices above + * must be maintained in each individual source file that is a derivative work + * of this source file; otherwise redistribution is prohibited. + */ + +#include "openpilot.h" +//#include "physical_constants.h" +#include "receiverstatus.h" +#include "hwsettings.h" +#include "flightbatterysettings.h" +#include "flightbatterystate.h" +#include "gpspositionsensor.h" +#include "manualcontrolcommand.h" +#include "accessorydesired.h" +#include "attitudestate.h" +#include "airspeedstate.h" +#include "actuatorsettings.h" +#include "actuatordesired.h" +#include "flightstatus.h" +#include "systemstats.h" +#include "systemalarms.h" +#include "homelocation.h" +#include "barosensor.h" +#include "stabilizationdesired.h" + +//#include "pios_thread.h" +#include "pios_sensors.h" + + +#define WGS84_RADIUS_EARTH_KM 6371.008f // Earth's radius in km +#define PI 3.14159265358979323846f // [-] + +#define PIOS_INCLUDE_MSP_BRIDGE + +#if defined(PIOS_INCLUDE_MSP_BRIDGE) + +#define MSP_SENSOR_ACC (1 << 0) +#define MSP_SENSOR_BARO (1 << 1) +#define MSP_SENSOR_MAG (1 << 2) +#define MSP_SENSOR_GPS (1 << 3) +#define MSP_SENSOR_SONAR (1 << 4) + +// Magic numbers copied from mwosd +#define MSP_IDENT 100 // multitype + multiwii version + protocol version + capability variable +#define MSP_STATUS 101 // cycletime & errors_count & sensor present & box activation & current setting number +#define MSP_RAW_IMU 102 // 9 DOF +#define MSP_SERVO 103 // 8 servos +#define MSP_MOTOR 104 // 8 motors +#define MSP_RC 105 // 8 rc chan and more +#define MSP_RAW_GPS 106 // fix, numsat, lat, lon, alt, speed, ground course +#define MSP_COMP_GPS 107 // distance home, direction home +#define MSP_ATTITUDE 108 // 2 angles 1 heading +#define MSP_ALTITUDE 109 // altitude, variometer +#define MSP_ANALOG 110 // vbat, powermetersum, rssi if available on RX +#define MSP_RC_TUNING 111 // rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define MSP_PID 112 // P I D coeff (9 are used currently) +#define MSP_BOX 113 // BOX setup (number is dependant of your setup) +#define MSP_MISC 114 // powermeter trig +#define MSP_MOTOR_PINS 115 // which pins are in use for motors & servos, for GUI +#define MSP_BOXNAMES 116 // the aux switch names +#define MSP_PIDNAMES 117 // the PID names +#define MSP_BOXIDS 119 // get the permanent IDs associated to BOXes +#define MSP_NAV_STATUS 121 // Returns navigation status +#define MSP_CELLS 130 // FrSky SPort Telemtry +#define MSP_ALARMS 242 // Alarm request + +typedef enum { + MSP_BOX_ARM, + MSP_BOX_ANGLE, + MSP_BOX_HORIZON, + MSP_BOX_BARO, + MSP_BOX_VARIO, + MSP_BOX_MAG, + MSP_BOX_GPSHOME, + MSP_BOX_GPSHOLD, + MSP_BOX_LAST, +} msp_box_t; + +static const struct { + msp_box_t mode; + uint8_t mwboxid; + FlightStatusFlightModeOptions flightMode; + StabilizationDesiredStabilizationModeOptions thrustMode; +} msp_boxes[] = { + { MSP_BOX_ARM, 0, 0, 0xff}, + { MSP_BOX_ANGLE, 1, 0, 0xff}, + { MSP_BOX_HORIZON, 2, 0, 0xff}, + { MSP_BOX_BARO, 3, 0, STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD }, + { MSP_BOX_VARIO, 4, 0, STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO }, + { MSP_BOX_MAG, 5, 0, 0xff}, + { MSP_BOX_GPSHOME, 10, FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE, 0xff}, + { MSP_BOX_GPSHOLD, 11, FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD, 0xff}, + { MSP_BOX_LAST, 0xff, 0, 0xff}, +}; + +typedef enum { + MSP_IDLE, + MSP_HEADER_START, + MSP_HEADER_M, + MSP_HEADER_SIZE, + MSP_HEADER_CMD, + MSP_FILLBUF, + MSP_CHECKSUM, + MSP_DISCARD, + MSP_MAYBE_UAVTALK2, + MSP_MAYBE_UAVTALK3, + MSP_MAYBE_UAVTALK4, + MSP_MAYBE_UAVTALK_SLOW2, + MSP_MAYBE_UAVTALK_SLOW3, + MSP_MAYBE_UAVTALK_SLOW4, + MSP_MAYBE_UAVTALK_SLOW5, + MSP_MAYBE_UAVTALK_SLOW6 +} msp_state; + +#define MSP_ANALOG_VOLTAGE (1 << 0) +#define MSP_ANALOG_CURRENT (1 << 1) + +struct msp_bridge { + uintptr_t com; + + uint8_t sensors; + uint8_t analog; + + msp_state state; + uint8_t cmd_size; + uint8_t cmd_id; + uint8_t cmd_i; + uint8_t checksum; + union { + uint8_t data[0]; + // Specific packed data structures go here. + } cmd_data; +}; + +#if defined(PIOS_MSP_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_MSP_STACK_SIZE +#else +#define STACK_SIZE_BYTES 768 +#endif +#define TASK_PRIORITY (tskIDLE_PRIORITY) + +#define MAX_ALARM_LEN 30 + +#define BOOT_DISPLAY_TIME_MS (10*1000) + +static bool module_enabled = false; +static struct msp_bridge *msp; +static int32_t uavoMSPBridgeInitialize(void); +static void uavoMSPBridgeTask(void *parameters); + +static void msp_send(struct msp_bridge *m, uint8_t cmd, const uint8_t *data, size_t len) +{ + uint8_t buf[5]; + uint8_t cs = (uint8_t)(len) ^ cmd; + + buf[0] = '$'; + buf[1] = 'M'; + buf[2] = '>'; + buf[3] = (uint8_t)(len); + buf[4] = cmd; + + PIOS_COM_SendBuffer(m->com, buf, sizeof(buf)); + PIOS_COM_SendBuffer(m->com, data, len); + + for (unsigned i = 0; i < len; i++) { + cs ^= data[i]; + } + cs ^= 0; + + buf[0] = cs; + PIOS_COM_SendBuffer(m->com, buf, 1); +} + +static msp_state msp_state_size(struct msp_bridge *m, uint8_t b) +{ + m->cmd_size = b; + m->checksum = b; + return MSP_HEADER_CMD; +} + +static msp_state msp_state_cmd(struct msp_bridge *m, uint8_t b) +{ + m->cmd_i = 0; + m->cmd_id = b; + m->checksum ^= m->cmd_id; + + if (m->cmd_size > sizeof(m->cmd_data)) { + // Too large a body. Let's ignore it. + return MSP_DISCARD; + } + + return m->cmd_size == 0 ? MSP_CHECKSUM : MSP_FILLBUF; +} + +static msp_state msp_state_fill_buf(struct msp_bridge *m, uint8_t b) +{ + m->cmd_data.data[m->cmd_i++] = b; + m->checksum ^= b; + return m->cmd_i == m->cmd_size ? MSP_CHECKSUM : MSP_FILLBUF; +} + +static void msp_send_attitude(struct msp_bridge *m) +{ + union { + uint8_t buf[0]; + struct { + int16_t x; + int16_t y; + int16_t h; + } att; + } data; + AttitudeStateData attState; + + AttitudeStateGet(&attState); + + // Roll and Pitch are in 10ths of a degree. + data.att.x = attState.Roll * 10; + data.att.y = attState.Pitch * -10; + // Yaw is just -180 -> 180 + data.att.h = attState.Yaw; + + msp_send(m, MSP_ATTITUDE, data.buf, sizeof(data)); +} + +static void msp_send_status(struct msp_bridge *m) +{ + union { + uint8_t buf[0]; + struct { + uint16_t cycleTime; + uint16_t i2cErrors; + uint16_t sensors; + uint32_t flags; + uint8_t setting; + } __attribute__((packed)) status; + } data; + // TODO: https://github.com/TauLabs/TauLabs/blob/next/shared/uavobjectdefinition/actuatordesired.xml#L8 + data.status.cycleTime = 0; + data.status.i2cErrors = 0; + + data.status.sensors = m->sensors; + + if(GPSPositionSensorHandle() != NULL) + { + GPSPositionSensorStatusOptions gpsStatus; + GPSPositionSensorStatusGet(&gpsStatus); + + if(gpsStatus != GPSPOSITIONSENSOR_STATUS_NOGPS) + { + data.status.sensors |= MSP_SENSOR_GPS; + } + } + + data.status.flags = 0; + data.status.setting = 0; + + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + StabilizationDesiredStabilizationModeData stabModeData; + StabilizationDesiredStabilizationModeGet(&stabModeData); + + data.status.flags = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; // activate "armed" box. + + for (int i = 1; msp_boxes[i].mode != MSP_BOX_LAST; i++) { + if ( (flightStatus.FlightMode == msp_boxes[i].flightMode) + || (stabModeData.Thrust == msp_boxes[i].thrustMode) ) { + data.status.flags |= (1 << i); + } + } + + msp_send(m, MSP_STATUS, data.buf, sizeof(data)); +} + +static void msp_send_analog(struct msp_bridge *m) +{ + union { + uint8_t buf[0]; + struct { + uint8_t vbat; + uint16_t powerMeterSum; + uint16_t rssi; + uint16_t current; + } __attribute__((packed)) status; + } data; + + data.status.powerMeterSum = 0; + + if(FlightBatteryStateHandle() != NULL) { + FlightBatteryStateData batState; + FlightBatteryStateGet(&batState); + + if(m->analog & MSP_ANALOG_VOLTAGE) { + data.status.vbat = (uint8_t)lroundf(batState.Voltage * 10); + } + if(m->analog & MSP_ANALOG_CURRENT) { + data.status.current = lroundf(batState.Current * 100); + data.status.powerMeterSum = lroundf(batState.ConsumedEnergy); + } + } + + uint8_t quality; + ReceiverStatusQualityGet(&quality); + + // MSP RSSI's range is 0-1023 + + data.status.rssi = (quality * 1023) / 100; + + if(data.status.rssi > 1023) { + data.status.rssi = 1023; + } + + msp_send(m, MSP_ANALOG, data.buf, sizeof(data)); +} + +static void msp_send_ident(__attribute__((unused)) struct msp_bridge *m) +{ + // TODO +} + +static void msp_send_raw_gps(struct msp_bridge *m) +{ + union { + uint8_t buf[0]; + struct { + uint8_t fix; // 0 or 1 + uint8_t num_sat; + int32_t lat; // 1 / 10 000 000 deg + int32_t lon; // 1 / 10 000 000 deg + uint16_t alt; // meter + uint16_t speed; // cm/s + int16_t ground_course; // degree * 10 + } __attribute__((packed)) raw_gps; + } data; + + GPSPositionSensorData gps_data; + + if (GPSPositionSensorHandle() != NULL) + { + GPSPositionSensorGet(&gps_data); + + data.raw_gps.fix = (gps_data.Status >= GPSPOSITIONSENSOR_STATUS_FIX2D ? 1 : 0); // Data will display on OSD if 2D fix or better + data.raw_gps.num_sat = gps_data.Satellites; + data.raw_gps.lat = gps_data.Latitude; + data.raw_gps.lon = gps_data.Longitude; + data.raw_gps.alt = (uint16_t)gps_data.Altitude; + data.raw_gps.speed = (uint16_t)gps_data.Groundspeed; + data.raw_gps.ground_course = (int16_t)(gps_data.Heading * 10.0f); + + msp_send(m, MSP_RAW_GPS, data.buf, sizeof(data)); + + } +} + +static void msp_send_comp_gps(struct msp_bridge *m) +{ + union { + uint8_t buf[0]; + struct { + uint16_t distance_to_home; // meter + int16_t direction_to_home; // degree [-180:180] + uint8_t home_position_valid; // 0 = Invalid, Dronin MSP specific + } __attribute__((packed)) comp_gps; + } data; + + GPSPositionSensorData gps_data; + HomeLocationData home_data; + + if ((GPSPositionSensorHandle() == NULL) || (HomeLocationHandle() == NULL)) + { + data.comp_gps.distance_to_home = 0; + data.comp_gps.direction_to_home = 0; + data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD + } + else + { + GPSPositionSensorGet(&gps_data); + HomeLocationGet(&home_data); + + if((gps_data.Status < GPSPOSITIONSENSOR_STATUS_FIX2D) || (home_data.Set == HOMELOCATION_SET_FALSE)) + { + data.comp_gps.distance_to_home = 0; + data.comp_gps.direction_to_home = 0; + data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD + } + else + { + data.comp_gps.home_position_valid = 1; // Home distance and direction will display on OSD + + int32_t delta_lon = (home_data.Longitude - gps_data.Longitude); // degrees * 1e7 + int32_t delta_lat = (home_data.Latitude - gps_data.Latitude ); // degrees * 1e7 + + float delta_y = DEG2RAD( (float)delta_lon * WGS84_RADIUS_EARTH_KM ); // KM * 1e7 + float delta_x = DEG2RAD( (float)delta_lat * WGS84_RADIUS_EARTH_KM ); // KM * 1e7 + + delta_y *= cosf( DEG2RAD((float)home_data.Latitude * 1e-7f) ); // Latitude compression correction + + data.comp_gps.distance_to_home = (uint16_t)(sqrtf(delta_x * delta_x + delta_y * delta_y) * 1e-4f); // meters + + if ((delta_lon == 0) && (delta_lat == 0)) + data.comp_gps.direction_to_home = 0; + else + data.comp_gps.direction_to_home = RAD2DEG((int16_t)(atan2f(delta_y, delta_x))); // degrees; + } + + msp_send(m, MSP_COMP_GPS, data.buf, sizeof(data)); + } + + +} + +static void msp_send_altitude(struct msp_bridge *m) +{ + union { + uint8_t buf[0]; + struct { + int32_t alt; // cm + uint16_t vario; // cm/s + } __attribute__((packed)) baro; + } data; + + + if (BaroSensorHandle() != NULL) + { + BaroSensorData baro; + BaroSensorGet(&baro); + + data.baro.alt = (int32_t)roundf(baro.Altitude * 100.0f); + + msp_send(m, MSP_ALTITUDE, data.buf, sizeof(data)); + } +} + +// Scale stick values whose input range is -1 to 1 to MSP's expected +// PWM range of 1000-2000. +static uint16_t msp_scale_rc(float percent) { + return percent*500 + 1500; +} + +// Throttle maps to 1100-1900 and a bit differently as -1 == 1000 and +// then jumps immediately to 0 -> 1 for the rest of the range. MWOSD +// can learn ranges as wide as they are sent, but defaults to +// 1100-1900 so the throttle indicator will vary for the same stick +// position unless we send it what it wants right away. +static uint16_t msp_scale_rc_thr(float percent) { + return percent <= 0 ? 1100 : percent*800 + 1100; +} + +// MSP RC order is Roll/Pitch/Yaw/Throttle/AUX1/AUX2/AUX3/AUX4 +static void msp_send_channels(struct msp_bridge *m) +{ + AccessoryDesiredData acc0, acc1, acc2; + ManualControlCommandData manualState; + ManualControlCommandGet(&manualState); + AccessoryDesiredInstGet(0, &acc0); + AccessoryDesiredInstGet(1, &acc1); + AccessoryDesiredInstGet(2, &acc2); + + union { + uint8_t buf[0]; + uint16_t channels[8]; + } data = { + .channels = { + msp_scale_rc(manualState.Roll), + msp_scale_rc(manualState.Pitch * -1), // TL pitch is backwards + msp_scale_rc(manualState.Yaw), + msp_scale_rc_thr(manualState.Throttle), + msp_scale_rc(acc0.AccessoryVal), + msp_scale_rc(acc1.AccessoryVal), + msp_scale_rc(acc2.AccessoryVal), + 1000, // no aux4 + } + }; + + msp_send(m, MSP_RC, data.buf, sizeof(data)); +} + +static void msp_send_boxids(struct msp_bridge *m) { + uint8_t boxes[MSP_BOX_LAST]; + int len = 0; + + for (int i = 0; msp_boxes[i].mode != MSP_BOX_LAST; i++) { + boxes[len++] = msp_boxes[i].mwboxid; + } + msp_send(m, MSP_BOXIDS, boxes, len); +} + +#define ALARM_OK 0 +#define ALARM_WARN 1 +#define ALARM_ERROR 2 +#define ALARM_CRIT 3 + +#define MS2TICKS(m) ((m) / (portTICK_RATE_MS)) + +static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m) { +#if 0 + union { + uint8_t buf[0]; + struct { + uint8_t state; + char msg[MAX_ALARM_LEN]; + } __attribute__((packed)) alarm; + } data; + + SystemAlarmsData alarm; + SystemAlarmsGet(&alarm); + + // Special case early boot times -- just report boot reason + + + if (xTaskGetTickCount() < MS2TICKS(10*1000)) { + data.alarm.state = ALARM_CRIT; + const char *boot_reason = AlarmBootReason(alarm.RebootCause); + strncpy((char*)data.alarm.msg, boot_reason, MAX_ALARM_LEN); + data.alarm.msg[MAX_ALARM_LEN-1] = '\0'; + msp_send(m, MSP_ALARMS, data.buf, strlen((char*)data.alarm.msg)+1); + return; + } + + uint8_t state; + data.alarm.state = ALARM_OK; + int32_t len = AlarmString(&alarm, data.alarm.msg, + sizeof(data.alarm.msg), false, &state); + switch (state) { + case SYSTEMALARMS_ALARM_WARNING: + data.alarm.state = ALARM_WARN; + break; + case SYSTEMALARMS_ALARM_ERROR: + break; + case SYSTEMALARMS_ALARM_CRITICAL: + data.alarm.state = ALARM_CRIT;; + } + + msp_send(m, MSP_ALARMS, data.buf, len+1); +#endif +} + +static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b) +{ + if ((m->checksum ^ b) != 0) { + return MSP_IDLE; + } + + // Respond to interesting things. + switch (m->cmd_id) { + case MSP_IDENT: + msp_send_ident(m); + break; + case MSP_RAW_GPS: + msp_send_raw_gps(m); + break; + case MSP_COMP_GPS: + msp_send_comp_gps(m); + break; + case MSP_ALTITUDE: + msp_send_altitude(m); + break; + case MSP_ATTITUDE: + msp_send_attitude(m); + break; + case MSP_STATUS: + msp_send_status(m); + break; + case MSP_ANALOG: + msp_send_analog(m); + break; + case MSP_RC: + msp_send_channels(m); + break; + case MSP_BOXIDS: + msp_send_boxids(m); + break; + case MSP_ALARMS: + msp_send_alarms(m); + break; + } + return MSP_IDLE; +} + +static msp_state msp_state_discard(struct msp_bridge *m, __attribute__((unused)) uint8_t b) +{ + return m->cmd_i++ == m->cmd_size ? MSP_IDLE : MSP_DISCARD; +} + +/** + * Process incoming bytes from an MSP query thing. + * @param[in] b received byte + * @return true if we should continue processing bytes + */ +static bool msp_receive_byte(struct msp_bridge *m, uint8_t b) +{ + switch (m->state) { + case MSP_IDLE: + switch (b) { + case 0xe0: // uavtalk matching first part of 0x3c @ 57600 baud + m->state = MSP_MAYBE_UAVTALK_SLOW2; + break; + case '<': // uavtalk matching with 0x3c 0x2x 0xxx 0x0x + m->state = MSP_MAYBE_UAVTALK2; + break; + case '$': + m->state = MSP_HEADER_START; + break; + default: + m->state = MSP_IDLE; + } + break; + case MSP_HEADER_START: + m->state = b == 'M' ? MSP_HEADER_M : MSP_IDLE; + break; + case MSP_HEADER_M: + m->state = b == '<' ? MSP_HEADER_SIZE : MSP_IDLE; + break; + case MSP_HEADER_SIZE: + m->state = msp_state_size(m, b); + break; + case MSP_HEADER_CMD: + m->state = msp_state_cmd(m, b); + break; + case MSP_FILLBUF: + m->state = msp_state_fill_buf(m, b); + break; + case MSP_CHECKSUM: + m->state = msp_state_checksum(m, b); + break; + case MSP_DISCARD: + m->state = msp_state_discard(m, b); + break; + case MSP_MAYBE_UAVTALK2: + // e.g. 3c 20 1d 00 + // second possible uavtalk byte + m->state = (b&0xf0) == 0x20 ? MSP_MAYBE_UAVTALK3 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK3: + // third possible uavtalk byte can be anything + m->state = MSP_MAYBE_UAVTALK4; + break; + case MSP_MAYBE_UAVTALK4: + m->state = MSP_IDLE; + // If this looks like the fourth possible uavtalk byte, we're done + if ((b & 0xf0) == 0) { + PIOS_COM_TELEM_RF = m->com; + return false; + } + break; + case MSP_MAYBE_UAVTALK_SLOW2: + m->state = b == 0x18 ? MSP_MAYBE_UAVTALK_SLOW3 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK_SLOW3: + m->state = b == 0x98 ? MSP_MAYBE_UAVTALK_SLOW4 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK_SLOW4: + m->state = b == 0x7e ? MSP_MAYBE_UAVTALK_SLOW5 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK_SLOW5: + m->state = b == 0x00 ? MSP_MAYBE_UAVTALK_SLOW6 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK_SLOW6: + m->state = MSP_IDLE; + // If this looks like the sixth possible 57600 baud uavtalk byte, we're done + if(b == 0x60) { + PIOS_COM_ChangeBaud(m->com, 57600); + PIOS_COM_TELEM_RF = m->com; + return false; + } + break; + } + + return true; +} + +/** + * Module start routine automatically called after initialization routine + * @return 0 when was successful + */ +static int32_t uavoMSPBridgeStart(void) +{ + if (!module_enabled) { + // give port to telemetry if it doesn't have one + // stops board getting stuck in condition where it can't be connected to gcs + if(!PIOS_COM_TELEM_RF) + PIOS_COM_TELEM_RF = pios_com_msp_id; + + return -1; + } + + xTaskHandle taskHandle; + + xTaskCreate(uavoMSPBridgeTask, "uavoMSPBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); +// PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOMSPBRIDGE, taskHandle); + + return 0; +} + +static uint32_t hwsettings_mspspeed_enum_to_baud(uint8_t baud) +{ + switch (baud) { + case HWSETTINGS_MSPSPEED_2400: + return 2400; + + case HWSETTINGS_MSPSPEED_4800: + return 4800; + + default: + case HWSETTINGS_MSPSPEED_9600: + return 9600; + + case HWSETTINGS_MSPSPEED_19200: + return 19200; + + case HWSETTINGS_MSPSPEED_38400: + return 38400; + + case HWSETTINGS_MSPSPEED_57600: + return 57600; + + case HWSETTINGS_MSPSPEED_115200: + return 115200; + } +} + + +/** + * Module initialization routine + * @return 0 when initialization was successful + */ +static int32_t uavoMSPBridgeInitialize(void) +{ + if (pios_com_msp_id) { + msp = pios_malloc(sizeof(*msp)); + if (msp != NULL) { + memset(msp, 0x00, sizeof(*msp)); + + msp->com = pios_com_msp_id; + + // now figure out enabled features: registered sensors, ADC routing, GPS + + if( PIOS_SENSORS_GetInstanceByType( 0, PIOS_SENSORS_TYPE_3AXIS_ACCEL ) ) + { + msp->sensors |= MSP_SENSOR_ACC; + } + if( PIOS_SENSORS_GetInstanceByType( 0, PIOS_SENSORS_TYPE_1AXIS_BARO ) ) + { + msp->sensors |= MSP_SENSOR_BARO; + } + if( PIOS_SENSORS_GetInstanceByType( 0, PIOS_SENSORS_TYPE_3AXIS_MAG|PIOS_SENSORS_TYPE_3AXIS_AUXMAG ) ) + { + msp->sensors |= MSP_SENSOR_MAG; + } +#ifdef PIOS_SENSORS_TYPE_1AXIS_SONAR + if( PIOS_SENSORS_GetInstanceByType( 0, PIOS_SENSORS_TYPE_1AXIS_SONAR ) ) + { + msp->sensors |= MSP_SENSOR_SONAR; + } +#endif + + // MAP_SENSOR_GPS is hot-pluggable type + + HwSettingsADCRoutingDataArray adcRoutingDataArray; + HwSettingsADCRoutingArrayGet(adcRoutingDataArray.array); + + for(unsigned i = 0; i < sizeof(adcRoutingDataArray.array)/sizeof(adcRoutingDataArray.array[0]); ++i) + { + switch( adcRoutingDataArray.array[i] ) + { + case HWSETTINGS_ADCROUTING_BATTERYVOLTAGE: + msp->analog |= MSP_ANALOG_VOLTAGE; + break; + case HWSETTINGS_ADCROUTING_BATTERYCURRENT: + msp->analog |= MSP_ANALOG_CURRENT; + break; + default:; + } + } + + + HwSettingsMSPSpeedOptions mspSpeed; + HwSettingsMSPSpeedGet(&mspSpeed); + + PIOS_COM_ChangeBaud(pios_com_msp_id, hwsettings_mspspeed_enum_to_baud(mspSpeed)); + + module_enabled = true; + return 0; + } + } + + return -1; +} +MODULE_INITCALL(uavoMSPBridgeInitialize, uavoMSPBridgeStart); + +/** + * Main task routine + * @param[in] parameters parameter given by PIOS_Thread_Create() + */ +static void uavoMSPBridgeTask(__attribute__((unused)) void *parameters) +{ + while (1) { + uint8_t b = 0; + uint16_t count = PIOS_COM_ReceiveBuffer(msp->com, &b, 1, ~0); + if (count) { + if (!msp_receive_byte(msp, b)) { + // Returning is considered risky here as + // that's unusual and this is an edge case. + while (1) { + PIOS_DELAY_WaitmS(60*1000); + } + } + } + } +} + +#endif //PIOS_INCLUDE_MSP_BRIDGE +/** + * @} + * @} + */ diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 1c73f4845..2327c2d71 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -53,6 +53,7 @@ MODULES += Telemetry MODULES += Notify OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 018d7a397..61cc31674 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -255,6 +255,9 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_HKOSD_RX_BUF_LEN 22 #define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_MSP_TX_BUF_LEN 128 +#define PIOS_COM_MSP_RX_BUF_LEN 64 + #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 uint32_t pios_com_debug_id; @@ -268,6 +271,7 @@ uint32_t pios_com_bridge_id = 0; uint32_t pios_com_overo_id = 0; uint32_t pios_com_hkosd_id = 0; uint32_t pios_com_vcp_id = 0; +uint32_t pios_com_msp_id = 0; #if defined(PIOS_INCLUDE_RFM22B) uint32_t pios_rfm22b_id = 0; @@ -547,6 +551,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RM_FLEXIPORT_MSP: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; case HWSETTINGS_RM_FLEXIPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; @@ -811,6 +818,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RM_MAINPORT_MSP: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; case HWSETTINGS_RM_MAINPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 5195be8a2..29892fe53 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -148,6 +148,7 @@ extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_hkosd_id; +extern uint32_t pios_com_msp_id; #define PIOS_COM_GPS (pios_com_gps_id) #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) @@ -155,6 +156,7 @@ extern uint32_t pios_com_hkosd_id; #define PIOS_COM_BRIDGE (pios_com_bridge_id) #define PIOS_COM_VCP (pios_com_vcp_id) #define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_MSP (pios_com_msp_id) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) extern uint32_t pios_com_debug_id; diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f79cb6496..afab93170 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -14,11 +14,12 @@ - - + + + From 6b5e43a57d493c8e0106b4f2e9b91a64dc61d3bb Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 8 Apr 2016 14:20:20 +0200 Subject: [PATCH 02/23] LP-291 Changed default baudrate to 115200 --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 2 +- shared/uavobjectdefinition/hwsettings.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 1c660e32c..de0245cd0 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -731,7 +731,6 @@ static uint32_t hwsettings_mspspeed_enum_to_baud(uint8_t baud) case HWSETTINGS_MSPSPEED_4800: return 4800; - default: case HWSETTINGS_MSPSPEED_9600: return 9600; @@ -744,6 +743,7 @@ static uint32_t hwsettings_mspspeed_enum_to_baud(uint8_t baud) case HWSETTINGS_MSPSPEED_57600: return 57600; + default: case HWSETTINGS_MSPSPEED_115200: return 115200; } diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index afab93170..88030cc72 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -19,7 +19,7 @@ - + From d8bdd8bfcf9a680558653241c94bc1204684e87d Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Tue, 19 Apr 2016 23:23:06 +0200 Subject: [PATCH 03/23] LP-291 Added TaskInfo entry for UAVOMSPBridge task --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 4 ++-- shared/uavobjectdefinition/taskinfo.xml | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index de0245cd0..8b6e7970d 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -52,7 +52,7 @@ #include "homelocation.h" #include "barosensor.h" #include "stabilizationdesired.h" - +#include "taskinfo.h" //#include "pios_thread.h" #include "pios_sensors.h" @@ -717,7 +717,7 @@ static int32_t uavoMSPBridgeStart(void) xTaskHandle taskHandle; xTaskCreate(uavoMSPBridgeTask, "uavoMSPBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); -// PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOMSPBRIDGE, taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOMSPBRIDGE, taskHandle); return 0; } diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 893660676..b9facf626 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -31,6 +31,7 @@ GPS OSDGen + UAVOMSPBridge @@ -63,6 +64,7 @@ GPS OSDGen + UAVOMSPBridge @@ -99,6 +101,7 @@ GPS OSDGen + UAVOMSPBridge From db5807e54cdfe384b87508b20af9a77e362b71a1 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 22 Apr 2016 03:29:38 +0200 Subject: [PATCH 04/23] LP-291 Cleaned up flight modes and additional icons. Implemented MSP_PID message. --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 266 ++++++++++++++++--- 1 file changed, 224 insertions(+), 42 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 8b6e7970d..1980c5412 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -37,6 +37,7 @@ //#include "physical_constants.h" #include "receiverstatus.h" #include "hwsettings.h" +#include "flightmodesettings.h" #include "flightbatterysettings.h" #include "flightbatterystate.h" #include "gpspositionsensor.h" @@ -53,6 +54,13 @@ #include "barosensor.h" #include "stabilizationdesired.h" #include "taskinfo.h" +#include "stabilizationsettings.h" +#include "stabilizationbank.h" +#include "stabilizationsettingsbank1.h" +#include "stabilizationsettingsbank2.h" +#include "stabilizationsettingsbank3.h" +#include "magstate.h" + //#include "pios_thread.h" #include "pios_sensors.h" @@ -83,7 +91,7 @@ #define MSP_ALTITUDE 109 // altitude, variometer #define MSP_ANALOG 110 // vbat, powermetersum, rssi if available on RX #define MSP_RC_TUNING 111 // rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID -#define MSP_PID 112 // P I D coeff (9 are used currently) +#define MSP_PID 112 // P I D coeff (10 are used currently) #define MSP_BOX 113 // BOX setup (number is dependant of your setup) #define MSP_MISC 114 // powermeter trig #define MSP_MOTOR_PINS 115 // which pins are in use for motors & servos, for GUI @@ -94,33 +102,70 @@ #define MSP_CELLS 130 // FrSky SPort Telemtry #define MSP_ALARMS 242 // Alarm request -typedef enum { - MSP_BOX_ARM, - MSP_BOX_ANGLE, - MSP_BOX_HORIZON, - MSP_BOX_BARO, - MSP_BOX_VARIO, - MSP_BOX_MAG, - MSP_BOX_GPSHOME, - MSP_BOX_GPSHOLD, - MSP_BOX_LAST, -} msp_box_t; +#define MSP_SET_PID 202 // set P I D coeff -static const struct { - msp_box_t mode; - uint8_t mwboxid; - FlightStatusFlightModeOptions flightMode; - StabilizationDesiredStabilizationModeOptions thrustMode; -} msp_boxes[] = { - { MSP_BOX_ARM, 0, 0, 0xff}, - { MSP_BOX_ANGLE, 1, 0, 0xff}, - { MSP_BOX_HORIZON, 2, 0, 0xff}, - { MSP_BOX_BARO, 3, 0, STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD }, - { MSP_BOX_VARIO, 4, 0, STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO }, - { MSP_BOX_MAG, 5, 0, 0xff}, - { MSP_BOX_GPSHOME, 10, FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE, 0xff}, - { MSP_BOX_GPSHOLD, 11, FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD, 0xff}, - { MSP_BOX_LAST, 0xff, 0, 0xff}, +typedef enum { + MSP_BOX_ID_ARM, + MSP_BOX_ID_ANGLE, // mode.stable (attitude??) - [sensor icon] [ fligth mode icon ] + MSP_BOX_ID_HORIZON, // [sensor icon] [ flight mode icon ] + MSP_BOX_ID_BARO, // [sensor icon] + MSP_BOX_ID_VARIO, + MSP_BOX_ID_MAG, // [sensor icon] + MSP_BOX_ID_HEADFREE, + MSP_BOX_ID_HEADADJ, + MSP_BOX_ID_CAMSTAB, // [gimbal icon] + MSP_BOX_ID_CAMTRIG, + MSP_BOX_ID_GPSHOME, // [ flight mode icon ] + MSP_BOX_ID_GPSHOLD, // [ flight mode icon ] + MSP_BOX_ID_PASSTHRU, // [ flight mode icon ] + MSP_BOX_ID_BEEPER, + MSP_BOX_ID_LEDMAX, + MSP_BOX_ID_LEDLOW, + MSP_BOX_ID_LLIGHTS, // landing lights + MSP_BOX_ID_CALIB, + MSP_BOX_ID_GOVERNOR, + MSP_BOX_ID_OSD_SWITCH, // OSD on or off, maybe. + MSP_BOX_ID_GPSMISSION, // [ flight mode icon ] + MSP_BOX_ID_GPSLAND, // [ flight mode icon ] + + MSP_BOX_ID_AIR = 28, // this box will add AIRMODE icon to flight mode. + MSP_BOX_ID_ACROPLUS = 29, // this will add PLUS icon to ACRO. ACRO = !ANGLE && !HORIZON +} msp_boxid_t; + +enum +{ + MSP_STATUS_ARMED, + MSP_STATUS_FLIGHTMODE_STABILIZED, + MSP_STATUS_FLIGHTMODE_HORIZON, + MSP_STATUS_SENSOR_BARO, + MSP_STATUS_SENSOR_MAG, + MSP_STATUS_MISC_GIMBAL, + MSP_STATUS_FLIGHTMODE_GPSHOME, + MSP_STATUS_FLIGHTMODE_GPSHOLD, + MSP_STATUS_FLIGHTMODE_GPSMISSION, + MSP_STATUS_FLIGHTMODE_GPSLAND, + MSP_STATUS_FLIGHTMODE_PASSTHRU, + MSP_STATUS_OSD_SWITCH, + MSP_STATUS_ICON_AIRMODE, + MSP_STATUS_ICON_ACROPLUS, +}; + +static const uint8_t msp_boxes[] = +{ + [ MSP_STATUS_ARMED ] = MSP_BOX_ID_ARM, + [ MSP_STATUS_FLIGHTMODE_STABILIZED ] = MSP_BOX_ID_ANGLE, // flight mode + [ MSP_STATUS_FLIGHTMODE_HORIZON ] = MSP_BOX_ID_HORIZON, // flight mode + [ MSP_STATUS_SENSOR_BARO ] = MSP_BOX_ID_BARO, // sensor icon only + [ MSP_STATUS_SENSOR_MAG ] = MSP_BOX_ID_MAG, // sensor icon only + [ MSP_STATUS_MISC_GIMBAL ] = MSP_BOX_ID_CAMSTAB, // gimbal icon only + [ MSP_STATUS_FLIGHTMODE_GPSHOME ] = MSP_BOX_ID_GPSHOME, // flight mode + [ MSP_STATUS_FLIGHTMODE_GPSHOLD ] = MSP_BOX_ID_GPSHOLD, // flight mode + [ MSP_STATUS_FLIGHTMODE_GPSMISSION ] = MSP_BOX_ID_GPSMISSION, // flight mode + [ MSP_STATUS_FLIGHTMODE_GPSLAND ] = MSP_BOX_ID_GPSLAND, // flight mode + [ MSP_STATUS_FLIGHTMODE_PASSTHRU ] = MSP_BOX_ID_PASSTHRU, // flight mode + [ MSP_STATUS_OSD_SWITCH ] = MSP_BOX_ID_OSD_SWITCH, // switch OSD mode + [ MSP_STATUS_ICON_AIRMODE ] = MSP_BOX_ID_AIR, + [ MSP_STATUS_ICON_ACROPLUS ] = MSP_BOX_ID_ACROPLUS, }; typedef enum { @@ -142,6 +187,26 @@ typedef enum { MSP_MAYBE_UAVTALK_SLOW6 } msp_state; +typedef struct __attribute__((packed)) +{ + uint8_t P, I, D; +} msp_pid_t; + +typedef enum { + PIDROLL, + PIDPITCH, + PIDYAW, + PIDALT, + PIDPOS, + PIDPOSR, + PIDNAVR, + PIDLEVEL, + PIDMAG, + PIDVEL, + PID_ITEM_COUNT +} pidIndex_e; + + #define MSP_ANALOG_VOLTAGE (1 << 0) #define MSP_ANALOG_CURRENT (1 << 1) @@ -252,6 +317,8 @@ static void msp_send_attitude(struct msp_bridge *m) msp_send(m, MSP_ATTITUDE, data.buf, sizeof(data)); } +#define IS_STAB_MODE(d,m) ( ( (d).Roll == (m)) && ((d).Pitch == (m)) ) + static void msp_send_status(struct msp_bridge *m) { union { @@ -289,15 +356,85 @@ static void msp_send_status(struct msp_bridge *m) StabilizationDesiredStabilizationModeData stabModeData; StabilizationDesiredStabilizationModeGet(&stabModeData); - - data.status.flags = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; // activate "armed" box. - for (int i = 1; msp_boxes[i].mode != MSP_BOX_LAST; i++) { - if ( (flightStatus.FlightMode == msp_boxes[i].flightMode) - || (stabModeData.Thrust == msp_boxes[i].thrustMode) ) { - data.status.flags |= (1 << i); - } - } + if( flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ) + { + data.status.flags |= (1 << MSP_STATUS_ARMED); + } + + // flightmode + + if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ) + { + data.status.flags |= ( 1 << MSP_STATUS_FLIGHTMODE_GPSHOLD ); + } + else if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ) + { + data.status.flags |= ( 1 << MSP_STATUS_FLIGHTMODE_GPSHOME ); + } + else if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ) + { + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSMISSION); + } + else if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND ) + { + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSLAND); + } + else if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_MANUAL ) + { + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_PASSTHRU); + } + else + { // + // if Roll(Rate) && Pitch(Rate) => Acro + // if Roll(Acro+) && Pitch(Acro+) => Acro+ + // if Roll(Rattitude) && Pitch(Rattitude) => Horizon + // else => Stabilized + + if( IS_STAB_MODE( stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATE ) + || IS_STAB_MODE( stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER ) ) + { + // data.status.flags should not set any mode flags + } + else if( IS_STAB_MODE( stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO ) ) // this is Acro+ mode + { + data.status.flags |= ( 1 << MSP_STATUS_ICON_ACROPLUS ); + } + else if( IS_STAB_MODE( stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE ) ) + { + data.status.flags |= ( 1 << MSP_STATUS_FLIGHTMODE_HORIZON ); + } + else // looks like stabilized mode, whichever it is.. + { + data.status.flags |= ( 1 << MSP_STATUS_FLIGHTMODE_STABILIZED ); + } + } + + // sensors in use? + // flight mode HORIZON or STABILIZED will turn on Accelerometer icon. + // Barometer? + + if( ( stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD ) + || ( stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO ) ) + { + data.status.flags |= ( 1 << MSP_STATUS_SENSOR_BARO ); + } + + if( MagStateHandle() != NULL ) + { + MagStateSourceOptions magSource; + MagStateSourceGet(&magSource); + + if(magSource != MAGSTATE_SOURCE_INVALID) + { + data.status.flags |= ( 1 << MSP_STATUS_SENSOR_MAG ); + } + } + + if( flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE ) + { + data.status.flags |= ( 1 << MSP_STATUS_ICON_AIRMODE ); + } msp_send(m, MSP_STATUS, data.buf, sizeof(data)); } @@ -505,14 +642,55 @@ static void msp_send_channels(struct msp_bridge *m) msp_send(m, MSP_RC, data.buf, sizeof(data)); } -static void msp_send_boxids(struct msp_bridge *m) { - uint8_t boxes[MSP_BOX_LAST]; - int len = 0; +static void msp_send_boxids(struct msp_bridge *m) { // This is actually sending a map of MSP_STATUS.flag bits to BOX ids. + msp_send( m, MSP_BOXIDS, msp_boxes, sizeof( msp_boxes ) ); +} - for (int i = 0; msp_boxes[i].mode != MSP_BOX_LAST; i++) { - boxes[len++] = msp_boxes[i].mwboxid; - } - msp_send(m, MSP_BOXIDS, boxes, len); +static void msp_send_pid(struct msp_bridge *m) +{ + uint8_t fm; + ManualControlCommandFlightModeSwitchPositionGet(&fm); + + if( fm >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM ) + { + return; + } + + StabilizationSettingsFlightModeMapOptions flightModeMap[STABILIZATIONSETTINGS_FLIGHTMODEMAP_NUMELEM]; + StabilizationSettingsFlightModeMapGet(flightModeMap); + + StabilizationBankData bankData; + + switch(flightModeMap[fm]) + { + case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1: + StabilizationSettingsBank1Get( ((StabilizationSettingsBank1Data *) &bankData ) ); + break; + case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2: + StabilizationSettingsBank2Get( ((StabilizationSettingsBank2Data *) &bankData ) ); + break; + case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3: + StabilizationSettingsBank3Get( ((StabilizationSettingsBank3Data *) &bankData ) ); + break; + } + + msp_pid_t piditems[ PID_ITEM_COUNT ]; + + memset(piditems, 0, sizeof( piditems ) ); + + piditems[ PIDROLL ].P = bankData.RollRatePID.Kp * 10000; + piditems[ PIDROLL ].I = bankData.RollRatePID.Ki * 10000; + piditems[ PIDROLL ].D = bankData.RollRatePID.Kd * 10000; + + piditems[ PIDPITCH ].P = bankData.PitchRatePID.Kp * 10000; + piditems[ PIDPITCH ].I = bankData.PitchRatePID.Ki * 10000; + piditems[ PIDPITCH ].D = bankData.PitchRatePID.Kd * 10000; + + piditems[ PIDYAW ].P = bankData.YawRatePID.Kp * 10000; + piditems[ PIDYAW ].I = bankData.YawRatePID.Ki * 10000; + piditems[ PIDYAW ].D = bankData.YawRatePID.Kd * 10000; + + msp_send( m, MSP_PID, (const uint8_t *) piditems, sizeof( piditems ) ); } #define ALARM_OK 0 @@ -603,7 +781,11 @@ static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b) case MSP_ALARMS: msp_send_alarms(m); break; + case MSP_PID: + msp_send_pid(m); + break; } + return MSP_IDLE; } From d281d8ce0c8721bcd7c1e8027c4f26605effe779 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 20 Apr 2016 18:29:44 +0200 Subject: [PATCH 05/23] LP-291 Added MSP_SET_PID command. --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 105 ++++++++++++++----- 1 file changed, 81 insertions(+), 24 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 1980c5412..9a5d81e4b 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -224,6 +224,7 @@ struct msp_bridge { union { uint8_t data[0]; // Specific packed data structures go here. + msp_pid_t piditems[PID_ITEM_COUNT]; } cmd_data; }; @@ -255,11 +256,16 @@ static void msp_send(struct msp_bridge *m, uint8_t cmd, const uint8_t *data, siz buf[4] = cmd; PIOS_COM_SendBuffer(m->com, buf, sizeof(buf)); - PIOS_COM_SendBuffer(m->com, data, len); + + if(len > 0) + { + PIOS_COM_SendBuffer(m->com, data, len); + + for (unsigned i = 0; i < len; i++) { + cs ^= data[i]; + } + } - for (unsigned i = 0; i < len; i++) { - cs ^= data[i]; - } cs ^= 0; buf[0] = cs; @@ -646,53 +652,101 @@ static void msp_send_boxids(struct msp_bridge *m) { // This is actually sending msp_send( m, MSP_BOXIDS, msp_boxes, sizeof( msp_boxes ) ); } -static void msp_send_pid(struct msp_bridge *m) +static StabilizationSettingsFlightModeMapOptions get_current_stabilization_bank() { uint8_t fm; ManualControlCommandFlightModeSwitchPositionGet(&fm); if( fm >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM ) { - return; + return STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1; } StabilizationSettingsFlightModeMapOptions flightModeMap[STABILIZATIONSETTINGS_FLIGHTMODEMAP_NUMELEM]; StabilizationSettingsFlightModeMapGet(flightModeMap); - StabilizationBankData bankData; - - switch(flightModeMap[fm]) + return flightModeMap[fm]; +} + +static void get_current_stabilizationbankdata( StabilizationBankData *bankData ) +{ + switch( get_current_stabilization_bank() ) { case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1: - StabilizationSettingsBank1Get( ((StabilizationSettingsBank1Data *) &bankData ) ); + StabilizationSettingsBank1Get( (StabilizationSettingsBank1Data *) bankData ); break; case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2: - StabilizationSettingsBank2Get( ((StabilizationSettingsBank2Data *) &bankData ) ); + StabilizationSettingsBank2Get( (StabilizationSettingsBank2Data *) bankData ); break; case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3: - StabilizationSettingsBank3Get( ((StabilizationSettingsBank3Data *) &bankData ) ); + StabilizationSettingsBank3Get( (StabilizationSettingsBank3Data *) bankData ); break; } +} + +static void set_current_stabilizationbankdata( const StabilizationBankData *bankData ) +{ + // update just relevant parts. or not. + switch( get_current_stabilization_bank() ) + { + case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1: + StabilizationSettingsBank1Set( (const StabilizationSettingsBank1Data *) bankData ); + break; + case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2: + StabilizationSettingsBank2Set( (const StabilizationSettingsBank2Data *) bankData ); + break; + case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3: + StabilizationSettingsBank3Set( (const StabilizationSettingsBank3Data *) bankData ); + break; + } +} + +static void pid_native2msp( const float *native, msp_pid_t *piditem ) +{ + piditem->P = native[0] * 10000; + piditem->I = native[1] * 10000; + piditem->D = native[2] * 10000; +} + +static void pid_msp2native( const msp_pid_t *piditem, float *native ) +{ + native[0] = (float)piditem->P / 10000; + native[1] = (float)piditem->I / 10000; + native[2] = (float)piditem->D / 10000; +} + +static void msp_send_pid(struct msp_bridge *m) +{ + StabilizationBankData bankData; + + get_current_stabilizationbankdata( &bankData ); msp_pid_t piditems[ PID_ITEM_COUNT ]; memset(piditems, 0, sizeof( piditems ) ); - piditems[ PIDROLL ].P = bankData.RollRatePID.Kp * 10000; - piditems[ PIDROLL ].I = bankData.RollRatePID.Ki * 10000; - piditems[ PIDROLL ].D = bankData.RollRatePID.Kd * 10000; - - piditems[ PIDPITCH ].P = bankData.PitchRatePID.Kp * 10000; - piditems[ PIDPITCH ].I = bankData.PitchRatePID.Ki * 10000; - piditems[ PIDPITCH ].D = bankData.PitchRatePID.Kd * 10000; - - piditems[ PIDYAW ].P = bankData.YawRatePID.Kp * 10000; - piditems[ PIDYAW ].I = bankData.YawRatePID.Ki * 10000; - piditems[ PIDYAW ].D = bankData.YawRatePID.Kd * 10000; - + pid_native2msp( (float *) &bankData.RollRatePID, &piditems[ PIDROLL ] ); + pid_native2msp( (float *) &bankData.PitchRatePID, &piditems[ PIDPITCH ] ); + pid_native2msp( (float *) &bankData.YawRatePID, &piditems[ PIDYAW ] ); + msp_send( m, MSP_PID, (const uint8_t *) piditems, sizeof( piditems ) ); } +static void msp_set_pid(struct msp_bridge *m) +{ + StabilizationBankData bankData; + + get_current_stabilizationbankdata( &bankData ); + + pid_msp2native( &m->cmd_data.piditems[ PIDROLL ], (float *) &bankData.RollRatePID ); + pid_msp2native( &m->cmd_data.piditems[ PIDPITCH ], (float *) &bankData.PitchRatePID ); + pid_msp2native( &m->cmd_data.piditems[ PIDYAW ], (float *) &bankData.YawRatePID ); + + set_current_stabilizationbankdata( &bankData ); + + msp_send( m, MSP_SET_PID, 0, 0 ); // send ack. +} + #define ALARM_OK 0 #define ALARM_WARN 1 #define ALARM_ERROR 2 @@ -784,6 +838,9 @@ static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b) case MSP_PID: msp_send_pid(m); break; + case MSP_SET_PID: + msp_set_pid(m); + break; } return MSP_IDLE; From 1a870f0c0fb547c83586c3ba17bd305bac4f91be Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 20 Apr 2016 19:01:26 +0200 Subject: [PATCH 06/23] LP-291 Added proper float rounding before casting to uint8_t --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 9a5d81e4b..2468e6e9a 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -703,9 +703,9 @@ static void set_current_stabilizationbankdata( const StabilizationBankData *bank static void pid_native2msp( const float *native, msp_pid_t *piditem ) { - piditem->P = native[0] * 10000; - piditem->I = native[1] * 10000; - piditem->D = native[2] * 10000; + piditem->P = lroundf(native[0] * 10000); + piditem->I = lroundf(native[1] * 10000); + piditem->D = lroundf(native[2] * 10000); } static void pid_msp2native( const msp_pid_t *piditem, float *native ) From 365354ea745471bc4990347735d7a9ad940bc219 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 22 Apr 2016 00:22:07 +0200 Subject: [PATCH 07/23] LP-291 Changed AlarmString() arguments --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 2468e6e9a..50420ed3f 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -769,7 +769,7 @@ static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m) { // Special case early boot times -- just report boot reason - +#if 0 if (xTaskGetTickCount() < MS2TICKS(10*1000)) { data.alarm.state = ALARM_CRIT; const char *boot_reason = AlarmBootReason(alarm.RebootCause); @@ -778,11 +778,12 @@ static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m) { msp_send(m, MSP_ALARMS, data.buf, strlen((char*)data.alarm.msg)+1); return; } +#endif uint8_t state; data.alarm.state = ALARM_OK; int32_t len = AlarmString(&alarm, data.alarm.msg, - sizeof(data.alarm.msg), false, &state); + sizeof(data.alarm.msg), SYSTEMALARMS_ALARM_CRITICAL, &state); // Include only CRITICAL and ERROR switch (state) { case SYSTEMALARMS_ALARM_WARNING: data.alarm.state = ALARM_WARN; From f04d44de30ad5745fb424a6a263785046c9271d1 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 22 Apr 2016 03:19:51 +0200 Subject: [PATCH 08/23] LP-291 Added MSP support to all boards (revolution, revonano, revoproto, discoveryf4bare and coptercontrol) --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 1466 ++++++++--------- .../boards/coptercontrol/firmware/Makefile | 1 + .../coptercontrol/firmware/pios_board.c | 40 +- .../targets/boards/coptercontrol/pios_board.h | 3 + .../boards/discoveryf4bare/firmware/Makefile | 1 + .../discoveryf4bare/firmware/pios_board.c | 11 +- .../boards/discoveryf4bare/pios_board.h | 2 + .../boards/revolution/firmware/pios_board.c | 7 +- .../targets/boards/revonano/firmware/Makefile | 1 + .../boards/revonano/firmware/pios_board.c | 11 +- flight/targets/boards/revonano/pios_board.h | 2 + .../boards/revoproto/firmware/Makefile | 1 + .../boards/revoproto/firmware/pios_board.c | 18 + flight/targets/boards/revoproto/pios_board.h | 2 + shared/uavobjectdefinition/hwsettings.xml | 16 +- 15 files changed, 822 insertions(+), 760 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 50420ed3f..2bf87a772 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -6,6 +6,7 @@ * @{ * * @file uavomspbridge.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. * @author Tau Labs, http://taulabs.org, Copyright (C) 2015 * @author dRonin, http://dronin.org Copyright (C) 2015-2016 * @brief Bridges selected UAVObjects to MSP for MWOSD and the like. @@ -34,7 +35,6 @@ */ #include "openpilot.h" -//#include "physical_constants.h" #include "receiverstatus.h" #include "hwsettings.h" #include "flightmodesettings.h" @@ -61,136 +61,133 @@ #include "stabilizationsettingsbank3.h" #include "magstate.h" -//#include "pios_thread.h" #include "pios_sensors.h" -#define WGS84_RADIUS_EARTH_KM 6371.008f // Earth's radius in km -#define PI 3.14159265358979323846f // [-] +#define WGS84_RADIUS_EARTH_KM 6371.008f // Earth's radius in km +#define PI 3.14159265358979323846f // [-] #define PIOS_INCLUDE_MSP_BRIDGE #if defined(PIOS_INCLUDE_MSP_BRIDGE) -#define MSP_SENSOR_ACC (1 << 0) -#define MSP_SENSOR_BARO (1 << 1) -#define MSP_SENSOR_MAG (1 << 2) -#define MSP_SENSOR_GPS (1 << 3) +#define MSP_SENSOR_ACC (1 << 0) +#define MSP_SENSOR_BARO (1 << 1) +#define MSP_SENSOR_MAG (1 << 2) +#define MSP_SENSOR_GPS (1 << 3) #define MSP_SENSOR_SONAR (1 << 4) // Magic numbers copied from mwosd -#define MSP_IDENT 100 // multitype + multiwii version + protocol version + capability variable -#define MSP_STATUS 101 // cycletime & errors_count & sensor present & box activation & current setting number -#define MSP_RAW_IMU 102 // 9 DOF -#define MSP_SERVO 103 // 8 servos -#define MSP_MOTOR 104 // 8 motors -#define MSP_RC 105 // 8 rc chan and more -#define MSP_RAW_GPS 106 // fix, numsat, lat, lon, alt, speed, ground course -#define MSP_COMP_GPS 107 // distance home, direction home -#define MSP_ATTITUDE 108 // 2 angles 1 heading -#define MSP_ALTITUDE 109 // altitude, variometer -#define MSP_ANALOG 110 // vbat, powermetersum, rssi if available on RX -#define MSP_RC_TUNING 111 // rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID -#define MSP_PID 112 // P I D coeff (10 are used currently) -#define MSP_BOX 113 // BOX setup (number is dependant of your setup) -#define MSP_MISC 114 // powermeter trig -#define MSP_MOTOR_PINS 115 // which pins are in use for motors & servos, for GUI -#define MSP_BOXNAMES 116 // the aux switch names -#define MSP_PIDNAMES 117 // the PID names -#define MSP_BOXIDS 119 // get the permanent IDs associated to BOXes -#define MSP_NAV_STATUS 121 // Returns navigation status -#define MSP_CELLS 130 // FrSky SPort Telemtry -#define MSP_ALARMS 242 // Alarm request +#define MSP_IDENT 100 // multitype + multiwii version + protocol version + capability variable +#define MSP_STATUS 101 // cycletime & errors_count & sensor present & box activation & current setting number +#define MSP_RAW_IMU 102 // 9 DOF +#define MSP_SERVO 103 // 8 servos +#define MSP_MOTOR 104 // 8 motors +#define MSP_RC 105 // 8 rc chan and more +#define MSP_RAW_GPS 106 // fix, numsat, lat, lon, alt, speed, ground course +#define MSP_COMP_GPS 107 // distance home, direction home +#define MSP_ATTITUDE 108 // 2 angles 1 heading +#define MSP_ALTITUDE 109 // altitude, variometer +#define MSP_ANALOG 110 // vbat, powermetersum, rssi if available on RX +#define MSP_RC_TUNING 111 // rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define MSP_PID 112 // P I D coeff (10 are used currently) +#define MSP_BOX 113 // BOX setup (number is dependant of your setup) +#define MSP_MISC 114 // powermeter trig +#define MSP_MOTOR_PINS 115 // which pins are in use for motors & servos, for GUI +#define MSP_BOXNAMES 116 // the aux switch names +#define MSP_PIDNAMES 117 // the PID names +#define MSP_BOXIDS 119 // get the permanent IDs associated to BOXes +#define MSP_NAV_STATUS 121 // Returns navigation status +#define MSP_CELLS 130 // FrSky SPort Telemtry +#define MSP_ALARMS 242 // Alarm request -#define MSP_SET_PID 202 // set P I D coeff +#define MSP_SET_PID 202 // set P I D coeff typedef enum { - MSP_BOX_ID_ARM, - MSP_BOX_ID_ANGLE, // mode.stable (attitude??) - [sensor icon] [ fligth mode icon ] - MSP_BOX_ID_HORIZON, // [sensor icon] [ flight mode icon ] - MSP_BOX_ID_BARO, // [sensor icon] - MSP_BOX_ID_VARIO, - MSP_BOX_ID_MAG, // [sensor icon] - MSP_BOX_ID_HEADFREE, - MSP_BOX_ID_HEADADJ, - MSP_BOX_ID_CAMSTAB, // [gimbal icon] - MSP_BOX_ID_CAMTRIG, - MSP_BOX_ID_GPSHOME, // [ flight mode icon ] - MSP_BOX_ID_GPSHOLD, // [ flight mode icon ] - MSP_BOX_ID_PASSTHRU, // [ flight mode icon ] - MSP_BOX_ID_BEEPER, - MSP_BOX_ID_LEDMAX, - MSP_BOX_ID_LEDLOW, - MSP_BOX_ID_LLIGHTS, // landing lights - MSP_BOX_ID_CALIB, - MSP_BOX_ID_GOVERNOR, - MSP_BOX_ID_OSD_SWITCH, // OSD on or off, maybe. - MSP_BOX_ID_GPSMISSION, // [ flight mode icon ] - MSP_BOX_ID_GPSLAND, // [ flight mode icon ] - - MSP_BOX_ID_AIR = 28, // this box will add AIRMODE icon to flight mode. - MSP_BOX_ID_ACROPLUS = 29, // this will add PLUS icon to ACRO. ACRO = !ANGLE && !HORIZON + MSP_BOX_ID_ARM, + MSP_BOX_ID_ANGLE, // mode.stable (attitude??) - [sensor icon] [ fligth mode icon ] + MSP_BOX_ID_HORIZON, // [sensor icon] [ flight mode icon ] + MSP_BOX_ID_BARO, // [sensor icon] + MSP_BOX_ID_VARIO, + MSP_BOX_ID_MAG, // [sensor icon] + MSP_BOX_ID_HEADFREE, + MSP_BOX_ID_HEADADJ, + MSP_BOX_ID_CAMSTAB, // [gimbal icon] + MSP_BOX_ID_CAMTRIG, + MSP_BOX_ID_GPSHOME, // [ flight mode icon ] + MSP_BOX_ID_GPSHOLD, // [ flight mode icon ] + MSP_BOX_ID_PASSTHRU, // [ flight mode icon ] + MSP_BOX_ID_BEEPER, + MSP_BOX_ID_LEDMAX, + MSP_BOX_ID_LEDLOW, + MSP_BOX_ID_LLIGHTS, // landing lights + MSP_BOX_ID_CALIB, + MSP_BOX_ID_GOVERNOR, + MSP_BOX_ID_OSD_SWITCH, // OSD on or off, maybe. + MSP_BOX_ID_GPSMISSION, // [ flight mode icon ] + MSP_BOX_ID_GPSLAND, // [ flight mode icon ] + + MSP_BOX_ID_AIR = 28, // this box will add AIRMODE icon to flight mode. + MSP_BOX_ID_ACROPLUS = 29, // this will add PLUS icon to ACRO. ACRO = !ANGLE && !HORIZON } msp_boxid_t; -enum -{ - MSP_STATUS_ARMED, - MSP_STATUS_FLIGHTMODE_STABILIZED, - MSP_STATUS_FLIGHTMODE_HORIZON, - MSP_STATUS_SENSOR_BARO, - MSP_STATUS_SENSOR_MAG, - MSP_STATUS_MISC_GIMBAL, - MSP_STATUS_FLIGHTMODE_GPSHOME, - MSP_STATUS_FLIGHTMODE_GPSHOLD, - MSP_STATUS_FLIGHTMODE_GPSMISSION, - MSP_STATUS_FLIGHTMODE_GPSLAND, - MSP_STATUS_FLIGHTMODE_PASSTHRU, - MSP_STATUS_OSD_SWITCH, - MSP_STATUS_ICON_AIRMODE, - MSP_STATUS_ICON_ACROPLUS, +enum { + MSP_STATUS_ARMED, + MSP_STATUS_FLIGHTMODE_STABILIZED, + MSP_STATUS_FLIGHTMODE_HORIZON, + MSP_STATUS_SENSOR_BARO, + MSP_STATUS_SENSOR_MAG, + MSP_STATUS_MISC_GIMBAL, + MSP_STATUS_FLIGHTMODE_GPSHOME, + MSP_STATUS_FLIGHTMODE_GPSHOLD, + MSP_STATUS_FLIGHTMODE_GPSMISSION, + MSP_STATUS_FLIGHTMODE_GPSLAND, + MSP_STATUS_FLIGHTMODE_PASSTHRU, + MSP_STATUS_OSD_SWITCH, + MSP_STATUS_ICON_AIRMODE, + MSP_STATUS_ICON_ACROPLUS, }; -static const uint8_t msp_boxes[] = -{ - [ MSP_STATUS_ARMED ] = MSP_BOX_ID_ARM, - [ MSP_STATUS_FLIGHTMODE_STABILIZED ] = MSP_BOX_ID_ANGLE, // flight mode - [ MSP_STATUS_FLIGHTMODE_HORIZON ] = MSP_BOX_ID_HORIZON, // flight mode - [ MSP_STATUS_SENSOR_BARO ] = MSP_BOX_ID_BARO, // sensor icon only - [ MSP_STATUS_SENSOR_MAG ] = MSP_BOX_ID_MAG, // sensor icon only - [ MSP_STATUS_MISC_GIMBAL ] = MSP_BOX_ID_CAMSTAB, // gimbal icon only - [ MSP_STATUS_FLIGHTMODE_GPSHOME ] = MSP_BOX_ID_GPSHOME, // flight mode - [ MSP_STATUS_FLIGHTMODE_GPSHOLD ] = MSP_BOX_ID_GPSHOLD, // flight mode - [ MSP_STATUS_FLIGHTMODE_GPSMISSION ] = MSP_BOX_ID_GPSMISSION, // flight mode - [ MSP_STATUS_FLIGHTMODE_GPSLAND ] = MSP_BOX_ID_GPSLAND, // flight mode - [ MSP_STATUS_FLIGHTMODE_PASSTHRU ] = MSP_BOX_ID_PASSTHRU, // flight mode - [ MSP_STATUS_OSD_SWITCH ] = MSP_BOX_ID_OSD_SWITCH, // switch OSD mode - [ MSP_STATUS_ICON_AIRMODE ] = MSP_BOX_ID_AIR, - [ MSP_STATUS_ICON_ACROPLUS ] = MSP_BOX_ID_ACROPLUS, +static const uint8_t msp_boxes[] = { + [MSP_STATUS_ARMED] = MSP_BOX_ID_ARM, + [MSP_STATUS_FLIGHTMODE_STABILIZED] = MSP_BOX_ID_ANGLE, // flight mode + [MSP_STATUS_FLIGHTMODE_HORIZON] = MSP_BOX_ID_HORIZON, // flight mode + [MSP_STATUS_SENSOR_BARO] = MSP_BOX_ID_BARO, // sensor icon only + [MSP_STATUS_SENSOR_MAG] = MSP_BOX_ID_MAG, // sensor icon only + [MSP_STATUS_MISC_GIMBAL] = MSP_BOX_ID_CAMSTAB, // gimbal icon only + [MSP_STATUS_FLIGHTMODE_GPSHOME] = MSP_BOX_ID_GPSHOME, // flight mode + [MSP_STATUS_FLIGHTMODE_GPSHOLD] = MSP_BOX_ID_GPSHOLD, // flight mode + [MSP_STATUS_FLIGHTMODE_GPSMISSION] = MSP_BOX_ID_GPSMISSION, // flight mode + [MSP_STATUS_FLIGHTMODE_GPSLAND] = MSP_BOX_ID_GPSLAND, // flight mode + [MSP_STATUS_FLIGHTMODE_PASSTHRU] = MSP_BOX_ID_PASSTHRU, // flight mode + [MSP_STATUS_OSD_SWITCH] = MSP_BOX_ID_OSD_SWITCH, // switch OSD mode + [MSP_STATUS_ICON_AIRMODE] = MSP_BOX_ID_AIR, + [MSP_STATUS_ICON_ACROPLUS] = MSP_BOX_ID_ACROPLUS, }; typedef enum { - MSP_IDLE, - MSP_HEADER_START, - MSP_HEADER_M, - MSP_HEADER_SIZE, - MSP_HEADER_CMD, - MSP_FILLBUF, - MSP_CHECKSUM, - MSP_DISCARD, - MSP_MAYBE_UAVTALK2, - MSP_MAYBE_UAVTALK3, - MSP_MAYBE_UAVTALK4, - MSP_MAYBE_UAVTALK_SLOW2, - MSP_MAYBE_UAVTALK_SLOW3, - MSP_MAYBE_UAVTALK_SLOW4, - MSP_MAYBE_UAVTALK_SLOW5, - MSP_MAYBE_UAVTALK_SLOW6 + MSP_IDLE, + MSP_HEADER_START, + MSP_HEADER_M, + MSP_HEADER_SIZE, + MSP_HEADER_CMD, + MSP_FILLBUF, + MSP_CHECKSUM, + MSP_DISCARD, + MSP_MAYBE_UAVTALK2, + MSP_MAYBE_UAVTALK3, + MSP_MAYBE_UAVTALK4, + MSP_MAYBE_UAVTALK_SLOW2, + MSP_MAYBE_UAVTALK_SLOW3, + MSP_MAYBE_UAVTALK_SLOW4, + MSP_MAYBE_UAVTALK_SLOW5, + MSP_MAYBE_UAVTALK_SLOW6 } msp_state; -typedef struct __attribute__((packed)) -{ - uint8_t P, I, D; -} msp_pid_t; +typedef struct __attribute__((packed)) { + uint8_t P, I, D; +} +msp_pid_t; typedef enum { PIDROLL, @@ -206,38 +203,38 @@ typedef enum { PID_ITEM_COUNT } pidIndex_e; - + #define MSP_ANALOG_VOLTAGE (1 << 0) #define MSP_ANALOG_CURRENT (1 << 1) struct msp_bridge { - uintptr_t com; - - uint8_t sensors; - uint8_t analog; + uintptr_t com; - msp_state state; - uint8_t cmd_size; - uint8_t cmd_id; - uint8_t cmd_i; - uint8_t checksum; - union { - uint8_t data[0]; - // Specific packed data structures go here. - msp_pid_t piditems[PID_ITEM_COUNT]; - } cmd_data; + uint8_t sensors; + uint8_t analog; + + msp_state state; + uint8_t cmd_size; + uint8_t cmd_id; + uint8_t cmd_i; + uint8_t checksum; + union { + uint8_t data[0]; + // Specific packed data structures go here. + msp_pid_t piditems[PID_ITEM_COUNT]; + } cmd_data; }; #if defined(PIOS_MSP_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_MSP_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_MSP_STACK_SIZE #else -#define STACK_SIZE_BYTES 768 +#define STACK_SIZE_BYTES 768 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY) +#define TASK_PRIORITY (tskIDLE_PRIORITY) -#define MAX_ALARM_LEN 30 +#define MAX_ALARM_LEN 30 -#define BOOT_DISPLAY_TIME_MS (10*1000) +#define BOOT_DISPLAY_TIME_MS (10 * 1000) static bool module_enabled = false; static struct msp_bridge *msp; @@ -246,368 +243,339 @@ static void uavoMSPBridgeTask(void *parameters); static void msp_send(struct msp_bridge *m, uint8_t cmd, const uint8_t *data, size_t len) { - uint8_t buf[5]; - uint8_t cs = (uint8_t)(len) ^ cmd; + uint8_t buf[5]; + uint8_t cs = (uint8_t)(len) ^ cmd; - buf[0] = '$'; - buf[1] = 'M'; - buf[2] = '>'; - buf[3] = (uint8_t)(len); - buf[4] = cmd; + buf[0] = '$'; + buf[1] = 'M'; + buf[2] = '>'; + buf[3] = (uint8_t)(len); + buf[4] = cmd; - PIOS_COM_SendBuffer(m->com, buf, sizeof(buf)); - - if(len > 0) - { - PIOS_COM_SendBuffer(m->com, data, len); + PIOS_COM_SendBuffer(m->com, buf, sizeof(buf)); - for (unsigned i = 0; i < len; i++) { - cs ^= data[i]; - } + if (len > 0) { + PIOS_COM_SendBuffer(m->com, data, len); + + for (unsigned i = 0; i < len; i++) { + cs ^= data[i]; } + } - cs ^= 0; + cs ^= 0; - buf[0] = cs; - PIOS_COM_SendBuffer(m->com, buf, 1); + buf[0] = cs; + PIOS_COM_SendBuffer(m->com, buf, 1); } static msp_state msp_state_size(struct msp_bridge *m, uint8_t b) { - m->cmd_size = b; - m->checksum = b; - return MSP_HEADER_CMD; + m->cmd_size = b; + m->checksum = b; + return MSP_HEADER_CMD; } static msp_state msp_state_cmd(struct msp_bridge *m, uint8_t b) { - m->cmd_i = 0; - m->cmd_id = b; - m->checksum ^= m->cmd_id; + m->cmd_i = 0; + m->cmd_id = b; + m->checksum ^= m->cmd_id; - if (m->cmd_size > sizeof(m->cmd_data)) { - // Too large a body. Let's ignore it. - return MSP_DISCARD; - } + if (m->cmd_size > sizeof(m->cmd_data)) { + // Too large a body. Let's ignore it. + return MSP_DISCARD; + } - return m->cmd_size == 0 ? MSP_CHECKSUM : MSP_FILLBUF; + return m->cmd_size == 0 ? MSP_CHECKSUM : MSP_FILLBUF; } static msp_state msp_state_fill_buf(struct msp_bridge *m, uint8_t b) { - m->cmd_data.data[m->cmd_i++] = b; - m->checksum ^= b; - return m->cmd_i == m->cmd_size ? MSP_CHECKSUM : MSP_FILLBUF; + m->cmd_data.data[m->cmd_i++] = b; + m->checksum ^= b; + return m->cmd_i == m->cmd_size ? MSP_CHECKSUM : MSP_FILLBUF; } static void msp_send_attitude(struct msp_bridge *m) { - union { - uint8_t buf[0]; - struct { - int16_t x; - int16_t y; - int16_t h; - } att; - } data; - AttitudeStateData attState; + union { + uint8_t buf[0]; + struct { + int16_t x; + int16_t y; + int16_t h; + } att; + } data; + AttitudeStateData attState; - AttitudeStateGet(&attState); + AttitudeStateGet(&attState); - // Roll and Pitch are in 10ths of a degree. - data.att.x = attState.Roll * 10; - data.att.y = attState.Pitch * -10; - // Yaw is just -180 -> 180 - data.att.h = attState.Yaw; + // Roll and Pitch are in 10ths of a degree. + data.att.x = attState.Roll * 10; + data.att.y = attState.Pitch * -10; + // Yaw is just -180 -> 180 + data.att.h = attState.Yaw; - msp_send(m, MSP_ATTITUDE, data.buf, sizeof(data)); + msp_send(m, MSP_ATTITUDE, data.buf, sizeof(data)); } -#define IS_STAB_MODE(d,m) ( ( (d).Roll == (m)) && ((d).Pitch == (m)) ) +#define IS_STAB_MODE(d, m) (((d).Roll == (m)) && ((d).Pitch == (m))) static void msp_send_status(struct msp_bridge *m) { - union { - uint8_t buf[0]; - struct { - uint16_t cycleTime; - uint16_t i2cErrors; - uint16_t sensors; - uint32_t flags; - uint8_t setting; - } __attribute__((packed)) status; - } data; - // TODO: https://github.com/TauLabs/TauLabs/blob/next/shared/uavobjectdefinition/actuatordesired.xml#L8 - data.status.cycleTime = 0; - data.status.i2cErrors = 0; - - data.status.sensors = m->sensors; - - if(GPSPositionSensorHandle() != NULL) - { - GPSPositionSensorStatusOptions gpsStatus; - GPSPositionSensorStatusGet(&gpsStatus); - - if(gpsStatus != GPSPOSITIONSENSOR_STATUS_NOGPS) - { - data.status.sensors |= MSP_SENSOR_GPS; - } - } + union { + uint8_t buf[0]; + struct { + uint16_t cycleTime; + uint16_t i2cErrors; + uint16_t sensors; + uint32_t flags; + uint8_t setting; + } __attribute__((packed)) status; + } data; + // TODO: https://github.com/TauLabs/TauLabs/blob/next/shared/uavobjectdefinition/actuatordesired.xml#L8 + data.status.cycleTime = 0; + data.status.i2cErrors = 0; - data.status.flags = 0; - data.status.setting = 0; + data.status.sensors = m->sensors; - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - StabilizationDesiredStabilizationModeData stabModeData; - StabilizationDesiredStabilizationModeGet(&stabModeData); - - if( flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ) - { - data.status.flags |= (1 << MSP_STATUS_ARMED); - } - - // flightmode - - if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ) - { - data.status.flags |= ( 1 << MSP_STATUS_FLIGHTMODE_GPSHOLD ); - } - else if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ) - { - data.status.flags |= ( 1 << MSP_STATUS_FLIGHTMODE_GPSHOME ); - } - else if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ) - { - data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSMISSION); - } - else if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND ) - { - data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSLAND); - } - else if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_MANUAL ) - { - data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_PASSTHRU); - } - else - { // - // if Roll(Rate) && Pitch(Rate) => Acro - // if Roll(Acro+) && Pitch(Acro+) => Acro+ - // if Roll(Rattitude) && Pitch(Rattitude) => Horizon - // else => Stabilized - - if( IS_STAB_MODE( stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATE ) - || IS_STAB_MODE( stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER ) ) - { - // data.status.flags should not set any mode flags - } - else if( IS_STAB_MODE( stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO ) ) // this is Acro+ mode - { - data.status.flags |= ( 1 << MSP_STATUS_ICON_ACROPLUS ); - } - else if( IS_STAB_MODE( stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE ) ) - { - data.status.flags |= ( 1 << MSP_STATUS_FLIGHTMODE_HORIZON ); - } - else // looks like stabilized mode, whichever it is.. - { - data.status.flags |= ( 1 << MSP_STATUS_FLIGHTMODE_STABILIZED ); - } - } - - // sensors in use? - // flight mode HORIZON or STABILIZED will turn on Accelerometer icon. - // Barometer? - - if( ( stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD ) - || ( stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO ) ) - { - data.status.flags |= ( 1 << MSP_STATUS_SENSOR_BARO ); - } - - if( MagStateHandle() != NULL ) - { - MagStateSourceOptions magSource; - MagStateSourceGet(&magSource); - - if(magSource != MAGSTATE_SOURCE_INVALID) - { - data.status.flags |= ( 1 << MSP_STATUS_SENSOR_MAG ); - } - } - - if( flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE ) - { - data.status.flags |= ( 1 << MSP_STATUS_ICON_AIRMODE ); - } + if (GPSPositionSensorHandle() != NULL) { + GPSPositionSensorStatusOptions gpsStatus; + GPSPositionSensorStatusGet(&gpsStatus); - msp_send(m, MSP_STATUS, data.buf, sizeof(data)); + if (gpsStatus != GPSPOSITIONSENSOR_STATUS_NOGPS) { + data.status.sensors |= MSP_SENSOR_GPS; + } + } + + data.status.flags = 0; + data.status.setting = 0; + + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + StabilizationDesiredStabilizationModeData stabModeData; + StabilizationDesiredStabilizationModeGet(&stabModeData); + + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { + data.status.flags |= (1 << MSP_STATUS_ARMED); + } + + // flightmode + + if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) { + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSHOLD); + } else if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) { + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSHOME); + } else if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSMISSION); + } else if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSLAND); + } else if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_MANUAL) { + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_PASSTHRU); + } else { // + // if Roll(Rate) && Pitch(Rate) => Acro + // if Roll(Acro+) && Pitch(Acro+) => Acro+ + // if Roll(Rattitude) && Pitch(Rattitude) => Horizon + // else => Stabilized + + if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) + || IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER)) { + // data.status.flags should not set any mode flags + } else if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO)) { // this is Acro+ mode + data.status.flags |= (1 << MSP_STATUS_ICON_ACROPLUS); + } else if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE)) { + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_HORIZON); + } else { // looks like stabilized mode, whichever it is.. + data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_STABILIZED); + } + } + + // sensors in use? + // flight mode HORIZON or STABILIZED will turn on Accelerometer icon. + // Barometer? + + if ((stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) + || (stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO)) { + data.status.flags |= (1 << MSP_STATUS_SENSOR_BARO); + } + +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + if (MagStateHandle() != NULL) { + MagStateSourceOptions magSource; + MagStateSourceGet(&magSource); + + if (magSource != MAGSTATE_SOURCE_INVALID) { + data.status.flags |= (1 << MSP_STATUS_SENSOR_MAG); + } + } +#endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */ + + if (flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE) { + data.status.flags |= (1 << MSP_STATUS_ICON_AIRMODE); + } + + msp_send(m, MSP_STATUS, data.buf, sizeof(data)); } static void msp_send_analog(struct msp_bridge *m) { - union { - uint8_t buf[0]; - struct { - uint8_t vbat; - uint16_t powerMeterSum; - uint16_t rssi; - uint16_t current; - } __attribute__((packed)) status; - } data; + union { + uint8_t buf[0]; + struct { + uint8_t vbat; + uint16_t powerMeterSum; + uint16_t rssi; + uint16_t current; + } __attribute__((packed)) status; + } data; - data.status.powerMeterSum = 0; - - if(FlightBatteryStateHandle() != NULL) { - FlightBatteryStateData batState; - FlightBatteryStateGet(&batState); + memset(&data, 0, sizeof(data)); - if(m->analog & MSP_ANALOG_VOLTAGE) { - data.status.vbat = (uint8_t)lroundf(batState.Voltage * 10); - } - if(m->analog & MSP_ANALOG_CURRENT) { - data.status.current = lroundf(batState.Current * 100); - data.status.powerMeterSum = lroundf(batState.ConsumedEnergy); - } - } +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + if (FlightBatteryStateHandle() != NULL) { + FlightBatteryStateData batState; + FlightBatteryStateGet(&batState); - uint8_t quality; - ReceiverStatusQualityGet(&quality); - - // MSP RSSI's range is 0-1023 + if (m->analog & MSP_ANALOG_VOLTAGE) { + data.status.vbat = (uint8_t)lroundf(batState.Voltage * 10); + } + if (m->analog & MSP_ANALOG_CURRENT) { + data.status.current = lroundf(batState.Current * 100); + data.status.powerMeterSum = lroundf(batState.ConsumedEnergy); + } + } +#endif + uint8_t quality; + ReceiverStatusQualityGet(&quality); - data.status.rssi = (quality * 1023) / 100; - - if(data.status.rssi > 1023) { - data.status.rssi = 1023; - } + // MSP RSSI's range is 0-1023 - msp_send(m, MSP_ANALOG, data.buf, sizeof(data)); + data.status.rssi = (quality * 1023) / 100; + + if (data.status.rssi > 1023) { + data.status.rssi = 1023; + } + + msp_send(m, MSP_ANALOG, data.buf, sizeof(data)); } static void msp_send_ident(__attribute__((unused)) struct msp_bridge *m) { - // TODO + // TODO } static void msp_send_raw_gps(struct msp_bridge *m) { - union { - uint8_t buf[0]; - struct { - uint8_t fix; // 0 or 1 - uint8_t num_sat; - int32_t lat; // 1 / 10 000 000 deg - int32_t lon; // 1 / 10 000 000 deg - uint16_t alt; // meter - uint16_t speed; // cm/s - int16_t ground_course; // degree * 10 - } __attribute__((packed)) raw_gps; - } data; - - GPSPositionSensorData gps_data; - - if (GPSPositionSensorHandle() != NULL) - { - GPSPositionSensorGet(&gps_data); + union { + uint8_t buf[0]; + struct { + uint8_t fix; // 0 or 1 + uint8_t num_sat; + int32_t lat; // 1 / 10 000 000 deg + int32_t lon; // 1 / 10 000 000 deg + uint16_t alt; // meter + uint16_t speed; // cm/s + int16_t ground_course; // degree * 10 + } __attribute__((packed)) raw_gps; + } data; - data.raw_gps.fix = (gps_data.Status >= GPSPOSITIONSENSOR_STATUS_FIX2D ? 1 : 0); // Data will display on OSD if 2D fix or better - data.raw_gps.num_sat = gps_data.Satellites; - data.raw_gps.lat = gps_data.Latitude; - data.raw_gps.lon = gps_data.Longitude; - data.raw_gps.alt = (uint16_t)gps_data.Altitude; - data.raw_gps.speed = (uint16_t)gps_data.Groundspeed; - data.raw_gps.ground_course = (int16_t)(gps_data.Heading * 10.0f); - - msp_send(m, MSP_RAW_GPS, data.buf, sizeof(data)); - - } + GPSPositionSensorData gps_data; + + if (GPSPositionSensorHandle() != NULL) { + GPSPositionSensorGet(&gps_data); + + data.raw_gps.fix = (gps_data.Status >= GPSPOSITIONSENSOR_STATUS_FIX2D ? 1 : 0); // Data will display on OSD if 2D fix or better + data.raw_gps.num_sat = gps_data.Satellites; + data.raw_gps.lat = gps_data.Latitude; + data.raw_gps.lon = gps_data.Longitude; + data.raw_gps.alt = (uint16_t)gps_data.Altitude; + data.raw_gps.speed = (uint16_t)gps_data.Groundspeed; + data.raw_gps.ground_course = (int16_t)(gps_data.Heading * 10.0f); + + msp_send(m, MSP_RAW_GPS, data.buf, sizeof(data)); + } } +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES static void msp_send_comp_gps(struct msp_bridge *m) { - union { - uint8_t buf[0]; - struct { - uint16_t distance_to_home; // meter - int16_t direction_to_home; // degree [-180:180] - uint8_t home_position_valid; // 0 = Invalid, Dronin MSP specific - } __attribute__((packed)) comp_gps; - } data; - - GPSPositionSensorData gps_data; - HomeLocationData home_data; - - if ((GPSPositionSensorHandle() == NULL) || (HomeLocationHandle() == NULL)) - { - data.comp_gps.distance_to_home = 0; - data.comp_gps.direction_to_home = 0; - data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD - } - else - { - GPSPositionSensorGet(&gps_data); - HomeLocationGet(&home_data); - - if((gps_data.Status < GPSPOSITIONSENSOR_STATUS_FIX2D) || (home_data.Set == HOMELOCATION_SET_FALSE)) - { - data.comp_gps.distance_to_home = 0; - data.comp_gps.direction_to_home = 0; - data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD - } - else - { - data.comp_gps.home_position_valid = 1; // Home distance and direction will display on OSD - - int32_t delta_lon = (home_data.Longitude - gps_data.Longitude); // degrees * 1e7 - int32_t delta_lat = (home_data.Latitude - gps_data.Latitude ); // degrees * 1e7 - - float delta_y = DEG2RAD( (float)delta_lon * WGS84_RADIUS_EARTH_KM ); // KM * 1e7 - float delta_x = DEG2RAD( (float)delta_lat * WGS84_RADIUS_EARTH_KM ); // KM * 1e7 - - delta_y *= cosf( DEG2RAD((float)home_data.Latitude * 1e-7f) ); // Latitude compression correction - - data.comp_gps.distance_to_home = (uint16_t)(sqrtf(delta_x * delta_x + delta_y * delta_y) * 1e-4f); // meters - - if ((delta_lon == 0) && (delta_lat == 0)) - data.comp_gps.direction_to_home = 0; - else - data.comp_gps.direction_to_home = RAD2DEG((int16_t)(atan2f(delta_y, delta_x))); // degrees; - } + union { + uint8_t buf[0]; + struct { + uint16_t distance_to_home; // meter + int16_t direction_to_home; // degree [-180:180] + uint8_t home_position_valid; // 0 = Invalid, Dronin MSP specific + } __attribute__((packed)) comp_gps; + } data; - msp_send(m, MSP_COMP_GPS, data.buf, sizeof(data)); - } + GPSPositionSensorData gps_data; + HomeLocationData home_data; + if ((GPSPositionSensorHandle() == NULL) || (HomeLocationHandle() == NULL)) { + data.comp_gps.distance_to_home = 0; + data.comp_gps.direction_to_home = 0; + data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD + } else { + GPSPositionSensorGet(&gps_data); + HomeLocationGet(&home_data); + if ((gps_data.Status < GPSPOSITIONSENSOR_STATUS_FIX2D) || (home_data.Set == HOMELOCATION_SET_FALSE)) { + data.comp_gps.distance_to_home = 0; + data.comp_gps.direction_to_home = 0; + data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD + } else { + data.comp_gps.home_position_valid = 1; // Home distance and direction will display on OSD + + int32_t delta_lon = (home_data.Longitude - gps_data.Longitude); // degrees * 1e7 + int32_t delta_lat = (home_data.Latitude - gps_data.Latitude); // degrees * 1e7 + + float delta_y = DEG2RAD((float)delta_lon * WGS84_RADIUS_EARTH_KM); // KM * 1e7 + float delta_x = DEG2RAD((float)delta_lat * WGS84_RADIUS_EARTH_KM); // KM * 1e7 + + delta_y *= cosf(DEG2RAD((float)home_data.Latitude * 1e-7f)); // Latitude compression correction + + data.comp_gps.distance_to_home = (uint16_t)(sqrtf(delta_x * delta_x + delta_y * delta_y) * 1e-4f); // meters + + if ((delta_lon == 0) && (delta_lat == 0)) { + data.comp_gps.direction_to_home = 0; + } else { + data.comp_gps.direction_to_home = RAD2DEG((int16_t)(atan2f(delta_y, delta_x))); // degrees; + } + } + + msp_send(m, MSP_COMP_GPS, data.buf, sizeof(data)); + } } static void msp_send_altitude(struct msp_bridge *m) { - union { - uint8_t buf[0]; - struct { - int32_t alt; // cm - uint16_t vario; // cm/s - } __attribute__((packed)) baro; - } data; + union { + uint8_t buf[0]; + struct { + int32_t alt; // cm + uint16_t vario; // cm/s + } __attribute__((packed)) baro; + } data; - if (BaroSensorHandle() != NULL) - { - BaroSensorData baro; - BaroSensorGet(&baro); + if (BaroSensorHandle() != NULL) { + BaroSensorData baro; + BaroSensorGet(&baro); - data.baro.alt = (int32_t)roundf(baro.Altitude * 100.0f); + data.baro.alt = (int32_t)roundf(baro.Altitude * 100.0f); - msp_send(m, MSP_ALTITUDE, data.buf, sizeof(data)); - } + msp_send(m, MSP_ALTITUDE, data.buf, sizeof(data)); + } } +#endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */ + // Scale stick values whose input range is -1 to 1 to MSP's expected // PWM range of 1000-2000. -static uint16_t msp_scale_rc(float percent) { - return percent*500 + 1500; +static uint16_t msp_scale_rc(float percent) +{ + return percent * 500 + 1500; } // Throttle maps to 1100-1900 and a bit differently as -1 == 1000 and @@ -615,241 +583,245 @@ static uint16_t msp_scale_rc(float percent) { // can learn ranges as wide as they are sent, but defaults to // 1100-1900 so the throttle indicator will vary for the same stick // position unless we send it what it wants right away. -static uint16_t msp_scale_rc_thr(float percent) { - return percent <= 0 ? 1100 : percent*800 + 1100; +static uint16_t msp_scale_rc_thr(float percent) +{ + return percent <= 0 ? 1100 : percent *800 + 1100; } // MSP RC order is Roll/Pitch/Yaw/Throttle/AUX1/AUX2/AUX3/AUX4 static void msp_send_channels(struct msp_bridge *m) { - AccessoryDesiredData acc0, acc1, acc2; - ManualControlCommandData manualState; - ManualControlCommandGet(&manualState); - AccessoryDesiredInstGet(0, &acc0); - AccessoryDesiredInstGet(1, &acc1); - AccessoryDesiredInstGet(2, &acc2); + AccessoryDesiredData acc0, acc1, acc2; + ManualControlCommandData manualState; - union { - uint8_t buf[0]; - uint16_t channels[8]; - } data = { - .channels = { - msp_scale_rc(manualState.Roll), - msp_scale_rc(manualState.Pitch * -1), // TL pitch is backwards - msp_scale_rc(manualState.Yaw), - msp_scale_rc_thr(manualState.Throttle), - msp_scale_rc(acc0.AccessoryVal), - msp_scale_rc(acc1.AccessoryVal), - msp_scale_rc(acc2.AccessoryVal), - 1000, // no aux4 - } - }; + ManualControlCommandGet(&manualState); + AccessoryDesiredInstGet(0, &acc0); + AccessoryDesiredInstGet(1, &acc1); + AccessoryDesiredInstGet(2, &acc2); - msp_send(m, MSP_RC, data.buf, sizeof(data)); + union { + uint8_t buf[0]; + uint16_t channels[8]; + } data = { + .channels = { + msp_scale_rc(manualState.Roll), + msp_scale_rc(manualState.Pitch * -1), // TL pitch is backwards + msp_scale_rc(manualState.Yaw), + msp_scale_rc_thr(manualState.Throttle), + msp_scale_rc(acc0.AccessoryVal), + msp_scale_rc(acc1.AccessoryVal), + msp_scale_rc(acc2.AccessoryVal), + 1000, // no aux4 + } + }; + + msp_send(m, MSP_RC, data.buf, sizeof(data)); } -static void msp_send_boxids(struct msp_bridge *m) { // This is actually sending a map of MSP_STATUS.flag bits to BOX ids. - msp_send( m, MSP_BOXIDS, msp_boxes, sizeof( msp_boxes ) ); +static void msp_send_boxids(struct msp_bridge *m) // This is actually sending a map of MSP_STATUS.flag bits to BOX ids. +{ + msp_send(m, MSP_BOXIDS, msp_boxes, sizeof(msp_boxes)); } static StabilizationSettingsFlightModeMapOptions get_current_stabilization_bank() { - uint8_t fm; - ManualControlCommandFlightModeSwitchPositionGet(&fm); + uint8_t fm; - if( fm >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM ) - { - return STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1; - } - - StabilizationSettingsFlightModeMapOptions flightModeMap[STABILIZATIONSETTINGS_FLIGHTMODEMAP_NUMELEM]; - StabilizationSettingsFlightModeMapGet(flightModeMap); - - return flightModeMap[fm]; + ManualControlCommandFlightModeSwitchPositionGet(&fm); + + if (fm >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) { + return STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1; + } + + StabilizationSettingsFlightModeMapOptions flightModeMap[STABILIZATIONSETTINGS_FLIGHTMODEMAP_NUMELEM]; + StabilizationSettingsFlightModeMapGet(flightModeMap); + + return flightModeMap[fm]; } -static void get_current_stabilizationbankdata( StabilizationBankData *bankData ) +static void get_current_stabilizationbankdata(StabilizationBankData *bankData) { - switch( get_current_stabilization_bank() ) - { + switch (get_current_stabilization_bank()) { case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1: - StabilizationSettingsBank1Get( (StabilizationSettingsBank1Data *) bankData ); - break; + StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)bankData); + break; case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2: - StabilizationSettingsBank2Get( (StabilizationSettingsBank2Data *) bankData ); - break; + StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)bankData); + break; case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3: - StabilizationSettingsBank3Get( (StabilizationSettingsBank3Data *) bankData ); - break; - } + StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)bankData); + break; + } } -static void set_current_stabilizationbankdata( const StabilizationBankData *bankData ) +static void set_current_stabilizationbankdata(const StabilizationBankData *bankData) { - // update just relevant parts. or not. - switch( get_current_stabilization_bank() ) - { + // update just relevant parts. or not. + switch (get_current_stabilization_bank()) { case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1: - StabilizationSettingsBank1Set( (const StabilizationSettingsBank1Data *) bankData ); - break; + StabilizationSettingsBank1Set((const StabilizationSettingsBank1Data *)bankData); + break; case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2: - StabilizationSettingsBank2Set( (const StabilizationSettingsBank2Data *) bankData ); - break; + StabilizationSettingsBank2Set((const StabilizationSettingsBank2Data *)bankData); + break; case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3: - StabilizationSettingsBank3Set( (const StabilizationSettingsBank3Data *) bankData ); - break; - } + StabilizationSettingsBank3Set((const StabilizationSettingsBank3Data *)bankData); + break; + } } -static void pid_native2msp( const float *native, msp_pid_t *piditem ) +static void pid_native2msp(const float *native, msp_pid_t *piditem) { - piditem->P = lroundf(native[0] * 10000); - piditem->I = lroundf(native[1] * 10000); - piditem->D = lroundf(native[2] * 10000); + piditem->P = lroundf(native[0] * 10000); + piditem->I = lroundf(native[1] * 10000); + piditem->D = lroundf(native[2] * 10000); } -static void pid_msp2native( const msp_pid_t *piditem, float *native ) +static void pid_msp2native(const msp_pid_t *piditem, float *native) { - native[0] = (float)piditem->P / 10000; - native[1] = (float)piditem->I / 10000; - native[2] = (float)piditem->D / 10000; + native[0] = (float)piditem->P / 10000; + native[1] = (float)piditem->I / 10000; + native[2] = (float)piditem->D / 10000; } static void msp_send_pid(struct msp_bridge *m) { - StabilizationBankData bankData; - - get_current_stabilizationbankdata( &bankData ); - - msp_pid_t piditems[ PID_ITEM_COUNT ]; + StabilizationBankData bankData; - memset(piditems, 0, sizeof( piditems ) ); - - pid_native2msp( (float *) &bankData.RollRatePID, &piditems[ PIDROLL ] ); - pid_native2msp( (float *) &bankData.PitchRatePID, &piditems[ PIDPITCH ] ); - pid_native2msp( (float *) &bankData.YawRatePID, &piditems[ PIDYAW ] ); - - msp_send( m, MSP_PID, (const uint8_t *) piditems, sizeof( piditems ) ); + get_current_stabilizationbankdata(&bankData); + + msp_pid_t piditems[PID_ITEM_COUNT]; + + memset(piditems, 0, sizeof(piditems)); + + pid_native2msp((float *)&bankData.RollRatePID, &piditems[PIDROLL]); + pid_native2msp((float *)&bankData.PitchRatePID, &piditems[PIDPITCH]); + pid_native2msp((float *)&bankData.YawRatePID, &piditems[PIDYAW]); + + msp_send(m, MSP_PID, (const uint8_t *)piditems, sizeof(piditems)); } static void msp_set_pid(struct msp_bridge *m) { - StabilizationBankData bankData; - - get_current_stabilizationbankdata( &bankData ); - - pid_msp2native( &m->cmd_data.piditems[ PIDROLL ], (float *) &bankData.RollRatePID ); - pid_msp2native( &m->cmd_data.piditems[ PIDPITCH ], (float *) &bankData.PitchRatePID ); - pid_msp2native( &m->cmd_data.piditems[ PIDYAW ], (float *) &bankData.YawRatePID ); - - set_current_stabilizationbankdata( &bankData ); - - msp_send( m, MSP_SET_PID, 0, 0 ); // send ack. + StabilizationBankData bankData; + + get_current_stabilizationbankdata(&bankData); + + pid_msp2native(&m->cmd_data.piditems[PIDROLL], (float *)&bankData.RollRatePID); + pid_msp2native(&m->cmd_data.piditems[PIDPITCH], (float *)&bankData.PitchRatePID); + pid_msp2native(&m->cmd_data.piditems[PIDYAW], (float *)&bankData.YawRatePID); + + set_current_stabilizationbankdata(&bankData); + + msp_send(m, MSP_SET_PID, 0, 0); // send ack. } -#define ALARM_OK 0 -#define ALARM_WARN 1 +#define ALARM_OK 0 +#define ALARM_WARN 1 #define ALARM_ERROR 2 -#define ALARM_CRIT 3 +#define ALARM_CRIT 3 #define MS2TICKS(m) ((m) / (portTICK_RATE_MS)) -static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m) { +static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m) +{ #if 0 - union { - uint8_t buf[0]; - struct { - uint8_t state; - char msg[MAX_ALARM_LEN]; - } __attribute__((packed)) alarm; - } data; + union { + uint8_t buf[0]; + struct { + uint8_t state; + char msg[MAX_ALARM_LEN]; + } __attribute__((packed)) alarm; + } data; - SystemAlarmsData alarm; - SystemAlarmsGet(&alarm); + SystemAlarmsData alarm; + SystemAlarmsGet(&alarm); - // Special case early boot times -- just report boot reason + // Special case early boot times -- just report boot reason #if 0 - if (xTaskGetTickCount() < MS2TICKS(10*1000)) { - data.alarm.state = ALARM_CRIT; - const char *boot_reason = AlarmBootReason(alarm.RebootCause); - strncpy((char*)data.alarm.msg, boot_reason, MAX_ALARM_LEN); - data.alarm.msg[MAX_ALARM_LEN-1] = '\0'; - msp_send(m, MSP_ALARMS, data.buf, strlen((char*)data.alarm.msg)+1); - return; - } + if (xTaskGetTickCount() < MS2TICKS(10 * 1000)) { + data.alarm.state = ALARM_CRIT; + const char *boot_reason = AlarmBootReason(alarm.RebootCause); + strncpy((char *)data.alarm.msg, boot_reason, MAX_ALARM_LEN); + data.alarm.msg[MAX_ALARM_LEN - 1] = '\0'; + msp_send(m, MSP_ALARMS, data.buf, strlen((char *)data.alarm.msg) + 1); + return; + } #endif - uint8_t state; - data.alarm.state = ALARM_OK; - int32_t len = AlarmString(&alarm, data.alarm.msg, - sizeof(data.alarm.msg), SYSTEMALARMS_ALARM_CRITICAL, &state); // Include only CRITICAL and ERROR - switch (state) { - case SYSTEMALARMS_ALARM_WARNING: - data.alarm.state = ALARM_WARN; - break; - case SYSTEMALARMS_ALARM_ERROR: - break; - case SYSTEMALARMS_ALARM_CRITICAL: - data.alarm.state = ALARM_CRIT;; - } + uint8_t state; + data.alarm.state = ALARM_OK; + int32_t len = AlarmString(&alarm, data.alarm.msg, + sizeof(data.alarm.msg), SYSTEMALARMS_ALARM_CRITICAL, &state); // Include only CRITICAL and ERROR + switch (state) { + case SYSTEMALARMS_ALARM_WARNING: + data.alarm.state = ALARM_WARN; + break; + case SYSTEMALARMS_ALARM_ERROR: + break; + case SYSTEMALARMS_ALARM_CRITICAL: + data.alarm.state = ALARM_CRIT;; + } - msp_send(m, MSP_ALARMS, data.buf, len+1); -#endif + msp_send(m, MSP_ALARMS, data.buf, len + 1); +#endif /* if 0 */ } static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b) { - if ((m->checksum ^ b) != 0) { - return MSP_IDLE; - } + if ((m->checksum ^ b) != 0) { + return MSP_IDLE; + } - // Respond to interesting things. - switch (m->cmd_id) { - case MSP_IDENT: - msp_send_ident(m); - break; - case MSP_RAW_GPS: - msp_send_raw_gps(m); - break; - case MSP_COMP_GPS: - msp_send_comp_gps(m); - break; - case MSP_ALTITUDE: - msp_send_altitude(m); - break; - case MSP_ATTITUDE: - msp_send_attitude(m); - break; - case MSP_STATUS: - msp_send_status(m); - break; - case MSP_ANALOG: - msp_send_analog(m); - break; - case MSP_RC: - msp_send_channels(m); - break; - case MSP_BOXIDS: - msp_send_boxids(m); - break; - case MSP_ALARMS: - msp_send_alarms(m); - break; - case MSP_PID: - msp_send_pid(m); - break; - case MSP_SET_PID: - msp_set_pid(m); - break; - } - - return MSP_IDLE; + // Respond to interesting things. + switch (m->cmd_id) { + case MSP_IDENT: + msp_send_ident(m); + break; + case MSP_RAW_GPS: + msp_send_raw_gps(m); + break; +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + case MSP_COMP_GPS: + msp_send_comp_gps(m); + break; + case MSP_ALTITUDE: + msp_send_altitude(m); + break; +#endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */ + case MSP_ATTITUDE: + msp_send_attitude(m); + break; + case MSP_STATUS: + msp_send_status(m); + break; + case MSP_ANALOG: + msp_send_analog(m); + break; + case MSP_RC: + msp_send_channels(m); + break; + case MSP_BOXIDS: + msp_send_boxids(m); + break; + case MSP_ALARMS: + msp_send_alarms(m); + break; + case MSP_PID: + msp_send_pid(m); + break; + case MSP_SET_PID: + msp_set_pid(m); + break; + } + + return MSP_IDLE; } static msp_state msp_state_discard(struct msp_bridge *m, __attribute__((unused)) uint8_t b) { - return m->cmd_i++ == m->cmd_size ? MSP_IDLE : MSP_DISCARD; + return m->cmd_i++ == m->cmd_size ? MSP_IDLE : MSP_DISCARD; } /** @@ -859,84 +831,84 @@ static msp_state msp_state_discard(struct msp_bridge *m, __attribute__((unused)) */ static bool msp_receive_byte(struct msp_bridge *m, uint8_t b) { - switch (m->state) { - case MSP_IDLE: - switch (b) { - case 0xe0: // uavtalk matching first part of 0x3c @ 57600 baud - m->state = MSP_MAYBE_UAVTALK_SLOW2; - break; - case '<': // uavtalk matching with 0x3c 0x2x 0xxx 0x0x - m->state = MSP_MAYBE_UAVTALK2; - break; - case '$': - m->state = MSP_HEADER_START; - break; - default: - m->state = MSP_IDLE; - } - break; - case MSP_HEADER_START: - m->state = b == 'M' ? MSP_HEADER_M : MSP_IDLE; - break; - case MSP_HEADER_M: - m->state = b == '<' ? MSP_HEADER_SIZE : MSP_IDLE; - break; - case MSP_HEADER_SIZE: - m->state = msp_state_size(m, b); - break; - case MSP_HEADER_CMD: - m->state = msp_state_cmd(m, b); - break; - case MSP_FILLBUF: - m->state = msp_state_fill_buf(m, b); - break; - case MSP_CHECKSUM: - m->state = msp_state_checksum(m, b); - break; - case MSP_DISCARD: - m->state = msp_state_discard(m, b); - break; - case MSP_MAYBE_UAVTALK2: - // e.g. 3c 20 1d 00 - // second possible uavtalk byte - m->state = (b&0xf0) == 0x20 ? MSP_MAYBE_UAVTALK3 : MSP_IDLE; - break; - case MSP_MAYBE_UAVTALK3: - // third possible uavtalk byte can be anything - m->state = MSP_MAYBE_UAVTALK4; - break; - case MSP_MAYBE_UAVTALK4: - m->state = MSP_IDLE; - // If this looks like the fourth possible uavtalk byte, we're done - if ((b & 0xf0) == 0) { - PIOS_COM_TELEM_RF = m->com; - return false; - } - break; - case MSP_MAYBE_UAVTALK_SLOW2: - m->state = b == 0x18 ? MSP_MAYBE_UAVTALK_SLOW3 : MSP_IDLE; - break; - case MSP_MAYBE_UAVTALK_SLOW3: - m->state = b == 0x98 ? MSP_MAYBE_UAVTALK_SLOW4 : MSP_IDLE; - break; - case MSP_MAYBE_UAVTALK_SLOW4: - m->state = b == 0x7e ? MSP_MAYBE_UAVTALK_SLOW5 : MSP_IDLE; - break; - case MSP_MAYBE_UAVTALK_SLOW5: - m->state = b == 0x00 ? MSP_MAYBE_UAVTALK_SLOW6 : MSP_IDLE; - break; - case MSP_MAYBE_UAVTALK_SLOW6: - m->state = MSP_IDLE; - // If this looks like the sixth possible 57600 baud uavtalk byte, we're done - if(b == 0x60) { - PIOS_COM_ChangeBaud(m->com, 57600); - PIOS_COM_TELEM_RF = m->com; - return false; - } - break; - } + switch (m->state) { + case MSP_IDLE: + switch (b) { + case 0xe0: // uavtalk matching first part of 0x3c @ 57600 baud + m->state = MSP_MAYBE_UAVTALK_SLOW2; + break; + case '<': // uavtalk matching with 0x3c 0x2x 0xxx 0x0x + m->state = MSP_MAYBE_UAVTALK2; + break; + case '$': + m->state = MSP_HEADER_START; + break; + default: + m->state = MSP_IDLE; + } + break; + case MSP_HEADER_START: + m->state = b == 'M' ? MSP_HEADER_M : MSP_IDLE; + break; + case MSP_HEADER_M: + m->state = b == '<' ? MSP_HEADER_SIZE : MSP_IDLE; + break; + case MSP_HEADER_SIZE: + m->state = msp_state_size(m, b); + break; + case MSP_HEADER_CMD: + m->state = msp_state_cmd(m, b); + break; + case MSP_FILLBUF: + m->state = msp_state_fill_buf(m, b); + break; + case MSP_CHECKSUM: + m->state = msp_state_checksum(m, b); + break; + case MSP_DISCARD: + m->state = msp_state_discard(m, b); + break; + case MSP_MAYBE_UAVTALK2: + // e.g. 3c 20 1d 00 + // second possible uavtalk byte + m->state = (b & 0xf0) == 0x20 ? MSP_MAYBE_UAVTALK3 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK3: + // third possible uavtalk byte can be anything + m->state = MSP_MAYBE_UAVTALK4; + break; + case MSP_MAYBE_UAVTALK4: + m->state = MSP_IDLE; + // If this looks like the fourth possible uavtalk byte, we're done + if ((b & 0xf0) == 0) { + PIOS_COM_TELEM_RF = m->com; + return false; + } + break; + case MSP_MAYBE_UAVTALK_SLOW2: + m->state = b == 0x18 ? MSP_MAYBE_UAVTALK_SLOW3 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK_SLOW3: + m->state = b == 0x98 ? MSP_MAYBE_UAVTALK_SLOW4 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK_SLOW4: + m->state = b == 0x7e ? MSP_MAYBE_UAVTALK_SLOW5 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK_SLOW5: + m->state = b == 0x00 ? MSP_MAYBE_UAVTALK_SLOW6 : MSP_IDLE; + break; + case MSP_MAYBE_UAVTALK_SLOW6: + m->state = MSP_IDLE; + // If this looks like the sixth possible 57600 baud uavtalk byte, we're done + if (b == 0x60) { + PIOS_COM_ChangeBaud(m->com, 57600); + PIOS_COM_TELEM_RF = m->com; + return false; + } + break; + } - return true; + return true; } /** @@ -945,21 +917,22 @@ static bool msp_receive_byte(struct msp_bridge *m, uint8_t b) */ static int32_t uavoMSPBridgeStart(void) { - if (!module_enabled) { - // give port to telemetry if it doesn't have one - // stops board getting stuck in condition where it can't be connected to gcs - if(!PIOS_COM_TELEM_RF) - PIOS_COM_TELEM_RF = pios_com_msp_id; + if (!module_enabled) { + // give port to telemetry if it doesn't have one + // stops board getting stuck in condition where it can't be connected to gcs + if (!PIOS_COM_TELEM_RF) { + PIOS_COM_TELEM_RF = pios_com_msp_id; + } - return -1; - } + return -1; + } - xTaskHandle taskHandle; - - xTaskCreate(uavoMSPBridgeTask, "uavoMSPBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOMSPBRIDGE, taskHandle); + xTaskHandle taskHandle; - return 0; + xTaskCreate(uavoMSPBridgeTask, "uavoMSPBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOMSPBRIDGE, taskHandle); + + return 0; } static uint32_t hwsettings_mspspeed_enum_to_baud(uint8_t baud) @@ -996,65 +969,64 @@ static uint32_t hwsettings_mspspeed_enum_to_baud(uint8_t baud) */ static int32_t uavoMSPBridgeInitialize(void) { - if (pios_com_msp_id) { - msp = pios_malloc(sizeof(*msp)); - if (msp != NULL) { - memset(msp, 0x00, sizeof(*msp)); + if (pios_com_msp_id) { + msp = pios_malloc(sizeof(*msp)); + if (msp != NULL) { + memset(msp, 0x00, sizeof(*msp)); - msp->com = pios_com_msp_id; - - // now figure out enabled features: registered sensors, ADC routing, GPS - - if( PIOS_SENSORS_GetInstanceByType( 0, PIOS_SENSORS_TYPE_3AXIS_ACCEL ) ) - { - msp->sensors |= MSP_SENSOR_ACC; - } - if( PIOS_SENSORS_GetInstanceByType( 0, PIOS_SENSORS_TYPE_1AXIS_BARO ) ) - { - msp->sensors |= MSP_SENSOR_BARO; - } - if( PIOS_SENSORS_GetInstanceByType( 0, PIOS_SENSORS_TYPE_3AXIS_MAG|PIOS_SENSORS_TYPE_3AXIS_AUXMAG ) ) - { - msp->sensors |= MSP_SENSOR_MAG; - } + msp->com = pios_com_msp_id; + + // now figure out enabled features: registered sensors, ADC routing, GPS + +#ifdef PIOS_EXCLUDE_ADVANCED_FEATURES + msp->sensors |= MSP_SENSOR_ACC; +#else + + if (PIOS_SENSORS_GetInstanceByType(0, PIOS_SENSORS_TYPE_3AXIS_ACCEL)) { + msp->sensors |= MSP_SENSOR_ACC; + } + if (PIOS_SENSORS_GetInstanceByType(0, PIOS_SENSORS_TYPE_1AXIS_BARO)) { + msp->sensors |= MSP_SENSOR_BARO; + } + if (PIOS_SENSORS_GetInstanceByType(0, PIOS_SENSORS_TYPE_3AXIS_MAG | PIOS_SENSORS_TYPE_3AXIS_AUXMAG)) { + msp->sensors |= MSP_SENSOR_MAG; + } #ifdef PIOS_SENSORS_TYPE_1AXIS_SONAR - if( PIOS_SENSORS_GetInstanceByType( 0, PIOS_SENSORS_TYPE_1AXIS_SONAR ) ) - { - msp->sensors |= MSP_SENSOR_SONAR; - } -#endif - - // MAP_SENSOR_GPS is hot-pluggable type - - HwSettingsADCRoutingDataArray adcRoutingDataArray; - HwSettingsADCRoutingArrayGet(adcRoutingDataArray.array); + if (PIOS_SENSORS_GetInstanceByType(0, PIOS_SENSORS_TYPE_1AXIS_SONAR)) { + msp->sensors |= MSP_SENSOR_SONAR; + } +#endif /* PIOS_SENSORS_TYPE_1AXIS_SONAR */ +#endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */ - for(unsigned i = 0; i < sizeof(adcRoutingDataArray.array)/sizeof(adcRoutingDataArray.array[0]); ++i) - { - switch( adcRoutingDataArray.array[i] ) - { - case HWSETTINGS_ADCROUTING_BATTERYVOLTAGE: - msp->analog |= MSP_ANALOG_VOLTAGE; - break; - case HWSETTINGS_ADCROUTING_BATTERYCURRENT: - msp->analog |= MSP_ANALOG_CURRENT; - break; - default:; - } - } - - - HwSettingsMSPSpeedOptions mspSpeed; - HwSettingsMSPSpeedGet(&mspSpeed); - - PIOS_COM_ChangeBaud(pios_com_msp_id, hwsettings_mspspeed_enum_to_baud(mspSpeed)); - - module_enabled = true; - return 0; - } - } + // MAP_SENSOR_GPS is hot-pluggable type - return -1; + HwSettingsADCRoutingDataArray adcRoutingDataArray; + HwSettingsADCRoutingArrayGet(adcRoutingDataArray.array); + + for (unsigned i = 0; i < sizeof(adcRoutingDataArray.array) / sizeof(adcRoutingDataArray.array[0]); ++i) { + switch (adcRoutingDataArray.array[i]) { + case HWSETTINGS_ADCROUTING_BATTERYVOLTAGE: + msp->analog |= MSP_ANALOG_VOLTAGE; + break; + case HWSETTINGS_ADCROUTING_BATTERYCURRENT: + msp->analog |= MSP_ANALOG_CURRENT; + break; + default:; + } + } + + + HwSettingsMSPSpeedOptions mspSpeed; + HwSettingsMSPSpeedGet(&mspSpeed); + + PIOS_COM_ChangeBaud(pios_com_msp_id, hwsettings_mspspeed_enum_to_baud(mspSpeed)); + + module_enabled = true; + return 0; + } + } + + return -1; } MODULE_INITCALL(uavoMSPBridgeInitialize, uavoMSPBridgeStart); @@ -1064,22 +1036,22 @@ MODULE_INITCALL(uavoMSPBridgeInitialize, uavoMSPBridgeStart); */ static void uavoMSPBridgeTask(__attribute__((unused)) void *parameters) { - while (1) { - uint8_t b = 0; - uint16_t count = PIOS_COM_ReceiveBuffer(msp->com, &b, 1, ~0); - if (count) { - if (!msp_receive_byte(msp, b)) { - // Returning is considered risky here as - // that's unusual and this is an edge case. - while (1) { - PIOS_DELAY_WaitmS(60*1000); - } - } - } - } + while (1) { + uint8_t b = 0; + uint16_t count = PIOS_COM_ReceiveBuffer(msp->com, &b, 1, ~0); + if (count) { + if (!msp_receive_byte(msp, b)) { + // Returning is considered risky here as + // that's unusual and this is an edge case. + while (1) { + PIOS_DELAY_WaitmS(60 * 1000); + } + } + } + } } -#endif //PIOS_INCLUDE_MSP_BRIDGE +#endif // PIOS_INCLUDE_MSP_BRIDGE /** * @} * @} diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 44aa5ee90..4f28f3d12 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -54,6 +54,7 @@ OPTMODULES += TxPID OPTMODULES += Osd/osdoutput #OPTMODULES += Altitude #OPTMODULES += Fault +OPTMODULES += UAVOMSPBridge SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 90cbfa8b9..d6da2be90 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -75,6 +75,9 @@ static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); #define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_MSP_TX_BUF_LEN 128 +#define PIOS_COM_MSP_RX_BUF_LEN 64 + #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 uint32_t pios_com_debug_id; @@ -86,7 +89,7 @@ uint32_t pios_com_vcp_id; uint32_t pios_com_gps_id; uint32_t pios_com_bridge_id; uint32_t pios_com_hkosd_id; - +uint32_t pios_com_msp_id; uint32_t pios_usb_rctx_id; uintptr_t pios_uavo_settings_fs_id; @@ -551,6 +554,23 @@ void PIOS_Board_Init(void) PIOS_Assert(0); } } + case HWSETTINGS_CC_MAINPORT_MSP: + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_MSP_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_MSP_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_msp_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_MSP_RX_BUF_LEN, + tx_buffer, PIOS_COM_MSP_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } break; case HWSETTINGS_CC_MAINPORT_OSDHK: { @@ -614,6 +634,24 @@ void PIOS_Board_Init(void) } } break; + case HWSETTINGS_CC_FLEXIPORT_MSP: + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_MSP_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_MSP_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_msp_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_MSP_RX_BUF_LEN, + tx_buffer, PIOS_COM_MSP_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; case HWSETTINGS_CC_FLEXIPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 5b002e009..c19dfb0c5 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -156,6 +156,9 @@ extern uint32_t pios_com_debug_id; extern uint32_t pios_com_hkosd_id; #define PIOS_COM_OSDHK (pios_com_hkosd_id) +extern uint32_t pios_com_msp_id; +#define PIOS_COM_MSP (pios_com_msp_id) + // ------------------------- // ADC // PIOS_ADC_PinGet(0) = Gyro Z diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 74afbdc01..1f46a4f1a 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -56,6 +56,7 @@ MODULES += Logging MODULES += Telemetry OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index 9414d7f33..ef01e6566 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -248,6 +248,9 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_HKOSD_RX_BUF_LEN 22 #define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_MSP_TX_BUF_LEN 128 +#define PIOS_COM_MSP_RX_BUF_LEN 64 + #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 uint32_t pios_com_debug_id; @@ -259,7 +262,7 @@ uint32_t pios_com_telem_rf_id = 0; uint32_t pios_com_bridge_id = 0; uint32_t pios_com_overo_id = 0; uint32_t pios_com_hkosd_id = 0; - +uint32_t pios_com_msp_id = 0; uint32_t pios_com_vcp_id = 0; #if defined(PIOS_INCLUDE_RFM22B) @@ -660,6 +663,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RM_MAINPORT_MSP: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; case HWSETTINGS_RM_MAINPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; @@ -735,6 +741,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RM_FLEXIPORT_MSP: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; case HWSETTINGS_RM_FLEXIPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; diff --git a/flight/targets/boards/discoveryf4bare/pios_board.h b/flight/targets/boards/discoveryf4bare/pios_board.h index d25c71d53..80074241a 100644 --- a/flight/targets/boards/discoveryf4bare/pios_board.h +++ b/flight/targets/boards/discoveryf4bare/pios_board.h @@ -128,12 +128,14 @@ extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_hkosd_id; +extern uint32_t pios_com_msp_id; #define PIOS_COM_GPS (pios_com_gps_id) #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) #define PIOS_COM_BRIDGE (pios_com_bridge_id) #define PIOS_COM_VCP (pios_com_vcp_id) #define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_MSP (pios_com_msp_id) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) extern uint32_t pios_com_debug_id; diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 61cc31674..5aa6d22c4 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -255,8 +255,8 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_HKOSD_RX_BUF_LEN 22 #define PIOS_COM_HKOSD_TX_BUF_LEN 22 -#define PIOS_COM_MSP_TX_BUF_LEN 128 -#define PIOS_COM_MSP_RX_BUF_LEN 64 +#define PIOS_COM_MSP_TX_BUF_LEN 128 +#define PIOS_COM_MSP_RX_BUF_LEN 64 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 @@ -995,6 +995,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RM_RCVRPORT_MSP: + PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; } #if defined(PIOS_INCLUDE_GCSRCVR) diff --git a/flight/targets/boards/revonano/firmware/Makefile b/flight/targets/boards/revonano/firmware/Makefile index 94882efc3..7e3701a2f 100644 --- a/flight/targets/boards/revonano/firmware/Makefile +++ b/flight/targets/boards/revonano/firmware/Makefile @@ -52,6 +52,7 @@ MODULES += Telemetry MODULES += Notify OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge SRC += $(FLIGHTLIB)/notification.c diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 4cc6f58d9..ddc62b32b 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -207,6 +207,9 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_HKOSD_RX_BUF_LEN 22 #define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_MSP_TX_BUF_LEN 128 +#define PIOS_COM_MSP_RX_BUF_LEN 64 + #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 uint32_t pios_com_debug_id; @@ -219,7 +222,7 @@ uint32_t pios_com_rf_id = 0; uint32_t pios_com_bridge_id = 0; uint32_t pios_com_overo_id = 0; uint32_t pios_com_hkosd_id = 0; - +uint32_t pios_com_msp_id = 0; uint32_t pios_com_vcp_id = 0; uintptr_t pios_uavo_settings_fs_id; @@ -618,6 +621,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RM_MAINPORT_MSP: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; case HWSETTINGS_RM_MAINPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; @@ -693,6 +699,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RM_FLEXIPORT_MSP: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; case HWSETTINGS_RM_FLEXIPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 63ea052d5..c98cfc77a 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -148,6 +148,7 @@ extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_hkosd_id; +extern uint32_t pios_com_msp_id; #define PIOS_COM_GPS (pios_com_gps_id) #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) @@ -155,6 +156,7 @@ extern uint32_t pios_com_hkosd_id; #define PIOS_COM_BRIDGE (pios_com_bridge_id) #define PIOS_COM_VCP (pios_com_vcp_id) #define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_MSP (pios_com_msp_id) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) extern uint32_t pios_com_debug_id; diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 4468633d9..67ad81a59 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -53,6 +53,7 @@ MODULES += Telemetry SRC += $(FLIGHTLIB)/notification.c OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge # Include all camera options CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index b88abc4a7..44da3b974 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -327,6 +327,8 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_HKOSD_RX_BUF_LEN 22 #define PIOS_COM_HKOSD_TX_BUF_LEN 22 +#define PIOS_COM_MSP_TX_BUF_LEN 128 +#define PIOS_COM_MSP_RX_BUF_LEN 64 uint32_t pios_com_aux_id = 0; uint32_t pios_com_gps_id = 0; @@ -335,6 +337,7 @@ uint32_t pios_com_telem_rf_id = 0; uint32_t pios_com_bridge_id = 0; uint32_t pios_com_overo_id = 0; uint32_t pios_com_hkosd_id = 0; +uint32_t pios_com_msp_id = 0; uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id; @@ -664,6 +667,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RV_TELEMETRYPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RV_TELEMETRYPORT_MSP: + PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; } /* hwsettings_rv_telemetryport */ /* Configure GPS port */ @@ -688,6 +694,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RV_GPSPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RV_GPSPORT_MSP: + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; } /* hwsettings_rv_gpsport */ /* Configure AUXPort */ @@ -713,6 +722,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RV_AUXPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RV_AUXPORT_MSP: + PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; case HWSETTINGS_RV_AUXPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_aux_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; @@ -758,6 +770,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RV_AUXSBUSPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RV_AUXSBUSPORT_MSP: + PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; case HWSETTINGS_RV_AUXSBUSPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_auxsbus_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; @@ -817,6 +832,9 @@ void PIOS_Board_Init(void) case HWSETTINGS_RV_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; + case HWSETTINGS_RV_FLEXIPORT_MSP: + PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); + break; } /* hwsettings_rv_flexiport */ diff --git a/flight/targets/boards/revoproto/pios_board.h b/flight/targets/boards/revoproto/pios_board.h index 30cab6600..171c37f44 100644 --- a/flight/targets/boards/revoproto/pios_board.h +++ b/flight/targets/boards/revoproto/pios_board.h @@ -126,6 +126,7 @@ extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_hkosd_id; +extern uint32_t pios_com_msp_id; #define PIOS_COM_AUX (pios_com_aux_id) #define PIOS_COM_GPS (pios_com_gps_id) #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) @@ -134,6 +135,7 @@ extern uint32_t pios_com_hkosd_id; #define PIOS_COM_VCP (pios_com_vcp_id) #define PIOS_COM_DEBUG PIOS_COM_AUX #define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_MSP (pios_com_msp_id) // ------------------------ // TELEMETRY diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 88030cc72..0e0602767 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,16 +2,16 @@ Selection of optional hardware configurations. - - + + - - - - - + + + + + - From 26d63ee7aca2a3e0c0e6542331901acfd6b3fc40 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 22 Apr 2016 22:53:16 +0200 Subject: [PATCH 09/23] LP-291 fixed @author tags --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 2bf87a772..d71a522c4 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -7,8 +7,8 @@ * * @file uavomspbridge.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. - * @author Tau Labs, http://taulabs.org, Copyright (C) 2015 - * @author dRonin, http://dronin.org Copyright (C) 2015-2016 + * Tau Labs, http://taulabs.org, Copyright (C) 2015 + * dRonin, http://dronin.org Copyright (C) 2015-2016 * @brief Bridges selected UAVObjects to MSP for MWOSD and the like. * * @see The GNU Public License (GPL) Version 3 @@ -585,7 +585,7 @@ static uint16_t msp_scale_rc(float percent) // position unless we send it what it wants right away. static uint16_t msp_scale_rc_thr(float percent) { - return percent <= 0 ? 1100 : percent *800 + 1100; + return percent <= 0 ? 1100 : percent * 800 + 1100; } // MSP RC order is Roll/Pitch/Yaw/Throttle/AUX1/AUX2/AUX3/AUX4 From 5d72a9671c0543fd9817cbdc5d388683fddcdafe Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 24 Apr 2016 01:38:30 +0200 Subject: [PATCH 10/23] LP-291 msp_send_analog(): Use OPLinkStatus.RSSI if applicable --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 43 +++++++++++++++----- 1 file changed, 32 insertions(+), 11 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index d71a522c4..dea304a04 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -446,12 +446,33 @@ static void msp_send_analog(struct msp_bridge *m) } } #endif - uint8_t quality; - ReceiverStatusQualityGet(&quality); + // OPLink supports RSSI reported in dBm, but updates different value in ReceiverStatus.Quality (link_quality - function of lost, repaired and good packet count). + // We use same method as in Receiver module to decide if oplink is used for control: + // Check for Throttle channel group, if this belongs to oplink, we will use oplink rssi instead of ReceiverStatus.Quality. - // MSP RSSI's range is 0-1023 + ManualControlSettingsChannelGroupsData channelGroups; + ManualControlSettingsChannelGroupsGet(&channelGroups); + + if(channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) + { + int8_t rssi; + OPLinkStatusRSSIGet(&rssi); + + // oplink rssi - low: -127 noise floor (set by software when link is completely lost) + // max: various articles found on web quote -10 as maximum received signal power expressed in dBm. + // MSP values have no units, and OSD rssi display requires calibration anyway, so we will translate -127 to -10 -> 0-1023 + + data.status.rssi = ((127 + rssi) * 1023) / (127 - 10); + } + else + { + uint8_t quality; + ReceiverStatusQualityGet(&quality); - data.status.rssi = (quality * 1023) / 100; + // MSP RSSI's range is 0-1023 + + data.status.rssi = (quality * 1023) / 100; + } if (data.status.rssi > 1023) { data.status.rssi = 1023; @@ -508,7 +529,7 @@ static void msp_send_comp_gps(struct msp_bridge *m) uint8_t home_position_valid; // 0 = Invalid, Dronin MSP specific } __attribute__((packed)) comp_gps; } data; - + GPSPositionSensorData gps_data; HomeLocationData home_data; @@ -538,14 +559,14 @@ static void msp_send_comp_gps(struct msp_bridge *m) data.comp_gps.distance_to_home = (uint16_t)(sqrtf(delta_x * delta_x + delta_y * delta_y) * 1e-4f); // meters if ((delta_lon == 0) && (delta_lat == 0)) { - data.comp_gps.direction_to_home = 0; + data.comp_gps.direction_to_home = 0; } else { data.comp_gps.direction_to_home = RAD2DEG((int16_t)(atan2f(delta_y, delta_x))); // degrees; } } - - msp_send(m, MSP_COMP_GPS, data.buf, sizeof(data)); - } + + msp_send(m, MSP_COMP_GPS, data.buf, sizeof(data)); +} } static void msp_send_altitude(struct msp_bridge *m) @@ -565,8 +586,8 @@ static void msp_send_altitude(struct msp_bridge *m) data.baro.alt = (int32_t)roundf(baro.Altitude * 100.0f); - msp_send(m, MSP_ALTITUDE, data.buf, sizeof(data)); - } + msp_send(m, MSP_ALTITUDE, data.buf, sizeof(data)); +} } #endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */ From bb0f15024b1a671cba402b3feeedffadf8849914 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 24 Apr 2016 01:41:42 +0200 Subject: [PATCH 11/23] LP-291 msp_send_channels(): include Accessory3 --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index dea304a04..f64a50a63 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -612,13 +612,14 @@ static uint16_t msp_scale_rc_thr(float percent) // MSP RC order is Roll/Pitch/Yaw/Throttle/AUX1/AUX2/AUX3/AUX4 static void msp_send_channels(struct msp_bridge *m) { - AccessoryDesiredData acc0, acc1, acc2; + AccessoryDesiredData acc0, acc1, acc2, acc3; ManualControlCommandData manualState; ManualControlCommandGet(&manualState); AccessoryDesiredInstGet(0, &acc0); AccessoryDesiredInstGet(1, &acc1); AccessoryDesiredInstGet(2, &acc2); + AccessoryDesiredInstGet(3, &acc2); union { uint8_t buf[0]; @@ -632,7 +633,7 @@ static void msp_send_channels(struct msp_bridge *m) msp_scale_rc(acc0.AccessoryVal), msp_scale_rc(acc1.AccessoryVal), msp_scale_rc(acc2.AccessoryVal), - 1000, // no aux4 + msp_scale_rc(acc3.AccessoryVal), } }; From fec3924ee0c8ead6ac50c65e95d33146d7110172 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 24 Apr 2016 01:46:13 +0200 Subject: [PATCH 12/23] LP-291 msp_send_altitude(): use PositionState instead of BaroSensor and add VelocityState to provide climb rate --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 32 ++++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index f64a50a63..04b95815f 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @addtogroup TauLabsModules TauLabs Modules + * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup UAVOMSPBridge UAVO to MSP Bridge Module * @{ @@ -51,7 +51,8 @@ #include "systemstats.h" #include "systemalarms.h" #include "homelocation.h" -#include "barosensor.h" +#include "positionstate.h" +#include "velocitystate.h" #include "stabilizationdesired.h" #include "taskinfo.h" #include "stabilizationsettings.h" @@ -574,21 +575,32 @@ static void msp_send_altitude(struct msp_bridge *m) union { uint8_t buf[0]; struct { - int32_t alt; // cm - uint16_t vario; // cm/s - } __attribute__((packed)) baro; + int32_t estimatedAltitude; // cm + int16_t vario; // cm/s + } __attribute__((packed)) altitude; } data; - if (BaroSensorHandle() != NULL) { - BaroSensorData baro; - BaroSensorGet(&baro); + if (PositionStateHandle() != NULL) { + PositionStateData positionState; + PositionStateGet(&positionState); - data.baro.alt = (int32_t)roundf(baro.Altitude * 100.0f); + data.altitude.estimatedAltitude = (int32_t)roundf(positionState.Down * -100.0f); + } else { + data.altitude.estimatedAltitude = 0; + } + + if( VelocityStateHandle() != NULL) { + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + + data.altitude.vario = (int16_t)roundf(velocityState.Down * -100.0f); + } else { + data.altitude.vario = 0; + } msp_send(m, MSP_ALTITUDE, data.buf, sizeof(data)); } -} #endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */ From 69a51956e81098e6741b63a095d229c90f9a5de3 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 24 Apr 2016 01:49:09 +0200 Subject: [PATCH 13/23] LP-291 msp_send_comp_gps(): calculate distance and direction to home using PositionState and TakeOffLocation, same way it is done in GCS. --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 65 +++++++++----------- 1 file changed, 29 insertions(+), 36 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 04b95815f..f54e6385d 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -50,6 +50,7 @@ #include "flightstatus.h" #include "systemstats.h" #include "systemalarms.h" +#include "takeofflocation.h" #include "homelocation.h" #include "positionstate.h" #include "velocitystate.h" @@ -65,9 +66,6 @@ #include "pios_sensors.h" -#define WGS84_RADIUS_EARTH_KM 6371.008f // Earth's radius in km -#define PI 3.14159265358979323846f // [-] - #define PIOS_INCLUDE_MSP_BRIDGE #if defined(PIOS_INCLUDE_MSP_BRIDGE) @@ -520,6 +518,12 @@ static void msp_send_raw_gps(struct msp_bridge *m) } #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + +static inline float Sq(float x) +{ + return x * x; +} + static void msp_send_comp_gps(struct msp_bridge *m) { union { @@ -531,44 +535,33 @@ static void msp_send_comp_gps(struct msp_bridge *m) } __attribute__((packed)) comp_gps; } data; - GPSPositionSensorData gps_data; - HomeLocationData home_data; - - if ((GPSPositionSensorHandle() == NULL) || (HomeLocationHandle() == NULL)) { - data.comp_gps.distance_to_home = 0; - data.comp_gps.direction_to_home = 0; - data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD - } else { - GPSPositionSensorGet(&gps_data); - HomeLocationGet(&home_data); - - if ((gps_data.Status < GPSPOSITIONSENSOR_STATUS_FIX2D) || (home_data.Set == HOMELOCATION_SET_FALSE)) { - data.comp_gps.distance_to_home = 0; - data.comp_gps.direction_to_home = 0; - data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD - } else { - data.comp_gps.home_position_valid = 1; // Home distance and direction will display on OSD - - int32_t delta_lon = (home_data.Longitude - gps_data.Longitude); // degrees * 1e7 - int32_t delta_lat = (home_data.Latitude - gps_data.Latitude); // degrees * 1e7 - - float delta_y = DEG2RAD((float)delta_lon * WGS84_RADIUS_EARTH_KM); // KM * 1e7 - float delta_x = DEG2RAD((float)delta_lat * WGS84_RADIUS_EARTH_KM); // KM * 1e7 - - delta_y *= cosf(DEG2RAD((float)home_data.Latitude * 1e-7f)); // Latitude compression correction - - data.comp_gps.distance_to_home = (uint16_t)(sqrtf(delta_x * delta_x + delta_y * delta_y) * 1e-4f); // meters - - if ((delta_lon == 0) && (delta_lat == 0)) { + data.comp_gps.distance_to_home = 0; data.comp_gps.direction_to_home = 0; - } else { - data.comp_gps.direction_to_home = RAD2DEG((int16_t)(atan2f(delta_y, delta_x))); // degrees; - } + data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD + + if( PositionStateHandle() && TakeOffLocationHandle() ) + { + + TakeOffLocationData takeOffLocation; + TakeOffLocationGet(&takeOffLocation); + + if(takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) + { + PositionStateData positionState; + PositionStateGet(&positionState); + + data.comp_gps.distance_to_home = (uint16_t)sqrtf( Sq(takeOffLocation.East - positionState.East) + Sq(takeOffLocation.North - positionState.North) ); + data.comp_gps.direction_to_home = RAD2DEG( atan2f(takeOffLocation.East - positionState.East, takeOffLocation.North - positionState.North) ); + + data.comp_gps.home_position_valid = 1; } + } + + // here, CC3D implementation could use raw gps data (GPSPositionSensorData) and locally cached GPSPositionSensorData at arming time as TakeOffLocation. + msp_send(m, MSP_COMP_GPS, data.buf, sizeof(data)); } -} static void msp_send_altitude(struct msp_bridge *m) { From 33eb43486260e448fa706555126e4d06c7c457e1 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Sun, 24 Apr 2016 01:50:41 +0200 Subject: [PATCH 14/23] LP-291 add missing includes. Uncrustification. --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 50 +++++++++----------- 1 file changed, 23 insertions(+), 27 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index f54e6385d..dabfc7d40 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -42,6 +42,8 @@ #include "flightbatterystate.h" #include "gpspositionsensor.h" #include "manualcontrolcommand.h" +#include "manualcontrolsettings.h" +#include "oplinkstatus.h" #include "accessorydesired.h" #include "attitudestate.h" #include "airspeedstate.h" @@ -451,20 +453,17 @@ static void msp_send_analog(struct msp_bridge *m) ManualControlSettingsChannelGroupsData channelGroups; ManualControlSettingsChannelGroupsGet(&channelGroups); - - if(channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) - { + + if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) { int8_t rssi; OPLinkStatusRSSIGet(&rssi); - + // oplink rssi - low: -127 noise floor (set by software when link is completely lost) - // max: various articles found on web quote -10 as maximum received signal power expressed in dBm. + // max: various articles found on web quote -10 as maximum received signal power expressed in dBm. // MSP values have no units, and OSD rssi display requires calibration anyway, so we will translate -127 to -10 -> 0-1023 - + data.status.rssi = ((127 + rssi) * 1023) / (127 - 10); - } - else - { + } else { uint8_t quality; ReceiverStatusQualityGet(&quality); @@ -534,32 +533,29 @@ static void msp_send_comp_gps(struct msp_bridge *m) uint8_t home_position_valid; // 0 = Invalid, Dronin MSP specific } __attribute__((packed)) comp_gps; } data; - + data.comp_gps.distance_to_home = 0; data.comp_gps.direction_to_home = 0; data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD - - if( PositionStateHandle() && TakeOffLocationHandle() ) - { + if (PositionStateHandle() && TakeOffLocationHandle()) { TakeOffLocationData takeOffLocation; TakeOffLocationGet(&takeOffLocation); - - if(takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) - { - PositionStateData positionState; + + if (takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) { + PositionStateData positionState; PositionStateGet(&positionState); - - data.comp_gps.distance_to_home = (uint16_t)sqrtf( Sq(takeOffLocation.East - positionState.East) + Sq(takeOffLocation.North - positionState.North) ); - data.comp_gps.direction_to_home = RAD2DEG( atan2f(takeOffLocation.East - positionState.East, takeOffLocation.North - positionState.North) ); + + data.comp_gps.distance_to_home = (uint16_t)sqrtf(Sq(takeOffLocation.East - positionState.East) + Sq(takeOffLocation.North - positionState.North)); + data.comp_gps.direction_to_home = RAD2DEG(atan2f(takeOffLocation.East - positionState.East, takeOffLocation.North - positionState.North)); data.comp_gps.home_position_valid = 1; } } // here, CC3D implementation could use raw gps data (GPSPositionSensorData) and locally cached GPSPositionSensorData at arming time as TakeOffLocation. - - + + msp_send(m, MSP_COMP_GPS, data.buf, sizeof(data)); } @@ -568,8 +564,8 @@ static void msp_send_altitude(struct msp_bridge *m) union { uint8_t buf[0]; struct { - int32_t estimatedAltitude; // cm - int16_t vario; // cm/s + int32_t estimatedAltitude; // cm + int16_t vario; // cm/s } __attribute__((packed)) altitude; } data; @@ -582,11 +578,11 @@ static void msp_send_altitude(struct msp_bridge *m) } else { data.altitude.estimatedAltitude = 0; } - - if( VelocityStateHandle() != NULL) { + + if (VelocityStateHandle() != NULL) { VelocityStateData velocityState; VelocityStateGet(&velocityState); - + data.altitude.vario = (int16_t)roundf(velocityState.Down * -100.0f); } else { data.altitude.vario = 0; From 84886cbce488c4efbeea08b621a3b0a05563d341 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Mon, 25 Apr 2016 12:55:23 +0200 Subject: [PATCH 15/23] LP-291 fixed typo &acc2 should be &acc3 --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index dabfc7d40..b56df59b3 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -620,7 +620,7 @@ static void msp_send_channels(struct msp_bridge *m) AccessoryDesiredInstGet(0, &acc0); AccessoryDesiredInstGet(1, &acc1); AccessoryDesiredInstGet(2, &acc2); - AccessoryDesiredInstGet(3, &acc2); + AccessoryDesiredInstGet(3, &acc3); union { uint8_t buf[0]; From 22cf21c335489875f94701bf5e9458dfeb95e2bc Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Mon, 25 Apr 2016 12:57:27 +0200 Subject: [PATCH 16/23] LP-291 fix minor whitespace issue --- shared/uavobjectdefinition/hwsettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 0e0602767..9f116088d 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -14,7 +14,7 @@ - + From 8dbf9e7d3126dace330632908ceb9b549178ca0b Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Mon, 25 Apr 2016 13:28:10 +0200 Subject: [PATCH 17/23] LP-291 Changed noise floor level to -110 to be closer to real noise level instead of absolute -127 --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index b56df59b3..c635896e8 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -72,6 +72,13 @@ #if defined(PIOS_INCLUDE_MSP_BRIDGE) +// oplink rssi - absolute low : -127 noise floor (set by software when link is completely lost) +// in reality : around -110 +// max: various articles found on web quote -10 as maximum received signal power expressed in dBm. + +#define OPLINK_LOW_RSSI -110 +#define OPLINK_HIGH_RSSI -10 + #define MSP_SENSOR_ACC (1 << 0) #define MSP_SENSOR_BARO (1 << 1) #define MSP_SENSOR_MAG (1 << 2) @@ -458,11 +465,16 @@ static void msp_send_analog(struct msp_bridge *m) int8_t rssi; OPLinkStatusRSSIGet(&rssi); - // oplink rssi - low: -127 noise floor (set by software when link is completely lost) - // max: various articles found on web quote -10 as maximum received signal power expressed in dBm. - // MSP values have no units, and OSD rssi display requires calibration anyway, so we will translate -127 to -10 -> 0-1023 + // MSP values have no units, and OSD rssi display requires calibration anyway, so we will translate OPLINK_LOW_RSSI to OPLINK_HIGH_RSSI -> 0-1023 - data.status.rssi = ((127 + rssi) * 1023) / (127 - 10); + + if (rssi < OPLINK_LOW_RSSI) { + rssi = OPLINK_LOW_RSSI; + } else if (rssi > OPLINK_HIGH_RSSI) { + rssi = OPLINK_HIGH_RSSI; + } + + data.status.rssi = ((rssi - OPLINK_LOW_RSSI) * 1023) / (OPLINK_HIGH_RSSI - OPLINK_LOW_RSSI); } else { uint8_t quality; ReceiverStatusQualityGet(&quality); From 35ccc6a169a7615a6c03c7bdff1ff7af8dbb3db3 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Mon, 25 Apr 2016 14:58:46 +0200 Subject: [PATCH 18/23] LP-291 removed unused msp_send_ident() function --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index c635896e8..bf80d008b 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -72,9 +72,9 @@ #if defined(PIOS_INCLUDE_MSP_BRIDGE) -// oplink rssi - absolute low : -127 noise floor (set by software when link is completely lost) -// in reality : around -110 -// max: various articles found on web quote -10 as maximum received signal power expressed in dBm. +// oplink rssi - absolute low : -127 dBm noise floor (set by software when link is completely lost) +// in reality : around -110 dBm +// max: various articles found on web quote -10 dBm as maximum received signal power. #define OPLINK_LOW_RSSI -110 #define OPLINK_HIGH_RSSI -10 @@ -466,8 +466,6 @@ static void msp_send_analog(struct msp_bridge *m) OPLinkStatusRSSIGet(&rssi); // MSP values have no units, and OSD rssi display requires calibration anyway, so we will translate OPLINK_LOW_RSSI to OPLINK_HIGH_RSSI -> 0-1023 - - if (rssi < OPLINK_LOW_RSSI) { rssi = OPLINK_LOW_RSSI; } else if (rssi > OPLINK_HIGH_RSSI) { @@ -480,7 +478,6 @@ static void msp_send_analog(struct msp_bridge *m) ReceiverStatusQualityGet(&quality); // MSP RSSI's range is 0-1023 - data.status.rssi = (quality * 1023) / 100; } @@ -491,11 +488,6 @@ static void msp_send_analog(struct msp_bridge *m) msp_send(m, MSP_ANALOG, data.buf, sizeof(data)); } -static void msp_send_ident(__attribute__((unused)) struct msp_bridge *m) -{ - // TODO -} - static void msp_send_raw_gps(struct msp_bridge *m) { union { @@ -811,9 +803,6 @@ static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b) // Respond to interesting things. switch (m->cmd_id) { - case MSP_IDENT: - msp_send_ident(m); - break; case MSP_RAW_GPS: msp_send_raw_gps(m); break; From 7925f115df56271e05e3508f167ce306cd8bc52f Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Mon, 25 Apr 2016 15:15:03 +0200 Subject: [PATCH 19/23] LP-291 fix coptercontrol build. Oplink uavos are not available there. --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index bf80d008b..25b8be2cf 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -461,6 +461,7 @@ static void msp_send_analog(struct msp_bridge *m) ManualControlSettingsChannelGroupsData channelGroups; ManualControlSettingsChannelGroupsGet(&channelGroups); +#ifdef PIOS_INCLUDE_OPLINKRCVR if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) { int8_t rssi; OPLinkStatusRSSIGet(&rssi); @@ -474,12 +475,15 @@ static void msp_send_analog(struct msp_bridge *m) data.status.rssi = ((rssi - OPLINK_LOW_RSSI) * 1023) / (OPLINK_HIGH_RSSI - OPLINK_LOW_RSSI); } else { +#endif /* PIOS_INCLUDE_OPLINKRCVR */ uint8_t quality; ReceiverStatusQualityGet(&quality); // MSP RSSI's range is 0-1023 data.status.rssi = (quality * 1023) / 100; +#ifdef PIOS_INCLUDE_OPLINKRCVR } +#endif /* PIOS_INCLUDE_OPLINKRCVR */ if (data.status.rssi > 1023) { data.status.rssi = 1023; From 2a668d0615ec344ebebcec4b830b87e871bcf7af Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Tue, 26 Apr 2016 11:54:47 +0200 Subject: [PATCH 20/23] LP-291 Fixed alert severity translation to MSP alarm levels. --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 25b8be2cf..85f1c1d84 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -785,14 +785,21 @@ static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m) data.alarm.state = ALARM_OK; int32_t len = AlarmString(&alarm, data.alarm.msg, sizeof(data.alarm.msg), SYSTEMALARMS_ALARM_CRITICAL, &state); // Include only CRITICAL and ERROR + + // NOTE: LP alarm severity levels and MSP levels do not match. ERROR and CRITICAL are swapped. + // So far, MW-OSD code (MSP consumer) does not make difference between ALARM_ERROR and ALARM_CRITICAL. + // ALARM_WARN should be blinking if thats the highest severity level at the moment. + // There might be other types of MSP consumers. + switch (state) { case SYSTEMALARMS_ALARM_WARNING: data.alarm.state = ALARM_WARN; break; case SYSTEMALARMS_ALARM_ERROR: + data.alarm.state = ALARM_CRIT; break; case SYSTEMALARMS_ALARM_CRITICAL: - data.alarm.state = ALARM_CRIT;; + data.alarm.state = ALARM_ERROR; } msp_send(m, MSP_ALARMS, data.buf, len + 1); From 6e2ec8aecc7597aad483b145a6727e5b478054ed Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Tue, 26 Apr 2016 11:56:58 +0200 Subject: [PATCH 21/23] LP-291 Minor cosmetic change - added missing break in msp_send_alarms(). --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 85f1c1d84..f0cd50376 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -800,6 +800,7 @@ static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m) break; case SYSTEMALARMS_ALARM_CRITICAL: data.alarm.state = ALARM_ERROR; + break; } msp_send(m, MSP_ALARMS, data.buf, len + 1); From 240b299a147261a66e20e61de6cbc1792f2ee743 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 11 May 2016 13:35:40 +0200 Subject: [PATCH 22/23] LP-291 Added MSP_PIDNAMES in good hope that mwosd will once implement it. Added pid tuning for Attitude PI. msp_set_pid() will now commit changes to same bank msp_send_pid() got data from. Implemented saving of settings bank data in disarmed state. --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 154 +++++++++++-------- 1 file changed, 87 insertions(+), 67 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index f0cd50376..d85d59654 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -192,8 +192,9 @@ typedef enum { MSP_MAYBE_UAVTALK_SLOW6 } msp_state; + typedef struct __attribute__((packed)) { - uint8_t P, I, D; + uint8_t values[3]; } msp_pid_t; @@ -201,16 +202,25 @@ typedef enum { PIDROLL, PIDPITCH, PIDYAW, - PIDALT, - PIDPOS, - PIDPOSR, - PIDNAVR, - PIDLEVEL, + PIDAROLL, // (PIDALT) use this for Attitude ROLL + PIDPOS, // skipped by MWOSD + PIDPOSR, // skipped by MWOSD + PIDAPITCH, // (PIDNAVR) use this for Attitude PITCH + PIDAYAW, // (PIDAYAW) use this for Attitude YAW PIDMAG, - PIDVEL, + PIDVEL, // skipped by MWOSD PID_ITEM_COUNT } pidIndex_e; +static const char msp_pidnames[] = "ROLL;" + "PITCH;" + "YAW;" + "A.ROLL;" + "Pos;" + "PosR;" + "A.PITCH;" + "A.YAW;" + "VEL;"; #define MSP_ANALOG_VOLTAGE (1 << 0) #define MSP_ANALOG_CURRENT (1 << 1) @@ -220,7 +230,9 @@ struct msp_bridge { uint8_t sensors; uint8_t analog; - + + UAVObjHandle current_pid_bank; + msp_state state; uint8_t cmd_size; uint8_t cmd_id; @@ -654,7 +666,26 @@ static void msp_send_boxids(struct msp_bridge *m) // This is actually sending a msp_send(m, MSP_BOXIDS, msp_boxes, sizeof(msp_boxes)); } -static StabilizationSettingsFlightModeMapOptions get_current_stabilization_bank() +static void msp_send_pidnames(struct msp_bridge *m) +{ + msp_send(m, MSP_PIDNAMES, (const uint8_t *)msp_pidnames, sizeof(msp_pidnames) - 1); // without terminating 0 +} + +static void pid_native2msp(const float *native, msp_pid_t *piditem, float scale, unsigned numelem) +{ + for(unsigned i = 0; i < numelem; ++i) { + piditem->values[i] = lroundf(native[i] * scale); + } +} + +static void pid_msp2native(const msp_pid_t *piditem, float *native, float scale, unsigned numelem) +{ + for(unsigned i = 0; i < numelem; ++i) { + native[i] = (float)piditem->values[i] / scale; + } +} + +static UAVObjHandle get_current_pid_bank_handle() { uint8_t fm; @@ -667,83 +698,71 @@ static StabilizationSettingsFlightModeMapOptions get_current_stabilization_bank( StabilizationSettingsFlightModeMapOptions flightModeMap[STABILIZATIONSETTINGS_FLIGHTMODEMAP_NUMELEM]; StabilizationSettingsFlightModeMapGet(flightModeMap); - return flightModeMap[fm]; -} - -static void get_current_stabilizationbankdata(StabilizationBankData *bankData) -{ - switch (get_current_stabilization_bank()) { - case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1: - StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)bankData); - break; - case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2: - StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)bankData); - break; - case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3: - StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)bankData); - break; + switch(flightModeMap[fm]) + { + case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1: + return StabilizationSettingsBank1Handle(); + case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2: + return StabilizationSettingsBank2Handle(); + case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3: + return StabilizationSettingsBank3Handle(); } -} - -static void set_current_stabilizationbankdata(const StabilizationBankData *bankData) -{ - // update just relevant parts. or not. - switch (get_current_stabilization_bank()) { - case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1: - StabilizationSettingsBank1Set((const StabilizationSettingsBank1Data *)bankData); - break; - case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2: - StabilizationSettingsBank2Set((const StabilizationSettingsBank2Data *)bankData); - break; - case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3: - StabilizationSettingsBank3Set((const StabilizationSettingsBank3Data *)bankData); - break; - } -} - -static void pid_native2msp(const float *native, msp_pid_t *piditem) -{ - piditem->P = lroundf(native[0] * 10000); - piditem->I = lroundf(native[1] * 10000); - piditem->D = lroundf(native[2] * 10000); -} - -static void pid_msp2native(const msp_pid_t *piditem, float *native) -{ - native[0] = (float)piditem->P / 10000; - native[1] = (float)piditem->I / 10000; - native[2] = (float)piditem->D / 10000; + + return 0; } static void msp_send_pid(struct msp_bridge *m) { + m->current_pid_bank = get_current_pid_bank_handle(); + StabilizationBankData bankData; - - get_current_stabilizationbankdata(&bankData); + UAVObjGetData(m->current_pid_bank, &bankData); msp_pid_t piditems[PID_ITEM_COUNT]; memset(piditems, 0, sizeof(piditems)); - pid_native2msp((float *)&bankData.RollRatePID, &piditems[PIDROLL]); - pid_native2msp((float *)&bankData.PitchRatePID, &piditems[PIDPITCH]); - pid_native2msp((float *)&bankData.YawRatePID, &piditems[PIDYAW]); + pid_native2msp((float *)&bankData.RollRatePID, &piditems[PIDROLL], 10000, 3); + pid_native2msp((float *)&bankData.PitchRatePID, &piditems[PIDPITCH], 10000, 3); + pid_native2msp((float *)&bankData.YawRatePID, &piditems[PIDYAW], 10000, 3); + + pid_native2msp((float *)&bankData.RollPI, &piditems[PIDAROLL], 10, 2); + pid_native2msp((float *)&bankData.PitchPI, &piditems[PIDAPITCH], 10, 2); + pid_native2msp((float *)&bankData.YawPI, &piditems[PIDAYAW], 10, 2); msp_send(m, MSP_PID, (const uint8_t *)piditems, sizeof(piditems)); } static void msp_set_pid(struct msp_bridge *m) { + if(m->current_pid_bank == 0) { + return; + } + StabilizationBankData bankData; + UAVObjGetData(m->current_pid_bank, &bankData); - get_current_stabilizationbankdata(&bankData); + pid_msp2native(&m->cmd_data.piditems[PIDROLL], (float *)&bankData.RollRatePID, 10000, 3); + pid_msp2native(&m->cmd_data.piditems[PIDPITCH], (float *)&bankData.PitchRatePID, 10000, 3); + pid_msp2native(&m->cmd_data.piditems[PIDYAW], (float *)&bankData.YawRatePID, 10000, 3); - pid_msp2native(&m->cmd_data.piditems[PIDROLL], (float *)&bankData.RollRatePID); - pid_msp2native(&m->cmd_data.piditems[PIDPITCH], (float *)&bankData.PitchRatePID); - pid_msp2native(&m->cmd_data.piditems[PIDYAW], (float *)&bankData.YawRatePID); - - set_current_stabilizationbankdata(&bankData); + pid_msp2native(&m->cmd_data.piditems[PIDAROLL], (float *)&bankData.RollPI, 10, 2); + pid_msp2native(&m->cmd_data.piditems[PIDAPITCH], (float *)&bankData.PitchPI, 10, 2); + pid_msp2native(&m->cmd_data.piditems[PIDAYAW], (float *)&bankData.YawPI, 10, 2); + UAVObjSetData(m->current_pid_bank, &bankData); + + bool needSave = true; + + if(needSave) { + FlightStatusArmedOptions armed; + FlightStatusArmedGet(&armed); + + if(armed == FLIGHTSTATUS_ARMED_DISARMED) { + UAVObjSave(m->current_pid_bank, 0); + } + } + msp_send(m, MSP_SET_PID, 0, 0); // send ack. } @@ -756,7 +775,6 @@ static void msp_set_pid(struct msp_bridge *m) static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m) { -#if 0 union { uint8_t buf[0]; struct { @@ -804,7 +822,6 @@ static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m) } msp_send(m, MSP_ALARMS, data.buf, len + 1); -#endif /* if 0 */ } static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b) @@ -850,6 +867,9 @@ static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b) case MSP_SET_PID: msp_set_pid(m); break; + case MSP_PIDNAMES: + msp_send_pidnames(m); + break; } return MSP_IDLE; From 95719609399fafc01edec31bee851d615ba80243 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 11 May 2016 14:53:36 +0200 Subject: [PATCH 23/23] LP-291 Fixed PID mapping --- flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index d85d59654..bf0e31b7d 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -203,11 +203,11 @@ typedef enum { PIDPITCH, PIDYAW, PIDAROLL, // (PIDALT) use this for Attitude ROLL - PIDPOS, // skipped by MWOSD + PIDAPITCH, // (PIDPOS) use this for Attitude PITCH PIDPOSR, // skipped by MWOSD - PIDAPITCH, // (PIDNAVR) use this for Attitude PITCH - PIDAYAW, // (PIDAYAW) use this for Attitude YAW - PIDMAG, + PIDNAVR, // skipped by MWOSD + PIDAYAW, // (PIDLEVEL) use this for Attitude YAW + PIDMAG, // unused for now PIDVEL, // skipped by MWOSD PID_ITEM_COUNT } pidIndex_e; @@ -216,10 +216,11 @@ static const char msp_pidnames[] = "ROLL;" "PITCH;" "YAW;" "A.ROLL;" - "Pos;" - "PosR;" "A.PITCH;" + "PosR;" + "NavR;" "A.YAW;" + "MAG;" "VEL;"; #define MSP_ANALOG_VOLTAGE (1 << 0)