flight/modules/UAVOMSPBridge/UAVOMSPBridge.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup UAVOMSPBridge UAVO to MSP Bridge Module
   6  * @{
   7  *
   8  * @file       uavomspbridge.c
   9  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  10  *             Tau Labs, http://taulabs.org, Copyright (C) 2015
  11  *             dRonin, http://dronin.org Copyright (C) 2015-2016
  12  * @brief      Bridges selected UAVObjects to MSP for MWOSD and the like.
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  *****************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  *
  32  * Additional note on redistribution: The copyright and license notices above
  33  * must be maintained in each individual source file that is a derivative work
  34  * of this source file; otherwise redistribution is prohibited.
  35  */
  36
  37 #include "openpilot.h"
  38 #include "receiverstatus.h"
  39 #include "hwsettings.h"
  40 #include "flightmodesettings.h"
  41 #include "flightbatterysettings.h"
  42 #include "flightbatterystate.h"
  43 #include "gpspositionsensor.h"
  44 #include "manualcontrolcommand.h"
  45 #include "manualcontrolsettings.h"
  46 #include "oplinkstatus.h"
  47 #include "accessorydesired.h"
  48 #include "attitudestate.h"
  49 #include "airspeedstate.h"
  50 #include "actuatorsettings.h"
  51 #include "actuatordesired.h"
  52 #include "flightstatus.h"
  53 #include "systemstats.h"
  54 #include "systemalarms.h"
  55 #include "takeofflocation.h"
  56 #include "homelocation.h"
  57 #include "positionstate.h"
  58 #include "velocitystate.h"
  59 #include "stabilizationdesired.h"
  60 #include "taskinfo.h"
  61 #include "stabilizationsettings.h"
  62 #include "stabilizationbank.h"
  63 #include "stabilizationsettingsbank1.h"
  64 #include "stabilizationsettingsbank2.h"
  65 #include "stabilizationsettingsbank3.h"
  66 #include "magstate.h"
  67 #include "objectpersistence.h"
  68
  69 #include "pios_sensors.h"
  70
  71
  72 #define PIOS_INCLUDE_MSP_BRIDGE
  73
  74 #if defined(PIOS_INCLUDE_MSP_BRIDGE)
  75
  76 // oplink rssi - absolute low : -127 dBm noise floor (set by software when link is completely lost)
  77 // in reality  : around -110 dBm
  78 // max: various articles found on web quote -10 dBm as maximum received signal power.
  79
  80 #define OPLINK_LOW_RSSI  -110
  81 #define OPLINK_HIGH_RSSI -10
  82
  83 #define MSP_SENSOR_ACC   (1 << 0)
  84 #define MSP_SENSOR_BARO  (1 << 1)
  85 #define MSP_SENSOR_MAG   (1 << 2)
  86 #define MSP_SENSOR_GPS   (1 << 3)
  87 #define MSP_SENSOR_SONAR (1 << 4)
  88
  89 // Magic numbers copied from mwosd
  90 #define  MSP_IDENT       100 // multitype + multiwii version + protocol version + capability variable
  91 #define  MSP_STATUS      101 // cycletime & errors_count & sensor present & box activation & current setting number
  92 #define  MSP_RAW_IMU     102 // 9 DOF
  93 #define  MSP_SERVO       103 // 8 servos
  94 #define  MSP_MOTOR       104 // 8 motors
  95 #define  MSP_RC          105 // 8 rc chan and more
  96 #define  MSP_RAW_GPS     106 // fix, numsat, lat, lon, alt, speed, ground course
  97 #define  MSP_COMP_GPS    107 // distance home, direction home
  98 #define  MSP_ATTITUDE    108 // 2 angles 1 heading
  99 #define  MSP_ALTITUDE    109 // altitude, variometer
 100 #define  MSP_ANALOG      110 // vbat, powermetersum, rssi if available on RX
 101 #define  MSP_RC_TUNING   111 // rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
 102 #define  MSP_PID         112 // P I D coeff (10 are used currently)
 103 #define  MSP_BOX         113 // BOX setup (number is dependant of your setup)
 104 #define  MSP_MISC        114 // powermeter trig
 105 #define  MSP_MOTOR_PINS  115 // which pins are in use for motors & servos, for GUI
 106 #define  MSP_BOXNAMES    116 // the aux switch names
 107 #define  MSP_PIDNAMES    117 // the PID names
 108 #define  MSP_BOXIDS      119 // get the permanent IDs associated to BOXes
 109 #define  MSP_NAV_STATUS  121 // Returns navigation status
 110 #define  MSP_CELLS       130 // FrSky SPort Telemtry
 111 #define  MSP_ALARMS      242 // Alarm request
 112
 113 #define  MSP_SET_PID     202 // set P I D coeff
 114
 115 typedef enum {
 116     MSP_BOX_ID_ARM,
 117     MSP_BOX_ID_ANGLE, // mode.stable (attitude??) - [sensor icon] [ fligth mode icon ]
 118     MSP_BOX_ID_HORIZON, // [sensor icon] [ flight mode icon ]
 119     MSP_BOX_ID_BARO, // [sensor icon]
 120     MSP_BOX_ID_VARIO,
 121     MSP_BOX_ID_MAG, // [sensor icon]
 122     MSP_BOX_ID_HEADFREE,
 123     MSP_BOX_ID_HEADADJ,
 124     MSP_BOX_ID_CAMSTAB, // [gimbal icon]
 125     MSP_BOX_ID_CAMTRIG,
 126     MSP_BOX_ID_GPSHOME, // [ flight mode icon ]
 127     MSP_BOX_ID_GPSHOLD, // [ flight mode icon ]
 128     MSP_BOX_ID_PASSTHRU, // [ flight mode icon ]
 129     MSP_BOX_ID_BEEPER,
 130     MSP_BOX_ID_LEDMAX,
 131     MSP_BOX_ID_LEDLOW,
 132     MSP_BOX_ID_LLIGHTS, // landing lights
 133     MSP_BOX_ID_CALIB,
 134     MSP_BOX_ID_GOVERNOR,
 135     MSP_BOX_ID_OSD_SWITCH, // OSD on or off, maybe.
 136     MSP_BOX_ID_GPSMISSION, // [ flight mode icon ]
 137     MSP_BOX_ID_GPSLAND, // [ flight mode icon ]
 138
 139     MSP_BOX_ID_AIR = 28, // this box will add AIRMODE icon to flight mode.
 140     MSP_BOX_ID_ACROPLUS = 29, // this will add PLUS icon to ACRO. ACRO = !ANGLE && !HORIZON
 141 } msp_boxid_t;
 142
 143 enum {
 144     MSP_STATUS_ARMED,
 145     MSP_STATUS_FLIGHTMODE_STABILIZED,
 146     MSP_STATUS_FLIGHTMODE_HORIZON,
 147     MSP_STATUS_SENSOR_BARO,
 148     MSP_STATUS_SENSOR_MAG,
 149     MSP_STATUS_MISC_GIMBAL,
 150     MSP_STATUS_FLIGHTMODE_GPSHOME,
 151     MSP_STATUS_FLIGHTMODE_GPSHOLD,
 152     MSP_STATUS_FLIGHTMODE_GPSMISSION,
 153     MSP_STATUS_FLIGHTMODE_GPSLAND,
 154     MSP_STATUS_FLIGHTMODE_PASSTHRU,
 155     MSP_STATUS_OSD_SWITCH,
 156     MSP_STATUS_ICON_AIRMODE,
 157     MSP_STATUS_ICON_ACROPLUS,
 158 };
 159
 160 static const uint8_t msp_boxes[] = {
 161     [MSP_STATUS_ARMED] = MSP_BOX_ID_ARM,
 162     [MSP_STATUS_FLIGHTMODE_STABILIZED] = MSP_BOX_ID_ANGLE, // flight mode
 163     [MSP_STATUS_FLIGHTMODE_HORIZON]    = MSP_BOX_ID_HORIZON, // flight mode
 164     [MSP_STATUS_SENSOR_BARO] = MSP_BOX_ID_BARO,      // sensor icon only
 165     [MSP_STATUS_SENSOR_MAG]  = MSP_BOX_ID_MAG, // sensor icon only
 166     [MSP_STATUS_MISC_GIMBAL] = MSP_BOX_ID_CAMSTAB,   // gimbal icon only
 167     [MSP_STATUS_FLIGHTMODE_GPSHOME]    = MSP_BOX_ID_GPSHOME, // flight mode
 168     [MSP_STATUS_FLIGHTMODE_GPSHOLD]    = MSP_BOX_ID_GPSHOLD, // flight mode
 169     [MSP_STATUS_FLIGHTMODE_GPSMISSION] = MSP_BOX_ID_GPSMISSION, // flight mode
 170     [MSP_STATUS_FLIGHTMODE_GPSLAND]    = MSP_BOX_ID_GPSLAND, // flight mode
 171     [MSP_STATUS_FLIGHTMODE_PASSTHRU]   = MSP_BOX_ID_PASSTHRU, // flight mode
 172     [MSP_STATUS_OSD_SWITCH]    = MSP_BOX_ID_OSD_SWITCH, // switch OSD mode
 173     [MSP_STATUS_ICON_AIRMODE]  = MSP_BOX_ID_AIR,
 174     [MSP_STATUS_ICON_ACROPLUS] = MSP_BOX_ID_ACROPLUS,
 175 };
 176
 177 typedef enum {
 178     MSP_IDLE,
 179     MSP_HEADER_START,
 180     MSP_HEADER_M,
 181     MSP_HEADER_SIZE,
 182     MSP_HEADER_CMD,
 183     MSP_FILLBUF,
 184     MSP_CHECKSUM,
 185     MSP_DISCARD,
 186     MSP_MAYBE_UAVTALK2,
 187     MSP_MAYBE_UAVTALK3,
 188     MSP_MAYBE_UAVTALK4,
 189     MSP_MAYBE_UAVTALK_SLOW2,
 190     MSP_MAYBE_UAVTALK_SLOW3,
 191     MSP_MAYBE_UAVTALK_SLOW4,
 192     MSP_MAYBE_UAVTALK_SLOW5,
 193     MSP_MAYBE_UAVTALK_SLOW6
 194 } msp_state;
 195
 196
 197 typedef struct __attribute__((packed)) {
 198     uint8_t values[3];
 199 }
 200 msp_pid_t;
 201
 202 typedef enum {
 203     PIDROLL,
 204     PIDPITCH,
 205     PIDYAW,
 206     PIDAROLL, // (PIDALT) use this for Attitude ROLL
 207     PIDAPITCH, // (PIDPOS) use this for Attitude PITCH
 208     PIDPOSR, // skipped by MWOSD
 209     PIDNAVR, // skipped by MWOSD
 210     PIDAYAW, // (PIDLEVEL) use this for Attitude YAW
 211     PIDMAG, // unused for now
 212     PIDVEL, // skipped by MWOSD
 213     PID_ITEM_COUNT
 214 } pidIndex_e;
 215
 216 static const char msp_pidnames[] = "ROLL;"
 217                                    "PITCH;"
 218                                    "YAW;"
 219                                    "A.ROLL;"
 220                                    "A.PITCH;"
 221                                    "PosR;"
 222                                    "NavR;"
 223                                    "A.YAW;"
 224                                    "MAG;"
 225                                    "VEL;";
 226
 227 #define MSP_ANALOG_VOLTAGE (1 << 0)
 228 #define MSP_ANALOG_CURRENT (1 << 1)
 229
 230 struct msp_bridge {
 231     uintptr_t    com;
 232
 233     uint8_t      sensors;
 234     uint8_t      analog;
 235
 236     UAVObjHandle current_pid_bank;
 237
 238     msp_state    state;
 239     uint8_t      cmd_size;
 240     uint8_t      cmd_id;
 241     uint8_t      cmd_i;
 242     uint8_t      checksum;
 243     union {
 244         uint8_t   data[0];
 245         // Specific packed data structures go here.
 246         msp_pid_t piditems[PID_ITEM_COUNT];
 247     } cmd_data;
 248 };
 249
 250 #if defined(PIOS_MSP_STACK_SIZE)
 251 #define STACK_SIZE_BYTES     PIOS_MSP_STACK_SIZE
 252 #else
 253 #define STACK_SIZE_BYTES     768
 254 #endif
 255 #define TASK_PRIORITY        (tskIDLE_PRIORITY)
 256
 257 #define MAX_ALARM_LEN        30
 258
 259 #define BOOT_DISPLAY_TIME_MS (10 * 1000)
 260
 261 static bool module_enabled = false;
 262 static struct msp_bridge *msp;
 263 static int32_t uavoMSPBridgeInitialize(void);
 264 static void uavoMSPBridgeTask(void *parameters);
 265
 266 static void msp_send(struct msp_bridge *m, uint8_t cmd, const uint8_t *data, size_t len)
 267 {
 268     uint8_t buf[5];
 269     uint8_t cs = (uint8_t)(len) ^ cmd;
 270
 271     buf[0] = '$';
 272     buf[1] = 'M';
 273     buf[2] = '>';
 274     buf[3] = (uint8_t)(len);
 275     buf[4] = cmd;
 276
 277     PIOS_COM_SendBuffer(m->com, buf, sizeof(buf));
 278
 279     if (len > 0) {
 280         PIOS_COM_SendBuffer(m->com, data, len);
 281
 282         for (unsigned i = 0; i < len; i++) {
 283             cs ^= data[i];
 284         }
 285     }
 286
 287     cs    ^= 0;
 288
 289     buf[0] = cs;
 290     PIOS_COM_SendBuffer(m->com, buf, 1);
 291 }
 292
 293 static msp_state msp_state_size(struct msp_bridge *m, uint8_t b)
 294 {
 295     m->cmd_size = b;
 296     m->checksum = b;
 297     return MSP_HEADER_CMD;
 298 }
 299
 300 static msp_state msp_state_cmd(struct msp_bridge *m, uint8_t b)
 301 {
 302     m->cmd_i     = 0;
 303     m->cmd_id    = b;
 304     m->checksum ^= m->cmd_id;
 305
 306     if (m->cmd_size > sizeof(m->cmd_data)) {
 307         // Too large a body.  Let's ignore it.
 308         return MSP_DISCARD;
 309     }
 310
 311     return m->cmd_size == 0 ? MSP_CHECKSUM : MSP_FILLBUF;
 312 }
 313
 314 static msp_state msp_state_fill_buf(struct msp_bridge *m, uint8_t b)
 315 {
 316     m->cmd_data.data[m->cmd_i++] = b;
 317     m->checksum ^= b;
 318     return m->cmd_i == m->cmd_size ? MSP_CHECKSUM : MSP_FILLBUF;
 319 }
 320
 321 static void msp_send_attitude(struct msp_bridge *m)
 322 {
 323     union {
 324         uint8_t buf[0];
 325         struct {
 326             int16_t x;
 327             int16_t y;
 328             int16_t h;
 329         } att;
 330     } data;
 331     AttitudeStateData attState;
 332
 333     AttitudeStateGet(&attState);
 334
 335     // Roll and Pitch are in 10ths of a degree.
 336     data.att.x = attState.Roll * 10;
 337     data.att.y = attState.Pitch * -10;
 338     // Yaw is just -180 -> 180
 339     data.att.h = attState.Yaw;
 340
 341     msp_send(m, MSP_ATTITUDE, data.buf, sizeof(data));
 342 }
 343
 344 #define IS_STAB_MODE(d, m) (((d).Roll == (m)) && ((d).Pitch == (m)))
 345
 346 static void msp_send_status(struct msp_bridge *m)
 347 {
 348     union {
 349         uint8_t buf[0];
 350         struct {
 351             uint16_t cycleTime;
 352             uint16_t i2cErrors;
 353             uint16_t sensors;
 354             uint32_t flags;
 355             uint8_t  setting;
 356         } __attribute__((packed)) status;
 357     } data;
 358     // TODO: https://github.com/TauLabs/TauLabs/blob/next/shared/uavobjectdefinition/actuatordesired.xml#L8
 359     data.status.cycleTime = 0;
 360     data.status.i2cErrors = 0;
 361
 362     data.status.sensors   = m->sensors;
 363
 364     if (GPSPositionSensorHandle() != NULL) {
 365         GPSPositionSensorStatusOptions gpsStatus;
 366         GPSPositionSensorStatusGet(&gpsStatus);
 367
 368         if (gpsStatus != GPSPOSITIONSENSOR_STATUS_NOGPS) {
 369             data.status.sensors |= MSP_SENSOR_GPS;
 370         }
 371     }
 372
 373     data.status.flags   = 0;
 374     data.status.setting = 0;
 375
 376     FlightStatusData flightStatus;
 377     FlightStatusGet(&flightStatus);
 378
 379     StabilizationDesiredStabilizationModeData stabModeData;
 380     StabilizationDesiredStabilizationModeGet(&stabModeData);
 381
 382     if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
 383         data.status.flags |= (1 << MSP_STATUS_ARMED);
 384     }
 385
 386     // flightmode
 387
 388     if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) {
 389         data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSHOLD);
 390     } else if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) {
 391         data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSHOME);
 392     } else if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
 393         data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSMISSION);
 394     } else if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
 395         data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_GPSLAND);
 396     } else if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_MANUAL) {
 397         data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_PASSTHRU);
 398     } else { //
 399              // if Roll(Rate) && Pitch(Rate) => Acro
 400              // if Roll(Acro+) && Pitch(Acro+) => Acro+
 401              // if Roll(Rattitude) && Pitch(Rattitude) => Horizon
 402              // else => Stabilized
 403
 404         if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATE)
 405             || IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER)) {
 406             // data.status.flags should not set any mode flags
 407         } else if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO)) { // this is Acro+ mode
 408             data.status.flags |= (1 << MSP_STATUS_ICON_ACROPLUS);
 409         } else if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE)) {
 410             data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_HORIZON);
 411         } else { // looks like stabilized mode, whichever it is..
 412             data.status.flags |= (1 << MSP_STATUS_FLIGHTMODE_STABILIZED);
 413         }
 414     }
 415
 416     // sensors in use?
 417     // flight mode HORIZON or STABILIZED will turn on Accelerometer icon.
 418     // Barometer?
 419
 420     if ((stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD)
 421         || (stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO)) {
 422         data.status.flags |= (1 << MSP_STATUS_SENSOR_BARO);
 423     }
 424
 425 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 426     if (MagStateHandle() != NULL) {
 427         MagStateSourceOptions magSource;
 428         MagStateSourceGet(&magSource);
 429
 430         if (magSource != MAGSTATE_SOURCE_INVALID) {
 431             data.status.flags |= (1 << MSP_STATUS_SENSOR_MAG);
 432         }
 433     }
 434 #endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
 435
 436     if (flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE) {
 437         data.status.flags |= (1 << MSP_STATUS_ICON_AIRMODE);
 438     }
 439
 440     msp_send(m, MSP_STATUS, data.buf, sizeof(data));
 441 }
 442
 443 static void msp_send_analog(struct msp_bridge *m)
 444 {
 445     union {
 446         uint8_t buf[0];
 447         struct {
 448             uint8_t  vbat;
 449             uint16_t powerMeterSum;
 450             uint16_t rssi;
 451             uint16_t current;
 452         } __attribute__((packed)) status;
 453     } data;
 454
 455     memset(&data, 0, sizeof(data));
 456
 457 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 458     if (FlightBatteryStateHandle() != NULL) {
 459         FlightBatteryStateData batState;
 460         FlightBatteryStateGet(&batState);
 461
 462         if (m->analog & MSP_ANALOG_VOLTAGE) {
 463             data.status.vbat = (uint8_t)lroundf(batState.Voltage * 10);
 464         }
 465         if (m->analog & MSP_ANALOG_CURRENT) {
 466             data.status.current = lroundf(batState.Current * 100);
 467             data.status.powerMeterSum = lroundf(batState.ConsumedEnergy);
 468         }
 469     }
 470 #endif
 471     // OPLink supports RSSI reported in dBm, but updates different value in ReceiverStatus.Quality (link_quality - function of lost, repaired and good packet count).
 472     // We use same method as in Receiver module to decide if oplink is used for control:
 473     // Check for Throttle channel group, if this belongs to oplink, we will use oplink rssi instead of ReceiverStatus.Quality.
 474
 475     ManualControlSettingsChannelGroupsData channelGroups;
 476     ManualControlSettingsChannelGroupsGet(&channelGroups);
 477
 478 #ifdef PIOS_INCLUDE_OPLINKRCVR
 479     if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) {
 480         int8_t rssi;
 481         OPLinkStatusRSSIGet(&rssi);
 482
 483         // 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
 484         if (rssi < OPLINK_LOW_RSSI) {
 485             rssi = OPLINK_LOW_RSSI;
 486         } else if (rssi > OPLINK_HIGH_RSSI) {
 487             rssi = OPLINK_HIGH_RSSI;
 488         }
 489
 490         data.status.rssi = ((rssi - OPLINK_LOW_RSSI) * 1023) / (OPLINK_HIGH_RSSI - OPLINK_LOW_RSSI);
 491     } else {
 492 #endif /* PIOS_INCLUDE_OPLINKRCVR */
 493     uint8_t quality;
 494     ReceiverStatusQualityGet(&quality);
 495
 496     // MSP RSSI's range is 0-1023
 497     data.status.rssi = (quality * 1023) / 100;
 498 #ifdef PIOS_INCLUDE_OPLINKRCVR
 499 }
 500 #endif /* PIOS_INCLUDE_OPLINKRCVR */
 501
 502     if (data.status.rssi > 1023) {
 503         data.status.rssi = 1023;
 504     }
 505
 506     msp_send(m, MSP_ANALOG, data.buf, sizeof(data));
 507 }
 508
 509 static void msp_send_raw_gps(struct msp_bridge *m)
 510 {
 511     union {
 512         uint8_t buf[0];
 513         struct {
 514             uint8_t  fix;                 // 0 or 1
 515             uint8_t  num_sat;
 516             int32_t  lat;                  // 1 / 10 000 000 deg
 517             int32_t  lon;                  // 1 / 10 000 000 deg
 518             uint16_t alt; // meter
 519             uint16_t speed; // cm/s
 520             int16_t  ground_course;        // degree * 10
 521         } __attribute__((packed)) raw_gps;
 522     } data;
 523
 524     GPSPositionSensorData gps_data;
 525
 526     if (GPSPositionSensorHandle() != NULL) {
 527         GPSPositionSensorGet(&gps_data);
 528
 529         data.raw_gps.fix     = (gps_data.Status >= GPSPOSITIONSENSOR_STATUS_FIX2D ? 1 : 0);  // Data will display on OSD if 2D fix or better
 530         data.raw_gps.num_sat = gps_data.Satellites;
 531         data.raw_gps.lat     = gps_data.Latitude;
 532         data.raw_gps.lon     = gps_data.Longitude;
 533         data.raw_gps.alt     = (uint16_t)gps_data.Altitude;
 534         data.raw_gps.speed   = (uint16_t)gps_data.Groundspeed;
 535         data.raw_gps.ground_course = (int16_t)(gps_data.Heading * 10.0f);
 536
 537         msp_send(m, MSP_RAW_GPS, data.buf, sizeof(data));
 538     }
 539 }
 540
 541 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 542
 543 static inline float Sq(float x)
 544 {
 545     return x * x;
 546 }
 547
 548 static void msp_send_comp_gps(struct msp_bridge *m)
 549 {
 550     union {
 551         uint8_t buf[0];
 552         struct {
 553             uint16_t distance_to_home; // meter
 554             int16_t  direction_to_home;    // degree [-180:180]
 555             uint8_t  home_position_valid;  // 0 = Invalid, Dronin MSP specific
 556         } __attribute__((packed)) comp_gps;
 557     } data;
 558
 559     data.comp_gps.distance_to_home    = 0;
 560     data.comp_gps.direction_to_home   = 0;
 561     data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD
 562
 563     if (PositionStateHandle() && TakeOffLocationHandle()) {
 564         TakeOffLocationData takeOffLocation;
 565         TakeOffLocationGet(&takeOffLocation);
 566
 567         if (takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
 568             PositionStateData positionState;
 569             PositionStateGet(&positionState);
 570
 571             data.comp_gps.distance_to_home    = (uint16_t)sqrtf(Sq(takeOffLocation.East - positionState.East) + Sq(takeOffLocation.North - positionState.North));
 572             data.comp_gps.direction_to_home   = RAD2DEG(atan2f(takeOffLocation.East - positionState.East, takeOffLocation.North - positionState.North));
 573
 574             data.comp_gps.home_position_valid = 1;
 575         }
 576     }
 577
 578     // here, CC3D implementation could use raw gps data (GPSPositionSensorData) and locally cached GPSPositionSensorData at arming time as TakeOffLocation.
 579
 580
 581     msp_send(m, MSP_COMP_GPS, data.buf, sizeof(data));
 582 }
 583
 584 static void msp_send_altitude(struct msp_bridge *m)
 585 {
 586     union {
 587         uint8_t buf[0];
 588         struct {
 589             int32_t estimatedAltitude; // cm
 590             int16_t vario; // cm/s
 591         } __attribute__((packed)) altitude;
 592     } data;
 593
 594
 595     if (PositionStateHandle() != NULL) {
 596         PositionStateData positionState;
 597         PositionStateGet(&positionState);
 598
 599         data.altitude.estimatedAltitude = (int32_t)roundf(positionState.Down * -100.0f);
 600     } else {
 601         data.altitude.estimatedAltitude = 0;
 602     }
 603
 604     if (VelocityStateHandle() != NULL) {
 605         VelocityStateData velocityState;
 606         VelocityStateGet(&velocityState);
 607
 608         data.altitude.vario = (int16_t)roundf(velocityState.Down * -100.0f);
 609     } else {
 610         data.altitude.vario = 0;
 611     }
 612
 613     msp_send(m, MSP_ALTITUDE, data.buf, sizeof(data));
 614 }
 615 #endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
 616
 617
 618 // Scale stick values whose input range is -1 to 1 to MSP's expected
 619 // PWM range of 1000-2000.
 620 static uint16_t msp_scale_rc(float percent)
 621 {
 622     return percent * 500 + 1500;
 623 }
 624
 625 // Throttle maps to 1100-1900 and a bit differently as -1 == 1000 and
 626 // then jumps immediately to 0 -> 1 for the rest of the range.  MWOSD
 627 // can learn ranges as wide as they are sent, but defaults to
 628 // 1100-1900 so the throttle indicator will vary for the same stick
 629 // position unless we send it what it wants right away.
 630 static uint16_t msp_scale_rc_thr(float percent)
 631 {
 632     return percent <= 0 ? 1100 : percent * 800 + 1100;
 633 }
 634
 635 // MSP RC order is Roll/Pitch/Yaw/Throttle/AUX1/AUX2/AUX3/AUX4
 636 static void msp_send_channels(struct msp_bridge *m)
 637 {
 638     AccessoryDesiredData acc0, acc1, acc2, acc3;
 639     ManualControlCommandData manualState;
 640
 641     ManualControlCommandGet(&manualState);
 642     AccessoryDesiredInstGet(0, &acc0);
 643     AccessoryDesiredInstGet(1, &acc1);
 644     AccessoryDesiredInstGet(2, &acc2);
 645     AccessoryDesiredInstGet(3, &acc3);
 646
 647     union {
 648         uint8_t  buf[0];
 649         uint16_t channels[8];
 650     } data = {
 651         .channels = {
 652             msp_scale_rc(manualState.Roll),
 653             msp_scale_rc(manualState.Pitch * -1),  // TL pitch is backwards
 654             msp_scale_rc(manualState.Yaw),
 655             msp_scale_rc_thr(manualState.Throttle),
 656             msp_scale_rc(acc0.AccessoryVal),
 657             msp_scale_rc(acc1.AccessoryVal),
 658             msp_scale_rc(acc2.AccessoryVal),
 659             msp_scale_rc(acc3.AccessoryVal),
 660         }
 661     };
 662
 663     msp_send(m, MSP_RC, data.buf, sizeof(data));
 664 }
 665
 666 static void msp_send_boxids(struct msp_bridge *m) // This is actually sending a map of MSP_STATUS.flag bits to BOX ids.
 667 {
 668     msp_send(m, MSP_BOXIDS, msp_boxes, sizeof(msp_boxes));
 669 }
 670
 671 static void msp_send_pidnames(struct msp_bridge *m)
 672 {
 673     msp_send(m, MSP_PIDNAMES, (const uint8_t *)msp_pidnames, sizeof(msp_pidnames) - 1); // without terminating 0
 674 }
 675
 676 static void pid_native2msp(const float *native, msp_pid_t *piditem, float scale, unsigned numelem)
 677 {
 678     for (unsigned i = 0; i < numelem; ++i) {
 679         // Handle Dterm scale
 680         float s = (i == 2) ? scale * 100 : scale;
 681         piditem->values[i] = lroundf(native[i] * s);
 682     }
 683 }
 684
 685 static void pid_msp2native(const msp_pid_t *piditem, float *native, float scale, unsigned numelem)
 686 {
 687     for (unsigned i = 0; i < numelem; ++i) {
 688         // Handle Dterm scale
 689         float s = (i == 2) ? scale * 100 : scale;
 690         native[i] = (float)piditem->values[i] / s;
 691     }
 692 }
 693
 694 static UAVObjHandle get_current_pid_bank_handle()
 695 {
 696     uint8_t fm;
 697
 698     ManualControlCommandFlightModeSwitchPositionGet(&fm);
 699
 700     if (fm >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
 701         return STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1;
 702     }
 703
 704     StabilizationSettingsFlightModeMapOptions flightModeMap[STABILIZATIONSETTINGS_FLIGHTMODEMAP_NUMELEM];
 705     StabilizationSettingsFlightModeMapGet(flightModeMap);
 706
 707     switch (flightModeMap[fm]) {
 708     case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1:
 709         return StabilizationSettingsBank1Handle();
 710
 711     case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2:
 712         return StabilizationSettingsBank2Handle();
 713
 714     case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3:
 715         return StabilizationSettingsBank3Handle();
 716     }
 717
 718     return 0;
 719 }
 720
 721 static void msp_send_pid(struct msp_bridge *m)
 722 {
 723     m->current_pid_bank = get_current_pid_bank_handle();
 724
 725     StabilizationBankData bankData;
 726     UAVObjGetData(m->current_pid_bank, &bankData);
 727
 728     msp_pid_t piditems[PID_ITEM_COUNT];
 729
 730     memset(piditems, 0, sizeof(piditems));
 731
 732     pid_native2msp((float *)&bankData.RollRatePID, &piditems[PIDROLL], 10000, 3);
 733     pid_native2msp((float *)&bankData.PitchRatePID, &piditems[PIDPITCH], 10000, 3);
 734     pid_native2msp((float *)&bankData.YawRatePID, &piditems[PIDYAW], 10000, 3);
 735
 736     pid_native2msp((float *)&bankData.RollPI, &piditems[PIDAROLL], 10, 2);
 737     pid_native2msp((float *)&bankData.PitchPI, &piditems[PIDAPITCH], 10, 2);
 738     pid_native2msp((float *)&bankData.YawPI, &piditems[PIDAYAW], 10, 2);
 739
 740     msp_send(m, MSP_PID, (const uint8_t *)piditems, sizeof(piditems));
 741 }
 742
 743 static void msp_set_pid(struct msp_bridge *m)
 744 {
 745     if (m->current_pid_bank == 0) {
 746         return;
 747     }
 748
 749     StabilizationBankData bankData;
 750     UAVObjGetData(m->current_pid_bank, &bankData);
 751
 752     pid_msp2native(&m->cmd_data.piditems[PIDROLL], (float *)&bankData.RollRatePID, 10000, 3);
 753     pid_msp2native(&m->cmd_data.piditems[PIDPITCH], (float *)&bankData.PitchRatePID, 10000, 3);
 754     pid_msp2native(&m->cmd_data.piditems[PIDYAW], (float *)&bankData.YawRatePID, 10000, 3);
 755
 756     pid_msp2native(&m->cmd_data.piditems[PIDAROLL], (float *)&bankData.RollPI, 10, 2);
 757     pid_msp2native(&m->cmd_data.piditems[PIDAPITCH], (float *)&bankData.PitchPI, 10, 2);
 758     pid_msp2native(&m->cmd_data.piditems[PIDAYAW], (float *)&bankData.YawPI, 10, 2);
 759
 760     UAVObjSetData(m->current_pid_bank, &bankData);
 761
 762     bool needSave = true;
 763
 764     if (needSave) {
 765         FlightStatusArmedOptions armed;
 766         FlightStatusArmedGet(&armed);
 767
 768         if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
 769             ObjectPersistenceData op;
 770
 771             op.ObjectID   = UAVObjGetID(m->current_pid_bank);
 772             op.InstanceID = 0;
 773             op.Selection  = OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT;
 774             op.Operation  = OBJECTPERSISTENCE_OPERATION_SAVE;
 775
 776             ObjectPersistenceSet(&op);
 777         }
 778     }
 779
 780     msp_send(m, MSP_SET_PID, 0, 0); // send ack.
 781 }
 782
 783 #define ALARM_OK    0
 784 #define ALARM_WARN  1
 785 #define ALARM_ERROR 2
 786 #define ALARM_CRIT  3
 787
 788 #define MS2TICKS(m) ((m) / (portTICK_RATE_MS))
 789
 790 static void msp_send_alarms(__attribute__((unused)) struct msp_bridge *m)
 791 {
 792     union {
 793         uint8_t buf[0];
 794         struct {
 795             uint8_t state;
 796             char    msg[MAX_ALARM_LEN];
 797         } __attribute__((packed)) alarm;
 798     } data;
 799
 800     SystemAlarmsData alarm;
 801     SystemAlarmsGet(&alarm);
 802
 803     // Special case early boot times -- just report boot reason
 804
 805 #if 0
 806     if (xTaskGetTickCount() < MS2TICKS(10 * 1000)) {
 807         data.alarm.state = ALARM_CRIT;
 808         const char *boot_reason = AlarmBootReason(alarm.RebootCause);
 809         strncpy((char *)data.alarm.msg, boot_reason, MAX_ALARM_LEN);
 810         data.alarm.msg[MAX_ALARM_LEN - 1] = '\0';
 811         msp_send(m, MSP_ALARMS, data.buf, strlen((char *)data.alarm.msg) + 1);
 812         return;
 813     }
 814 #endif
 815
 816     uint8_t state;
 817     data.alarm.state = ALARM_OK;
 818     int32_t len = AlarmString(&alarm, data.alarm.msg,
 819                               sizeof(data.alarm.msg), SYSTEMALARMS_ALARM_CRITICAL, &state); // Include only CRITICAL and ERROR
 820
 821     // NOTE: LP alarm severity levels and MSP levels do not match. ERROR and CRITICAL are swapped.
 822     // So far, MW-OSD code (MSP consumer) does not make difference between ALARM_ERROR and ALARM_CRITICAL.
 823     // ALARM_WARN should be blinking if thats the highest severity level at the moment.
 824     // There might be other types of MSP consumers.
 825
 826     switch (state) {
 827     case SYSTEMALARMS_ALARM_WARNING:
 828         data.alarm.state = ALARM_WARN;
 829         break;
 830     case SYSTEMALARMS_ALARM_ERROR:
 831         data.alarm.state = ALARM_CRIT;
 832         break;
 833     case SYSTEMALARMS_ALARM_CRITICAL:
 834         data.alarm.state = ALARM_ERROR;
 835         break;
 836     }
 837
 838     msp_send(m, MSP_ALARMS, data.buf, len + 1);
 839 }
 840
 841 static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b)
 842 {
 843     if ((m->checksum ^ b) != 0) {
 844         return MSP_IDLE;
 845     }
 846
 847     // Respond to interesting things.
 848     switch (m->cmd_id) {
 849     case MSP_RAW_GPS:
 850         msp_send_raw_gps(m);
 851         break;
 852 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 853     case MSP_COMP_GPS:
 854         msp_send_comp_gps(m);
 855         break;
 856     case MSP_ALTITUDE:
 857         msp_send_altitude(m);
 858         break;
 859 #endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
 860     case MSP_ATTITUDE:
 861         msp_send_attitude(m);
 862         break;
 863     case MSP_STATUS:
 864         msp_send_status(m);
 865         break;
 866     case MSP_ANALOG:
 867         msp_send_analog(m);
 868         break;
 869     case MSP_RC:
 870         msp_send_channels(m);
 871         break;
 872     case MSP_BOXIDS:
 873         msp_send_boxids(m);
 874         break;
 875     case MSP_ALARMS:
 876         msp_send_alarms(m);
 877         break;
 878     case MSP_PID:
 879         msp_send_pid(m);
 880         break;
 881     case MSP_SET_PID:
 882         msp_set_pid(m);
 883         break;
 884     case MSP_PIDNAMES:
 885         msp_send_pidnames(m);
 886         break;
 887     }
 888
 889     return MSP_IDLE;
 890 }
 891
 892 static msp_state msp_state_discard(struct msp_bridge *m, __attribute__((unused)) uint8_t b)
 893 {
 894     return m->cmd_i++ == m->cmd_size ? MSP_IDLE : MSP_DISCARD;
 895 }
 896
 897 /**
 898  * Process incoming bytes from an MSP query thing.
 899  * @param[in] b received byte
 900  * @return true if we should continue processing bytes
 901  */
 902 static bool msp_receive_byte(struct msp_bridge *m, uint8_t b)
 903 {
 904     switch (m->state) {
 905     case MSP_IDLE:
 906         switch (b) {
 907         case 0xe0: // uavtalk matching first part of 0x3c @ 57600 baud
 908             m->state = MSP_MAYBE_UAVTALK_SLOW2;
 909             break;
 910         case '<': // uavtalk matching with 0x3c 0x2x 0xxx 0x0x
 911             m->state = MSP_MAYBE_UAVTALK2;
 912             break;
 913         case '$':
 914             m->state = MSP_HEADER_START;
 915             break;
 916         default:
 917             m->state = MSP_IDLE;
 918         }
 919         break;
 920     case MSP_HEADER_START:
 921         m->state = b == 'M' ? MSP_HEADER_M : MSP_IDLE;
 922         break;
 923     case MSP_HEADER_M:
 924         m->state = b == '<' ? MSP_HEADER_SIZE : MSP_IDLE;
 925         break;
 926     case MSP_HEADER_SIZE:
 927         m->state = msp_state_size(m, b);
 928         break;
 929     case MSP_HEADER_CMD:
 930         m->state = msp_state_cmd(m, b);
 931         break;
 932     case MSP_FILLBUF:
 933         m->state = msp_state_fill_buf(m, b);
 934         break;
 935     case MSP_CHECKSUM:
 936         m->state = msp_state_checksum(m, b);
 937         break;
 938     case MSP_DISCARD:
 939         m->state = msp_state_discard(m, b);
 940         break;
 941     case MSP_MAYBE_UAVTALK2:
 942         // e.g. 3c 20 1d 00
 943         // second possible uavtalk byte
 944         m->state = (b & 0xf0) == 0x20 ? MSP_MAYBE_UAVTALK3 : MSP_IDLE;
 945         break;
 946     case MSP_MAYBE_UAVTALK3:
 947         // third possible uavtalk byte can be anything
 948         m->state = MSP_MAYBE_UAVTALK4;
 949         break;
 950     case MSP_MAYBE_UAVTALK4:
 951         m->state = MSP_IDLE;
 952         // If this looks like the fourth possible uavtalk byte, we're done
 953         if ((b & 0xf0) == 0) {
 954             PIOS_COM_TELEM_RF = m->com;
 955             return false;
 956         }
 957         break;
 958     case MSP_MAYBE_UAVTALK_SLOW2:
 959         m->state = b == 0x18 ? MSP_MAYBE_UAVTALK_SLOW3 : MSP_IDLE;
 960         break;
 961     case MSP_MAYBE_UAVTALK_SLOW3:
 962         m->state = b == 0x98 ? MSP_MAYBE_UAVTALK_SLOW4 : MSP_IDLE;
 963         break;
 964     case MSP_MAYBE_UAVTALK_SLOW4:
 965         m->state = b == 0x7e ? MSP_MAYBE_UAVTALK_SLOW5 : MSP_IDLE;
 966         break;
 967     case MSP_MAYBE_UAVTALK_SLOW5:
 968         m->state = b == 0x00 ? MSP_MAYBE_UAVTALK_SLOW6 : MSP_IDLE;
 969         break;
 970     case MSP_MAYBE_UAVTALK_SLOW6:
 971         m->state = MSP_IDLE;
 972         // If this looks like the sixth possible 57600 baud uavtalk byte, we're done
 973         if (b == 0x60) {
 974             PIOS_COM_ChangeBaud(m->com, 57600);
 975             PIOS_COM_TELEM_RF = m->com;
 976             return false;
 977         }
 978         break;
 979     }
 980
 981     return true;
 982 }
 983
 984 /**
 985  * Module start routine automatically called after initialization routine
 986  * @return 0 when was successful
 987  */
 988 static int32_t uavoMSPBridgeStart(void)
 989 {
 990     if (!module_enabled) {
 991         // give port to telemetry if it doesn't have one
 992         // stops board getting stuck in condition where it can't be connected to gcs
 993         if (!PIOS_COM_TELEM_RF) {
 994             PIOS_COM_TELEM_RF = pios_com_msp_id;
 995         }
 996
 997         return -1;
 998     }
 999
1000     xTaskHandle taskHandle;
1001
1002     xTaskCreate(uavoMSPBridgeTask, "uavoMSPBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
1003     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOMSPBRIDGE, taskHandle);
1004
1005     return 0;
1006 }
1007
1008 static uint32_t hwsettings_mspspeed_enum_to_baud(uint8_t baud)
1009 {
1010     switch (baud) {
1011     case HWSETTINGS_MSPSPEED_2400:
1012         return 2400;
1013
1014     case HWSETTINGS_MSPSPEED_4800:
1015         return 4800;
1016
1017     case HWSETTINGS_MSPSPEED_9600:
1018         return 9600;
1019
1020     case HWSETTINGS_MSPSPEED_19200:
1021         return 19200;
1022
1023     case HWSETTINGS_MSPSPEED_38400:
1024         return 38400;
1025
1026     case HWSETTINGS_MSPSPEED_57600:
1027         return 57600;
1028
1029     default:
1030     case HWSETTINGS_MSPSPEED_115200:
1031         return 115200;
1032     }
1033 }
1034
1035
1036 /**
1037  * Module initialization routine
1038  * @return 0 when initialization was successful
1039  */
1040 static int32_t uavoMSPBridgeInitialize(void)
1041 {
1042     if (pios_com_msp_id) {
1043         msp = pios_malloc(sizeof(*msp));
1044         if (msp != NULL) {
1045             memset(msp, 0x00, sizeof(*msp));
1046
1047             msp->com = pios_com_msp_id;
1048
1049             // now figure out enabled features: registered sensors, ADC routing, GPS
1050
1051 #ifdef PIOS_EXCLUDE_ADVANCED_FEATURES
1052             msp->sensors |= MSP_SENSOR_ACC;
1053 #else
1054
1055             if (PIOS_SENSORS_GetInstanceByType(0, PIOS_SENSORS_TYPE_3AXIS_ACCEL)) {
1056                 msp->sensors |= MSP_SENSOR_ACC;
1057             }
1058             if (PIOS_SENSORS_GetInstanceByType(0, PIOS_SENSORS_TYPE_1AXIS_BARO)) {
1059                 msp->sensors |= MSP_SENSOR_BARO;
1060             }
1061             if (PIOS_SENSORS_GetInstanceByType(0, PIOS_SENSORS_TYPE_3AXIS_MAG | PIOS_SENSORS_TYPE_3AXIS_AUXMAG)) {
1062                 msp->sensors |= MSP_SENSOR_MAG;
1063             }
1064 #ifdef PIOS_SENSORS_TYPE_1AXIS_SONAR
1065             if (PIOS_SENSORS_GetInstanceByType(0, PIOS_SENSORS_TYPE_1AXIS_SONAR)) {
1066                 msp->sensors |= MSP_SENSOR_SONAR;
1067             }
1068 #endif /* PIOS_SENSORS_TYPE_1AXIS_SONAR */
1069 #endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
1070
1071             // MAP_SENSOR_GPS is hot-pluggable type
1072
1073             HwSettingsADCRoutingDataArray adcRoutingDataArray;
1074             HwSettingsADCRoutingArrayGet(adcRoutingDataArray.array);
1075
1076             for (unsigned i = 0; i < sizeof(adcRoutingDataArray.array) / sizeof(adcRoutingDataArray.array[0]); ++i) {
1077                 switch (adcRoutingDataArray.array[i]) {
1078                 case HWSETTINGS_ADCROUTING_BATTERYVOLTAGE:
1079                     msp->analog |= MSP_ANALOG_VOLTAGE;
1080                     break;
1081                 case HWSETTINGS_ADCROUTING_BATTERYCURRENT:
1082                     msp->analog |= MSP_ANALOG_CURRENT;
1083                     break;
1084                 default:;
1085                 }
1086             }
1087
1088
1089             HwSettingsMSPSpeedOptions mspSpeed;
1090             HwSettingsMSPSpeedGet(&mspSpeed);
1091
1092             PIOS_COM_ChangeBaud(pios_com_msp_id, hwsettings_mspspeed_enum_to_baud(mspSpeed));
1093
1094             module_enabled = true;
1095             return 0;
1096         }
1097     }
1098
1099     return -1;
1100 }
1101 MODULE_INITCALL(uavoMSPBridgeInitialize, uavoMSPBridgeStart);
1102
1103 /**
1104  * Main task routine
1105  * @param[in] parameters parameter given by PIOS_Thread_Create()
1106  */
1107 static void uavoMSPBridgeTask(__attribute__((unused)) void *parameters)
1108 {
1109     while (1) {
1110         uint8_t b = 0;
1111         uint16_t count = PIOS_COM_ReceiveBuffer(msp->com, &b, 1, ~0);
1112         if (count) {
1113             if (!msp_receive_byte(msp, b)) {
1114                 // Returning is considered risky here as
1115                 // that's unusual and this is an edge case.
1116                 while (1) {
1117                     PIOS_DELAY_WaitmS(60 * 1000);
1118                 }
1119             }
1120         }
1121     }
1122 }
1123
1124 #endif // PIOS_INCLUDE_MSP_BRIDGE
1125 /**
1126  * @}
1127  * @}
1128  */