flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup UAVOMavlinkBridge UAVO to Mavlink Bridge Module
   6  * @{
   7  *
   8  * @file       UAVOMavlinkBridge.c
   9  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  10  *             dRonin, http://dRonin.org/, Copyright (C) 2015-2016
  11  *             Tau Labs, http://taulabs.org, Copyright (C) 2013-2014
  12  * @brief      Bridges selected UAVObjects to Mavlink
  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 // ****************
  38 #include "openpilot.h"
  39 #include "stabilizationdesired.h"
  40 #include "flightbatterysettings.h"
  41 #include "flightbatterystate.h"
  42 #include "gpspositionsensor.h"
  43 #include "manualcontrolcommand.h"
  44 #include "attitudestate.h"
  45 #include "airspeedstate.h"
  46 #include "actuatordesired.h"
  47 #include "flightstatus.h"
  48 #include "systemstats.h"
  49 #include "homelocation.h"
  50 #include "positionstate.h"
  51 #include "velocitystate.h"
  52 #include "taskinfo.h"
  53 #include "mavlink.h"
  54 #include "hwsettings.h"
  55 #include "oplinkstatus.h"
  56 #include "receiverstatus.h"
  57 #include "manualcontrolsettings.h"
  58
  59 #include "custom_types.h"
  60
  61 #include <pios_board_io.h>
  62
  63
  64 #define OPLINK_LOW_RSSI  -110
  65 #define OPLINK_HIGH_RSSI -10
  66
  67 // ****************
  68 // Private functions
  69
  70 static void uavoMavlinkBridgeTask(void *parameters);
  71
  72 // ****************
  73 // Private constants
  74
  75 #if defined(PIOS_MAVLINK_STACK_SIZE)
  76 #define STACK_SIZE_BYTES PIOS_MAVLINK_STACK_SIZE
  77 #else
  78 #define STACK_SIZE_BYTES 696
  79 #endif
  80
  81 #define TASK_PRIORITY    (tskIDLE_PRIORITY)
  82 #define TASK_RATE_HZ     10
  83
  84 static void mavlink_send_extended_status();
  85 static void mavlink_send_rc_channels();
  86 static void mavlink_send_position();
  87 static void mavlink_send_extra1();
  88 static void mavlink_send_extra2();
  89
  90 static const struct {
  91     uint8_t rate;
  92     void    (*handler)();
  93 } mav_rates[] = {
  94     [MAV_DATA_STREAM_EXTENDED_STATUS] = {
  95         .rate    = 2, // Hz
  96         .handler = mavlink_send_extended_status,
  97     },
  98     [MAV_DATA_STREAM_RC_CHANNELS] =     {
  99         .rate    = 5, // Hz
 100         .handler = mavlink_send_rc_channels,
 101     },
 102     [MAV_DATA_STREAM_POSITION] =        {
 103         .rate    = 2, // Hz
 104         .handler = mavlink_send_position,
 105     },
 106     [MAV_DATA_STREAM_EXTRA1] =          {
 107         .rate    = 10, // Hz
 108         .handler = mavlink_send_extra1,
 109     },
 110     [MAV_DATA_STREAM_EXTRA2] =          {
 111         .rate    = 2, // Hz
 112         .handler = mavlink_send_extra2,
 113     },
 114 };
 115
 116 #define MAXSTREAMS NELEMENTS(mav_rates)
 117
 118 // ****************
 119 // Private variables
 120
 121 static bool module_enabled = false;
 122
 123 static uint8_t *stream_ticks;
 124
 125 static mavlink_message_t *mav_msg;
 126
 127 static void updateSettings();
 128
 129 /**
 130  * Initialise the module
 131  * \return -1 if initialisation failed
 132  * \return 0 on success
 133  */
 134 static int32_t uavoMavlinkBridgeStart(void)
 135 {
 136     if (module_enabled) {
 137         // Start tasks
 138         xTaskHandle taskHandle;
 139         xTaskCreate(uavoMavlinkBridgeTask, "uavoMavlinkBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
 140         PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOMAVLINKBRIDGE, taskHandle);
 141         return 0;
 142     }
 143     return -1;
 144 }
 145 /**
 146  * Initialise the module
 147  * \return -1 if initialisation failed
 148  * \return 0 on success
 149  */
 150 static int32_t uavoMavlinkBridgeInitialize(void)
 151 {
 152     if (PIOS_COM_MAVLINK) {
 153         updateSettings();
 154
 155         mav_msg = pios_malloc(sizeof(*mav_msg));
 156         stream_ticks = pios_malloc(MAXSTREAMS);
 157
 158         if (mav_msg && stream_ticks) {
 159             for (unsigned x = 0; x < MAXSTREAMS; ++x) {
 160                 stream_ticks[x] = (TASK_RATE_HZ / mav_rates[x].rate);
 161             }
 162
 163             module_enabled = true;
 164         }
 165     }
 166
 167     return 0;
 168 }
 169 MODULE_INITCALL(uavoMavlinkBridgeInitialize, uavoMavlinkBridgeStart);
 170
 171 static void send_message()
 172 {
 173     uint16_t msg_length = MAVLINK_NUM_NON_PAYLOAD_BYTES +
 174                           mav_msg->len;
 175
 176     PIOS_COM_SendBuffer(PIOS_COM_MAVLINK, &mav_msg->magic, msg_length);
 177 }
 178
 179 static void mavlink_send_extended_status()
 180 {
 181 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 182     FlightBatteryStateData batState;
 183     SystemStatsData systemStats;
 184
 185     if (FlightBatteryStateHandle() != NULL) {
 186         FlightBatteryStateGet(&batState);
 187     }
 188
 189     SystemStatsGet(&systemStats);
 190
 191     uint32_t battery_capacity = 0;
 192     if (FlightBatterySettingsHandle() != NULL) {
 193         FlightBatterySettingsCapacityGet(&battery_capacity);
 194     }
 195
 196     int8_t battery_remaining = 0;
 197     if (battery_capacity != 0) {
 198         if (batState.ConsumedEnergy < battery_capacity) {
 199             battery_remaining = 100 - lroundf(batState.ConsumedEnergy / battery_capacity * 100);
 200         }
 201     }
 202
 203     mavlink_msg_sys_status_pack(0, 200, mav_msg,
 204                                 // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 205                                 0,
 206                                 // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 207                                 0,
 208                                 // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 209                                 0,
 210                                 // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
 211                                 (uint16_t)systemStats.CPULoad * 10,
 212                                 // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
 213                                 lroundf(batState.Voltage * 1000), // No need to check for validity, Voltage reads 0.0 when measurement is not configured,
 214                                 // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
 215                                 lroundf(batState.Current * 100), // Same as for Voltage. 0 means no measurement
 216                                 // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
 217                                 battery_remaining,
 218                                 // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
 219                                 0,
 220                                 // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
 221                                 0,
 222                                 // errors_count1 Autopilot-specific errors
 223                                 0,
 224                                 // errors_count2 Autopilot-specific errors
 225                                 0,
 226                                 // errors_count3 Autopilot-specific errors
 227                                 0,
 228                                 // errors_count4 Autopilot-specific errors
 229                                 0);
 230
 231     send_message();
 232 #endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
 233 }
 234
 235 static void mavlink_send_rc_channels()
 236 {
 237     ManualControlCommandData manualState;
 238     FlightStatusData flightStatus;
 239     SystemStatsData systemStats;
 240
 241     ManualControlCommandGet(&manualState);
 242     FlightStatusGet(&flightStatus);
 243     SystemStatsGet(&systemStats);
 244
 245     uint8_t mavlinkRssi;
 246
 247     ManualControlSettingsChannelGroupsData channelGroups;
 248     ManualControlSettingsChannelGroupsGet(&channelGroups);
 249
 250 #ifdef PIOS_INCLUDE_OPLINKRCVR
 251     if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) ||
 252         (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) {
 253         int8_t rssi;
 254         OPLinkStatusRSSIGet(&rssi);
 255
 256         if (rssi < OPLINK_LOW_RSSI) {
 257             rssi = OPLINK_LOW_RSSI;
 258         } else if (rssi > OPLINK_HIGH_RSSI) {
 259             rssi = OPLINK_HIGH_RSSI;
 260         }
 261
 262         mavlinkRssi = ((rssi - OPLINK_LOW_RSSI) * 255) / (OPLINK_HIGH_RSSI - OPLINK_LOW_RSSI);
 263     } else {
 264 #endif /* PIOS_INCLUDE_OPLINKRCVR */
 265     uint8_t quality;
 266     ReceiverStatusQualityGet(&quality);
 267
 268     // MAVLink RSSI's range is 0-255
 269     mavlinkRssi = (quality * 255) / 100;
 270 #ifdef PIOS_INCLUDE_OPLINKRCVR
 271 }
 272 #endif
 273
 274     mavlink_msg_rc_channels_raw_pack(0, 200, mav_msg,
 275                                      // time_boot_ms Timestamp (milliseconds since system boot)
 276                                      systemStats.FlightTime,
 277                                      // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
 278                                      0,
 279                                      // chan1_raw RC channel 1 value, in microseconds
 280                                      manualState.Channel[0],
 281                                      // chan2_raw RC channel 2 value, in microseconds
 282                                      manualState.Channel[1],
 283                                      // chan3_raw RC channel 3 value, in microseconds
 284                                      manualState.Channel[2],
 285                                      // chan4_raw RC channel 4 value, in microseconds
 286                                      manualState.Channel[3],
 287                                      // chan5_raw RC channel 5 value, in microseconds
 288                                      manualState.Channel[4],
 289                                      // chan6_raw RC channel 6 value, in microseconds
 290                                      manualState.Channel[5],
 291                                      // chan7_raw RC channel 7 value, in microseconds
 292                                      manualState.Channel[6],
 293                                      // chan8_raw RC channel 8 value, in microseconds
 294                                      manualState.Channel[7],
 295                                      // rssi Receive signal strength indicator, 0: 0%, 255: 100%
 296                                      mavlinkRssi);
 297
 298     send_message();
 299 }
 300
 301 static void mavlink_send_position()
 302 {
 303     SystemStatsData systemStats;
 304
 305     SystemStatsGet(&systemStats);
 306
 307     if (GPSPositionSensorHandle() != NULL) {
 308         GPSPositionSensorData gpsPosData;
 309         GPSPositionSensorGet(&gpsPosData);
 310
 311         uint8_t gps_fix_type = 0;
 312
 313         switch (gpsPosData.Status) {
 314         case GPSPOSITIONSENSOR_STATUS_NOGPS:
 315             gps_fix_type = 0;
 316             break;
 317         case GPSPOSITIONSENSOR_STATUS_NOFIX:
 318             gps_fix_type = 1;
 319             break;
 320         case GPSPOSITIONSENSOR_STATUS_FIX2D:
 321             gps_fix_type = 2;
 322             break;
 323         case GPSPOSITIONSENSOR_STATUS_FIX3D:
 324         case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS:
 325             gps_fix_type = 3;
 326             break;
 327         }
 328
 329         mavlink_msg_gps_raw_int_pack(0, 200, mav_msg,
 330                                      // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
 331                                      (uint64_t)systemStats.FlightTime * 1000,
 332                                      // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
 333                                      gps_fix_type,
 334                                      // lat Latitude in 1E7 degrees
 335                                      gpsPosData.Latitude,
 336                                      // lon Longitude in 1E7 degrees
 337                                      gpsPosData.Longitude,
 338                                      // alt Altitude in 1E3 meters (millimeters) above MSL
 339                                      gpsPosData.Altitude * 1000,
 340                                      // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 341                                      gpsPosData.HDOP * 100,
 342                                      // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 343                                      gpsPosData.VDOP * 100,
 344                                      // vel GPS ground speed (m/s * 100). If unknown, set to: 65535
 345                                      gpsPosData.Groundspeed * 100,
 346                                      // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
 347                                      gpsPosData.Heading * 100,
 348                                      // satellites_visible Number of satellites visible. If unknown, set to 255
 349                                      gpsPosData.Satellites);
 350
 351         send_message();
 352     }
 353
 354 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 355     if (HomeLocationHandle() != NULL) {
 356         HomeLocationData homeLocation;
 357         HomeLocationGet(&homeLocation);
 358
 359         mavlink_msg_gps_global_origin_pack(0, 200, mav_msg,
 360                                            // latitude Latitude (WGS84), expressed as * 1E7
 361                                            homeLocation.Latitude,
 362                                            // longitude Longitude (WGS84), expressed as * 1E7
 363                                            homeLocation.Longitude,
 364                                            // altitude Altitude(WGS84), expressed as * 1000
 365                                            homeLocation.Altitude * 1000);
 366
 367         send_message();
 368     }
 369 #endif /* PIOS_EXCLUDE_ADVANCED_FEATURES */
 370
 371     // TODO add waypoint nav stuff
 372     // wp_target_bearing
 373     // wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg);
 374     // alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
 375     // aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
 376     // xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg);
 377     // mavlink_msg_nav_controller_output_pack
 378     // wp_number
 379     // mavlink_msg_mission_current_pack
 380 }
 381
 382 static void mavlink_send_extra1()
 383 {
 384     AttitudeStateData attState;
 385     SystemStatsData systemStats;
 386
 387     AttitudeStateGet(&attState);
 388     SystemStatsGet(&systemStats);
 389
 390     mavlink_msg_attitude_pack(0, 200, mav_msg,
 391                               // time_boot_ms Timestamp (milliseconds since system boot)
 392                               systemStats.FlightTime,
 393                               // roll Roll angle (rad)
 394                               DEG2RAD(attState.Roll),
 395                               // pitch Pitch angle (rad)
 396                               DEG2RAD(attState.Pitch),
 397                               // yaw Yaw angle (rad)
 398                               DEG2RAD(attState.Yaw),
 399                               // rollspeed Roll angular speed (rad/s)
 400                               0,
 401                               // pitchspeed Pitch angular speed (rad/s)
 402                               0,
 403                               // yawspeed Yaw angular speed (rad/s)
 404                               0);
 405
 406     send_message();
 407 }
 408
 409 static inline float Sq(float x)
 410 {
 411     return x * x;
 412 }
 413
 414 #define IS_STAB_MODE(d, m) (((d).Roll == (m)) && ((d).Pitch == (m)))
 415
 416 static void mavlink_send_extra2()
 417 {
 418     float airspeed    = 0;
 419     float altitude    = 0;
 420     float groundspeed = 0;
 421     float climbrate   = 0;
 422
 423
 424 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 425     if (AirspeedStateHandle() != NULL) {
 426         AirspeedStateTrueAirspeedGet(&airspeed);
 427     }
 428
 429     if (PositionStateHandle() != NULL) {
 430         PositionStateDownGet(&altitude);
 431         altitude *= -1;
 432     }
 433
 434     if (VelocityStateHandle() != NULL) {
 435         VelocityStateData velocityState;
 436         VelocityStateGet(&velocityState);
 437
 438         groundspeed = sqrtf(Sq(velocityState.North) + Sq(velocityState.East));
 439         climbrate   = velocityState.Down * -1;
 440     }
 441 #endif
 442
 443     float attitudeYaw = 0;
 444
 445     AttitudeStateYawGet(&attitudeYaw);
 446     // round attState.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360)
 447     int16_t heading = lroundf(attitudeYaw);
 448     if (heading < 0) {
 449         heading += 360;
 450     }
 451
 452
 453     float thrust;
 454     ActuatorDesiredThrustGet(&thrust);
 455
 456     mavlink_msg_vfr_hud_pack(0, 200, mav_msg,
 457                              // airspeed Current airspeed in m/s
 458                              airspeed,
 459                              // groundspeed Current ground speed in m/s
 460                              groundspeed,
 461                              // heading Current heading in degrees, in compass units (0..360, 0=north)
 462                              heading,
 463                              // throttle Current throttle setting in integer percent, 0 to 100
 464                              thrust * 100,
 465                              // alt Current altitude (MSL), in meters
 466                              altitude,
 467                              // climb Current climb rate in meters/second
 468                              climbrate);
 469
 470     send_message();
 471
 472     FlightStatusData flightStatus;
 473     FlightStatusGet(&flightStatus);
 474
 475     StabilizationDesiredStabilizationModeData stabModeData;
 476     StabilizationDesiredStabilizationModeGet(&stabModeData);
 477
 478     uint8_t armed_mode = 0;
 479     if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
 480         armed_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 481     }
 482
 483     uint8_t custom_mode = CUSTOM_MODE_STAB;
 484
 485     switch (flightStatus.FlightMode) {
 486     case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
 487         custom_mode = CUSTOM_MODE_PHLD;
 488         break;
 489
 490     case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
 491         custom_mode = CUSTOM_MODE_RTL;
 492         break;
 493
 494     case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
 495     case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
 496     case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
 497         custom_mode = CUSTOM_MODE_AUTO;
 498         break;
 499
 500     case FLIGHTSTATUS_FLIGHTMODE_LAND:
 501         custom_mode = CUSTOM_MODE_LAND;
 502         break;
 503
 504     case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
 505         custom_mode = CUSTOM_MODE_ACRO; // or
 506         break;
 507     case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
 508         custom_mode = CUSTOM_MODE_ATUN;
 509         break;
 510     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
 511     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
 512     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
 513     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
 514     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
 515     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
 516
 517         if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATE)
 518             || IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER)) {
 519             custom_mode = CUSTOM_MODE_ACRO;
 520         } else if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO)) { // this is Acro+ mode
 521             custom_mode = CUSTOM_MODE_ACRO;
 522         } else if (IS_STAB_MODE(stabModeData, STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE)) {
 523             custom_mode = CUSTOM_MODE_SPORT;
 524         } else if ((stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD)
 525                    || (stabModeData.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO)) {
 526             custom_mode = CUSTOM_MODE_ALTH;
 527         } else { // looks like stabilized mode, whichever it is..
 528             custom_mode = CUSTOM_MODE_STAB;
 529         }
 530
 531         break;
 532     case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
 533     case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
 534     case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
 535     case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
 536     case FLIGHTSTATUS_FLIGHTMODE_POI:
 537         custom_mode = CUSTOM_MODE_STAB; // Don't know any better
 538         break;
 539     }
 540
 541     mavlink_msg_heartbeat_pack(0, 200, mav_msg,
 542                                // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 543                                MAV_TYPE_GENERIC,
 544                                // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
 545                                MAV_AUTOPILOT_GENERIC, // or MAV_AUTOPILOT_OPENPILOT
 546                                // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
 547                                armed_mode,
 548                                // custom_mode A bitfield for use for autopilot-specific flags.
 549                                custom_mode,
 550                                // system_status System status flag, see MAV_STATE ENUM
 551                                0);
 552
 553     send_message();
 554 }
 555
 556 /**
 557  * Main task. It does not return.
 558  */
 559
 560 static void uavoMavlinkBridgeTask(__attribute__((unused)) void *parameters)
 561 {
 562     uint32_t lastSysTime;
 563
 564     // Main task loop
 565     lastSysTime = xTaskGetTickCount(); // portTICK_RATE_MS;
 566
 567     while (1) {
 568         vTaskDelayUntil(&lastSysTime, (1000 / TASK_RATE_HZ) / portTICK_RATE_MS);
 569
 570         for (unsigned i = 0; i < MAXSTREAMS; ++i) {
 571             if (!mav_rates[i].rate || !mav_rates[i].handler) {
 572                 continue;
 573             }
 574
 575             if (stream_ticks[i] == 0) {
 576                 // trigger now
 577                 uint8_t rate = mav_rates[i].rate;
 578                 if (rate > TASK_RATE_HZ) {
 579                     rate = TASK_RATE_HZ;
 580                 }
 581                 stream_ticks[i] = TASK_RATE_HZ / rate;
 582
 583                 mav_rates[i].handler();
 584             }
 585
 586             --stream_ticks[i];
 587         }
 588     }
 589 }
 590
 591 static uint32_t hwsettings_mavlinkspeed_enum_to_baud(uint8_t baud)
 592 {
 593     switch (baud) {
 594     case HWSETTINGS_MAVLINKSPEED_2400:
 595         return 2400;
 596
 597     case HWSETTINGS_MAVLINKSPEED_4800:
 598         return 4800;
 599
 600     case HWSETTINGS_MAVLINKSPEED_9600:
 601         return 9600;
 602
 603     case HWSETTINGS_MAVLINKSPEED_19200:
 604         return 19200;
 605
 606     case HWSETTINGS_MAVLINKSPEED_38400:
 607         return 38400;
 608
 609     case HWSETTINGS_MAVLINKSPEED_57600:
 610         return 57600;
 611
 612     default:
 613     case HWSETTINGS_MAVLINKSPEED_115200:
 614         return 115200;
 615     }
 616 }
 617
 618
 619 static void updateSettings()
 620 {
 621     if (PIOS_COM_MAVLINK) {
 622         // Retrieve settings
 623         HwSettingsMAVLinkSpeedOptions mavlinkSpeed;
 624         HwSettingsMAVLinkSpeedGet(&mavlinkSpeed);
 625
 626         PIOS_COM_ChangeBaud(PIOS_COM_MAVLINK, hwsettings_mavlinkspeed_enum_to_baud(mavlinkSpeed));
 627     }
 628 }
 629 /**
 630  * @}
 631  * @}
 632  */