src/main/fc/fc_msp_box.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #include "common/streambuf.h"
  25 #include "common/utils.h"
  26
  27 #include "config/feature.h"
  28
  29 #include "fc/config.h"
  30 #include "fc/fc_msp_box.h"
  31 #include "fc/runtime_config.h"
  32 #include "flight/mixer.h"
  33
  34 #include "io/osd.h"
  35
  36 #include "drivers/pwm_output.h"
  37
  38 #include "sensors/diagnostics.h"
  39 #include "sensors/sensors.h"
  40
  41 #include "navigation/navigation.h"
  42
  43 #include "telemetry/telemetry.h"
  44
  45 #define BOX_SUFFIX ';'
  46 #define BOX_SUFFIX_LEN 1
  47
  48 static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
  49     { .boxId = BOXARM,              .boxName = "ARM",               .permanentId = 0 },
  50     { .boxId = BOXANGLE,            .boxName = "ANGLE",             .permanentId = 1 },
  51     { .boxId = BOXHORIZON,          .boxName = "HORIZON",           .permanentId = 2 },
  52     { .boxId = BOXNAVALTHOLD,       .boxName = "NAV ALTHOLD",       .permanentId = 3 },
  53     { .boxId = BOXHEADINGHOLD,      .boxName = "HEADING HOLD",      .permanentId = 5 },
  54     { .boxId = BOXHEADFREE,         .boxName = "HEADFREE",          .permanentId = 6 },
  55     { .boxId = BOXHEADADJ,          .boxName = "HEADADJ",           .permanentId = 7 },
  56     { .boxId = BOXCAMSTAB,          .boxName = "CAMSTAB",           .permanentId = 8 },
  57     { .boxId = BOXNAVRTH,           .boxName = "NAV RTH",           .permanentId = 10 },
  58     { .boxId = BOXNAVPOSHOLD,       .boxName = "NAV POSHOLD",       .permanentId = 11 },
  59     { .boxId = BOXMANUAL,           .boxName = "MANUAL",            .permanentId = 12 },
  60     { .boxId = BOXBEEPERON,         .boxName = "BEEPER",            .permanentId = 13 },
  61     { .boxId = BOXLEDLOW,           .boxName = "LEDS OFF",          .permanentId = 15 },
  62     { .boxId = BOXLIGHTS,           .boxName = "LIGHTS",            .permanentId = 16 },
  63     { .boxId = BOXOSD,              .boxName = "OSD OFF",           .permanentId = 19 },
  64     { .boxId = BOXTELEMETRY,        .boxName = "TELEMETRY",         .permanentId = 20 },
  65     { .boxId = BOXAUTOTUNE,         .boxName = "AUTO TUNE",         .permanentId = 21 },
  66     { .boxId = BOXBLACKBOX,         .boxName = "BLACKBOX",          .permanentId = 26 },
  67     { .boxId = BOXFAILSAFE,         .boxName = "FAILSAFE",          .permanentId = 27 },
  68     { .boxId = BOXNAVWP,            .boxName = "NAV WP",            .permanentId = 28 },
  69     { .boxId = BOXAIRMODE,          .boxName = "AIR MODE",          .permanentId = 29 },
  70     { .boxId = BOXHOMERESET,        .boxName = "HOME RESET",        .permanentId = 30 },
  71     { .boxId = BOXGCSNAV,           .boxName = "GCS NAV",           .permanentId = 31 },
  72     { .boxId = BOXFPVANGLEMIX,      .boxName = "FPV ANGLE MIX",     .permanentId = 32 },
  73     { .boxId = BOXSURFACE,          .boxName = "SURFACE",           .permanentId = 33 },
  74     { .boxId = BOXFLAPERON,         .boxName = "FLAPERON",          .permanentId = 34 },
  75     { .boxId = BOXTURNASSIST,       .boxName = "TURN ASSIST",       .permanentId = 35 },
  76     { .boxId = BOXNAVLAUNCH,        .boxName = "NAV LAUNCH",        .permanentId = 36 },
  77     { .boxId = BOXAUTOTRIM,         .boxName = "SERVO AUTOTRIM",    .permanentId = 37 },
  78     { .boxId = BOXKILLSWITCH,       .boxName = "KILLSWITCH",        .permanentId = 38 },
  79     { .boxId = BOXCAMERA1,          .boxName = "CAMERA CONTROL 1",  .permanentId = 39 },
  80     { .boxId = BOXCAMERA2,          .boxName = "CAMERA CONTROL 2",  .permanentId = 40 },
  81     { .boxId = BOXCAMERA3,          .boxName = "CAMERA CONTROL 3",  .permanentId = 41 },
  82     { .boxId = BOXOSDALT1,          .boxName = "OSD ALT 1",         .permanentId = 42 },
  83     { .boxId = BOXOSDALT2,          .boxName = "OSD ALT 2",         .permanentId = 43 },
  84     { .boxId = BOXOSDALT3,          .boxName = "OSD ALT 3",         .permanentId = 44 },
  85     { .boxId = BOXNAVCOURSEHOLD,    .boxName = "NAV COURSE HOLD",   .permanentId = 45 },
  86     { .boxId = BOXBRAKING,          .boxName = "MC BRAKING",        .permanentId = 46 },
  87     { .boxId = BOXUSER1,            .boxName = "USER1",             .permanentId = BOX_PERMANENT_ID_USER1 }, // 47
  88     { .boxId = BOXUSER2,            .boxName = "USER2",             .permanentId = BOX_PERMANENT_ID_USER2 }, // 48
  89     { .boxId = BOXUSER3,            .boxName = "USER3",             .permanentId = BOX_PERMANENT_ID_USER3 }, // 57
  90     { .boxId = BOXUSER4,            .boxName = "USER4",             .permanentId = BOX_PERMANENT_ID_USER4 }, // 58
  91     { .boxId = BOXLOITERDIRCHN,     .boxName = "LOITER CHANGE",     .permanentId = 49 },
  92     { .boxId = BOXMSPRCOVERRIDE,    .boxName = "MSP RC OVERRIDE",   .permanentId = 50 },
  93     { .boxId = BOXPREARM,           .boxName = "PREARM",            .permanentId = 51 },
  94     { .boxId = BOXTURTLE,           .boxName = "TURTLE",            .permanentId = 52 },
  95     { .boxId = BOXNAVCRUISE,        .boxName = "NAV CRUISE",        .permanentId = 53 },
  96     { .boxId = BOXAUTOLEVEL,        .boxName = "AUTO LEVEL",        .permanentId = 54 },
  97     { .boxId = BOXPLANWPMISSION,    .boxName = "WP PLANNER",        .permanentId = 55 },
  98     { .boxId = BOXSOARING,          .boxName = "SOARING",           .permanentId = 56 },
  99     { .boxId = BOXCHANGEMISSION,    .boxName = "MISSION CHANGE",    .permanentId = 59 },
 100     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 101 };
 102
 103 // this is calculated at startup based on enabled features.
 104 static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
 105 // this is the number of filled indexes in above array
 106 uint8_t activeBoxIdCount = 0;
 107
 108 #define RESET_BOX_ID_COUNT activeBoxIdCount = 0
 109 #define ADD_ACTIVE_BOX(box) activeBoxIds[activeBoxIdCount++] = box
 110
 111 const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
 112 {
 113     for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
 114         const box_t *candidate = &boxes[boxIndex];
 115         if (candidate->boxId == activeBoxId) {
 116             return candidate;
 117         }
 118     }
 119     return NULL;
 120 }
 121
 122 const box_t *findBoxByPermanentId(uint8_t permenantId)
 123 {
 124     for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
 125         const box_t *candidate = &boxes[boxIndex];
 126         if (candidate->permanentId == permenantId) {
 127             return candidate;
 128         }
 129     }
 130     return NULL;
 131 }
 132
 133 bool serializeBoxNamesReply(sbuf_t *dst)
 134 {
 135     // First run of the loop - calculate total length of the reply
 136     int replyLengthTotal = 0;
 137     for (int i = 0; i < activeBoxIdCount; i++) {
 138         const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
 139         if (box) {
 140             replyLengthTotal += strlen(box->boxName) + BOX_SUFFIX_LEN;
 141         }
 142     }
 143
 144     // Check if we have enough space to send a reply
 145     if (sbufBytesRemaining(dst) < replyLengthTotal) {
 146         return false;
 147     }
 148
 149     for (int i = 0; i < activeBoxIdCount; i++) {
 150         const int activeBoxId = activeBoxIds[i];
 151         const box_t *box = findBoxByActiveBoxId(activeBoxId);
 152         if (box) {
 153             const int len = strlen(box->boxName);
 154             sbufWriteData(dst, box->boxName, len);
 155             sbufWriteU8(dst, BOX_SUFFIX);
 156         }
 157     }
 158
 159     return true;
 160 }
 161
 162 void serializeBoxReply(sbuf_t *dst)
 163 {
 164     for (int i = 0; i < activeBoxIdCount; i++) {
 165         const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
 166         if (!box) {
 167             continue;
 168         }
 169         sbufWriteU8(dst, box->permanentId);
 170     }
 171 }
 172
 173 void initActiveBoxIds(void)
 174 {
 175     // calculate used boxes based on features and fill availableBoxes[] array
 176     memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
 177
 178     RESET_BOX_ID_COUNT;
 179     ADD_ACTIVE_BOX(BOXARM);
 180     ADD_ACTIVE_BOX(BOXPREARM);
 181
 182     if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
 183         ADD_ACTIVE_BOX(BOXANGLE);
 184         ADD_ACTIVE_BOX(BOXHORIZON);
 185         ADD_ACTIVE_BOX(BOXTURNASSIST);
 186     }
 187
 188     if (!feature(FEATURE_AIRMODE) && STATE(ALTITUDE_CONTROL)) {
 189         ADD_ACTIVE_BOX(BOXAIRMODE);
 190     }
 191
 192     ADD_ACTIVE_BOX(BOXHEADINGHOLD);
 193
 194     //Camstab mode is enabled always
 195     ADD_ACTIVE_BOX(BOXCAMSTAB);
 196
 197     if (STATE(MULTIROTOR)) {
 198         if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG))) {
 199             ADD_ACTIVE_BOX(BOXHEADFREE);
 200             ADD_ACTIVE_BOX(BOXHEADADJ);
 201         }
 202         if (sensors(SENSOR_BARO) && sensors(SENSOR_RANGEFINDER)) {
 203             ADD_ACTIVE_BOX(BOXSURFACE);
 204         }
 205         ADD_ACTIVE_BOX(BOXFPVANGLEMIX);
 206     }
 207
 208     bool navReadyAltControl = sensors(SENSOR_BARO);
 209 #ifdef USE_GPS
 210     navReadyAltControl = navReadyAltControl || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro));
 211
 212     const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
 213     bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS);
 214     if (STATE(MULTIROTOR)) {
 215         navReadyPosControl = navReadyPosControl && getHwCompassStatus() != HW_SENSOR_NONE;
 216     }
 217
 218     if (STATE(ALTITUDE_CONTROL) && navReadyAltControl && (navReadyPosControl || navFlowDeadReckoning)) {
 219         ADD_ACTIVE_BOX(BOXNAVPOSHOLD);
 220         if (STATE(AIRPLANE)) {
 221             ADD_ACTIVE_BOX(BOXLOITERDIRCHN);
 222         }
 223     }
 224
 225     if (navReadyPosControl) {
 226         if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) {
 227             ADD_ACTIVE_BOX(BOXNAVRTH);
 228             ADD_ACTIVE_BOX(BOXNAVWP);
 229             ADD_ACTIVE_BOX(BOXHOMERESET);
 230             ADD_ACTIVE_BOX(BOXGCSNAV);
 231             ADD_ACTIVE_BOX(BOXPLANWPMISSION);
 232 #ifdef USE_MULTI_MISSION
 233             ADD_ACTIVE_BOX(BOXCHANGEMISSION);
 234 #endif
 235         }
 236
 237         if (STATE(AIRPLANE)) {
 238             ADD_ACTIVE_BOX(BOXNAVCRUISE);
 239             ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD);
 240             ADD_ACTIVE_BOX(BOXSOARING);
 241         }
 242     }
 243
 244 #ifdef USE_MR_BRAKING_MODE
 245     if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
 246         ADD_ACTIVE_BOX(BOXBRAKING);
 247     }
 248 #endif
 249 #endif  // GPS
 250     if (STATE(ALTITUDE_CONTROL) && navReadyAltControl) {
 251         ADD_ACTIVE_BOX(BOXNAVALTHOLD);
 252     }
 253
 254     if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
 255         ADD_ACTIVE_BOX(BOXMANUAL);
 256     }
 257
 258     if (STATE(AIRPLANE)) {
 259         if (!feature(FEATURE_FW_LAUNCH)) {
 260            ADD_ACTIVE_BOX(BOXNAVLAUNCH);
 261         }
 262
 263         if (!feature(FEATURE_FW_AUTOTRIM)) {
 264             ADD_ACTIVE_BOX(BOXAUTOTRIM);
 265         }
 266
 267 #if defined(USE_AUTOTUNE_FIXED_WING)
 268         ADD_ACTIVE_BOX(BOXAUTOTUNE);
 269 #endif
 270         if (sensors(SENSOR_BARO)) {
 271             ADD_ACTIVE_BOX(BOXAUTOLEVEL);
 272         }
 273     }
 274
 275     /*
 276      * FLAPERON mode active only in case of airplane and custom airplane. Activating on
 277      * flying wing can cause bad thing
 278      */
 279     if (STATE(FLAPERON_AVAILABLE)) {
 280         ADD_ACTIVE_BOX(BOXFLAPERON);
 281     }
 282
 283     ADD_ACTIVE_BOX(BOXBEEPERON);
 284
 285 #ifdef USE_LIGHTS
 286     ADD_ACTIVE_BOX(BOXLIGHTS);
 287 #endif
 288
 289 #ifdef USE_LED_STRIP
 290     if (feature(FEATURE_LED_STRIP)) {
 291         ADD_ACTIVE_BOX(BOXLEDLOW);
 292     }
 293 #endif
 294
 295     ADD_ACTIVE_BOX(BOXOSD);
 296
 297 #ifdef USE_TELEMETRY
 298     if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch) {
 299         ADD_ACTIVE_BOX(BOXTELEMETRY);
 300     }
 301 #endif
 302
 303 #ifdef USE_BLACKBOX
 304     if (feature(FEATURE_BLACKBOX)) {
 305         ADD_ACTIVE_BOX(BOXBLACKBOX);
 306     }
 307 #endif
 308
 309     ADD_ACTIVE_BOX(BOXKILLSWITCH);
 310     ADD_ACTIVE_BOX(BOXFAILSAFE);
 311
 312 #ifdef USE_RCDEVICE
 313     ADD_ACTIVE_BOX(BOXCAMERA1);
 314     ADD_ACTIVE_BOX(BOXCAMERA2);
 315     ADD_ACTIVE_BOX(BOXCAMERA3);
 316 #endif
 317
 318 #ifdef USE_PINIOBOX
 319     // USER modes are only used for PINIO at the moment
 320     ADD_ACTIVE_BOX(BOXUSER1);
 321     ADD_ACTIVE_BOX(BOXUSER2);
 322     ADD_ACTIVE_BOX(BOXUSER3);
 323     ADD_ACTIVE_BOX(BOXUSER4);
 324 #endif
 325
 326 #if defined(USE_OSD) && defined(OSD_LAYOUT_COUNT)
 327 #if OSD_LAYOUT_COUNT > 0
 328     ADD_ACTIVE_BOX(BOXOSDALT1);
 329 #if OSD_LAYOUT_COUNT > 1
 330     ADD_ACTIVE_BOX(BOXOSDALT2);
 331 #if OSD_LAYOUT_COUNT > 2
 332     ADD_ACTIVE_BOX(BOXOSDALT3);
 333 #endif
 334 #endif
 335 #endif
 336 #endif
 337
 338 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 339     ADD_ACTIVE_BOX(BOXMSPRCOVERRIDE);
 340 #endif
 341
 342 #ifdef USE_DSHOT
 343     if(STATE(MULTIROTOR) && isMotorProtocolDshot()) {
 344         ADD_ACTIVE_BOX(BOXTURTLE);
 345     }
 346 #endif
 347 }
 348
 349 #define IS_ENABLED(mask) ((mask) == 0 ? 0 : 1)
 350 #define CHECK_ACTIVE_BOX(condition, index)    do { if (IS_ENABLED(condition)) { activeBoxes[index] = 1; } } while(0)
 351
 352 void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 353 {
 354     uint8_t activeBoxes[CHECKBOX_ITEM_COUNT];
 355     ZERO_FARRAY(activeBoxes);
 356
 357     // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
 358     // Requires new Multiwii protocol version to fix
 359     // It would be preferable to setting the enabled bits based on BOXINDEX.
 360     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)),               BOXANGLE);
 361     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)),             BOXHORIZON);
 362     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADING_MODE)),             BOXHEADINGHOLD);
 363     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)),            BOXHEADFREE);
 364     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)),         BOXHEADADJ);
 365     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)),         BOXCAMSTAB);
 366     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)),     BOXFPVANGLEMIX);
 367     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(MANUAL_MODE)),              BOXMANUAL);
 368     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)),        BOXBEEPERON);
 369     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)),          BOXLEDLOW);
 370     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLIGHTS)),          BOXLIGHTS);
 371     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)),             BOXOSD);
 372     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)),       BOXTELEMETRY);
 373     CHECK_ACTIVE_BOX(IS_ENABLED(ARMING_FLAG(ARMED)),                    BOXARM);
 374     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)),        BOXBLACKBOX);
 375     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)),            BOXFAILSAFE);
 376     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)),         BOXNAVALTHOLD);
 377     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)),         BOXNAVPOSHOLD);
 378     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)),     BOXNAVCOURSEHOLD);
 379     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) && IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVCRUISE);
 380     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)),             BOXNAVRTH);
 381     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)),              BOXNAVWP);
 382     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)),         BOXAIRMODE);
 383     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)),          BOXGCSNAV);
 384     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)),                 BOXFLAPERON);
 385     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)),           BOXTURNASSIST);
 386     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)),          BOXNAVLAUNCH);
 387     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(AUTO_TUNE)),                BOXAUTOTUNE);
 388     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTRIM)),        BOXAUTOTRIM);
 389     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKILLSWITCH)),      BOXKILLSWITCH);
 390     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)),       BOXHOMERESET);
 391     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA1)),         BOXCAMERA1);
 392     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA2)),         BOXCAMERA2);
 393     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA3)),         BOXCAMERA3);
 394     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT1)),         BOXOSDALT1);
 395     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)),         BOXOSDALT2);
 396     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)),         BOXOSDALT3);
 397     CHECK_ACTIVE_BOX(IS_ENABLED(navigationTerrainFollowingEnabled()),   BOXSURFACE);
 398     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)),         BOXBRAKING);
 399     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)),           BOXUSER1);
 400     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)),           BOXUSER2);
 401     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER3)),           BOXUSER3);
 402     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER4)),           BOXUSER4);
 403     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)),    BOXLOITERDIRCHN);
 404 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 405     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)),   BOXMSPRCOVERRIDE);
 406 #endif
 407     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)),       BOXAUTOLEVEL);
 408     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)),   BOXPLANWPMISSION);
 409     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSOARING)),         BOXSOARING);
 410 #ifdef USE_MULTI_MISSION
 411     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)),   BOXCHANGEMISSION);
 412 #endif
 413
 414     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
 415     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
 416         if (activeBoxes[activeBoxIds[i]]) {
 417             bitArraySet(mspBoxModeFlags->bits, i);
 418         }
 419     }
 420 }
 421
 422 uint16_t packSensorStatus(void)
 423 {
 424     // Sensor bits
 425     uint16_t sensorStatus =
 426             IS_ENABLED(sensors(SENSOR_ACC))         << 0 |
 427             IS_ENABLED(sensors(SENSOR_BARO))        << 1 |
 428             IS_ENABLED(sensors(SENSOR_MAG))         << 2 |
 429             IS_ENABLED(sensors(SENSOR_GPS))         << 3 |
 430             IS_ENABLED(sensors(SENSOR_RANGEFINDER)) << 4 |
 431             IS_ENABLED(sensors(SENSOR_OPFLOW))      << 5 |
 432             IS_ENABLED(sensors(SENSOR_PITOT))       << 6 |
 433             IS_ENABLED(sensors(SENSOR_TEMP))        << 7;
 434
 435     // Hardware failure indication bit
 436     if (!isHardwareHealthy()) {
 437         sensorStatus |= 1 << 15;        // Bit 15 of sensor bit field indicates hardware failure
 438     }
 439
 440     return sensorStatus;
 441 }