From f04d44de30ad5745fb424a6a263785046c9271d1 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 22 Apr 2016 03:19:51 +0200 Subject: [PATCH] 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. - - + + - - - - - + + + + + -