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