src/main/msp/msp_box.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #include "common/bitarray.h"
  28 #include "common/streambuf.h"
  29 #include "common/utils.h"
  30
  31 #include "config/feature.h"
  32
  33 #include "config/config.h"
  34 #include "fc/runtime_config.h"
  35
  36 #include "flight/mixer.h"
  37 #include "flight/pid.h"
  38
  39 #include "sensors/sensors.h"
  40
  41 #include "telemetry/telemetry.h"
  42
  43 #include "pg/piniobox.h"
  44
  45 #include "msp_box.h"
  46
  47 // permanent IDs must uniquely identify BOX meaning, DO NOT REUSE THEM!
  48 static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
  49     { .boxId = BOXARM, .boxName = "ARM", .permanentId = 0 },
  50     { .boxId = BOXANGLE, .boxName = "ANGLE", .permanentId = 1 },
  51     { .boxId = BOXHORIZON, .boxName = "HORIZON", .permanentId = 2 },
  52     { .boxId = BOXALTHOLD, .boxName = "ALTHOLD", .permanentId = 3 },
  53     { .boxId = BOXANTIGRAVITY, .boxName = "ANTI GRAVITY", .permanentId = 4 },
  54     { .boxId = BOXMAG, .boxName = "MAG", .permanentId = 5 },
  55     { .boxId = BOXHEADFREE, .boxName = "HEADFREE", .permanentId = 6 },
  56     { .boxId = BOXHEADADJ, .boxName = "HEADADJ", .permanentId = 7 },
  57     { .boxId = BOXCAMSTAB, .boxName = "CAMSTAB", .permanentId = 8 },
  58 //    { .boxId = BOXCAMTRIG, .boxName = "CAMTRIG", .permanentId = 9 },
  59 //    { .boxId = BOXGPSHOME, .boxName = "GPS HOME", .permanentId = 10 },
  60     { .boxId = BOXPOSHOLD, .boxName = "POS HOLD", .permanentId = 11 },
  61     { .boxId = BOXPASSTHRU, .boxName = "PASSTHRU", .permanentId = 12 },
  62     { .boxId = BOXBEEPERON, .boxName = "BEEPER", .permanentId = 13 },
  63 //    { .boxId = BOXLEDMAX, .boxName = "LEDMAX", .permanentId = 14 }, (removed)
  64     { .boxId = BOXLEDLOW, .boxName = "LEDLOW", .permanentId = 15 },
  65 //    { .boxId = BOXLLIGHTS, .boxName = "LLIGHTS", .permanentId = 16 }, (removed)
  66     { .boxId = BOXCALIB, .boxName = "CALIB", .permanentId = 17 },
  67 //    { .boxId = BOXGOV, .boxName = "GOVERNOR", .permanentId = 18 }, (removed)
  68     { .boxId = BOXOSD, .boxName = "OSD DISABLE", .permanentId = 19 },
  69     { .boxId = BOXTELEMETRY, .boxName = "TELEMETRY", .permanentId = 20 },
  70 //    { .boxId = BOXGTUNE, .boxName = "GTUNE", .permanentId = 21 }, (removed)
  71 //    { .boxId = BOXRANGEFINDER, .boxName = "RANGEFINDER", .permanentId = 22 }, (removed)
  72     { .boxId = BOXSERVO1, .boxName = "SERVO1", .permanentId = 23 },
  73     { .boxId = BOXSERVO2, .boxName = "SERVO2", .permanentId = 24 },
  74     { .boxId = BOXSERVO3, .boxName = "SERVO3", .permanentId = 25 },
  75     { .boxId = BOXBLACKBOX, .boxName = "BLACKBOX", .permanentId = 26 },
  76     { .boxId = BOXFAILSAFE, .boxName = "FAILSAFE", .permanentId = 27 },
  77     { .boxId = BOXAIRMODE, .boxName = "AIR MODE", .permanentId = 28 },
  78     { .boxId = BOX3D, .boxName = "3D DISABLE", .permanentId = 29},
  79     { .boxId = BOXFPVANGLEMIX, .boxName = "FPV ANGLE MIX", .permanentId = 30},
  80     { .boxId = BOXBLACKBOXERASE, .boxName = "BLACKBOX ERASE", .permanentId = 31 },
  81     { .boxId = BOXCAMERA1, .boxName = "CAMERA CONTROL 1", .permanentId = 32},
  82     { .boxId = BOXCAMERA2, .boxName = "CAMERA CONTROL 2", .permanentId = 33},
  83     { .boxId = BOXCAMERA3, .boxName = "CAMERA CONTROL 3", .permanentId = 34 },
  84     { .boxId = BOXCRASHFLIP, .boxName = "FLIP OVER AFTER CRASH", .permanentId = 35 },
  85     { .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 36 },
  86     { .boxId = BOXBEEPGPSCOUNT, .boxName = "GPS BEEP SATELLITE COUNT", .permanentId = 37 },
  87 //    { .boxId = BOX3DONASWITCH, .boxName = "3D ON A SWITCH", .permanentId = 38 }, (removed)
  88     { .boxId = BOXVTXPITMODE, .boxName = "VTX PIT MODE", .permanentId = 39 },
  89     { .boxId = BOXUSER1, .boxName = BOX_USER1_NAME, .permanentId = 40 }, // may be overridden by modeActivationConfig
  90     { .boxId = BOXUSER2, .boxName = BOX_USER2_NAME, .permanentId = 41 },
  91     { .boxId = BOXUSER3, .boxName = BOX_USER3_NAME, .permanentId = 42 },
  92     { .boxId = BOXUSER4, .boxName = BOX_USER4_NAME, .permanentId = 43 },
  93     { .boxId = BOXPIDAUDIO, .boxName = "PID AUDIO", .permanentId = 44 },
  94     { .boxId = BOXPARALYZE, .boxName = "PARALYZE", .permanentId = 45 },
  95     { .boxId = BOXGPSRESCUE, .boxName = "GPS RESCUE", .permanentId = 46 },
  96     { .boxId = BOXACROTRAINER, .boxName = "ACRO TRAINER", .permanentId = 47 },
  97     { .boxId = BOXVTXCONTROLDISABLE, .boxName = "VTX CONTROL DISABLE", .permanentId = 48},
  98     { .boxId = BOXLAUNCHCONTROL, .boxName = "LAUNCH CONTROL", .permanentId = 49 },
  99     { .boxId = BOXMSPOVERRIDE, .boxName = "MSP OVERRIDE", .permanentId = 50},
 100     { .boxId = BOXSTICKCOMMANDDISABLE, .boxName = "STICK COMMANDS DISABLE", .permanentId = 51},
 101     { .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 52},
 102     { .boxId = BOXREADY, .boxName = "READY", .permanentId = 53},
 103     { .boxId = BOXLAPTIMERRESET, .boxName = "LAP TIMER RESET", .permanentId = 54},
 104 };
 105
 106 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
 107
 108 static boxBitmask_t activeBoxIds;
 109
 110 const box_t *findBoxByBoxId(boxId_e boxId)
 111 {
 112     for (unsigned i = 0; i < ARRAYLEN(boxes); i++) {
 113         const box_t *candidate = &boxes[i];
 114         if (candidate->boxId == boxId)
 115             return candidate;
 116     }
 117     return NULL;
 118 }
 119
 120 const box_t *findBoxByPermanentId(uint8_t permanentId)
 121 {
 122     for (unsigned i = 0; i < ARRAYLEN(boxes); i++) {
 123         const box_t *candidate = &boxes[i];
 124         if (candidate->permanentId == permanentId)
 125             return candidate;
 126     }
 127     return NULL;
 128 }
 129
 130 static bool activeBoxIdGet(boxId_e boxId)
 131 {
 132     if (boxId > sizeof(activeBoxIds) * 8) {
 133         return false;
 134     }
 135
 136     return bitArrayGet(&activeBoxIds, boxId);
 137 }
 138
 139 int serializeBoxNameFn(sbuf_t *dst, const box_t *box)
 140 {
 141     const char* name = NULL;
 142     int len;
 143 #if defined(USE_CUSTOM_BOX_NAMES)
 144     if (box->boxId >= BOXUSER1 && box->boxId <= BOXUSER4) {
 145         const int n = box->boxId - BOXUSER1;
 146         name = modeActivationConfig()->box_user_names[n];
 147         // possibly there is no '\0' in boxname
 148         if (*name) {
 149             len = strnlen(name, sizeof(modeActivationConfig()->box_user_names[n]));
 150         } else {
 151             name = NULL;
 152         }
 153     }
 154 #endif
 155     if (name == NULL) {
 156         name = box->boxName;
 157         len = strlen(name);
 158     }
 159     if (sbufBytesRemaining(dst) < len + 1) {
 160         // boxname or separator won't fit
 161         return -1;
 162     }
 163     sbufWriteData(dst, name, len);
 164     sbufWriteU8(dst, ';');
 165     return len + 1;
 166 }
 167
 168 int serializeBoxPermanentIdFn(sbuf_t *dst, const box_t *box)
 169 {
 170     if (sbufBytesRemaining(dst) < 1) {
 171         return -1;
 172     }
 173     sbufWriteU8(dst, box->permanentId);
 174     return 1;
 175 }
 176
 177 // serialize 'page' of boxNames.
 178 // Each page contains at most 32 boxes
 179 void serializeBoxReply(sbuf_t *dst, int page, serializeBoxFn *serializeBox)
 180 {
 181     unsigned boxIdx = 0;
 182     unsigned pageStart = page * 32;
 183     unsigned pageEnd = pageStart + 32;
 184     for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) {
 185         if (activeBoxIdGet(id)) {
 186             if (boxIdx >= pageStart && boxIdx < pageEnd) {
 187                 if ((*serializeBox)(dst, findBoxByBoxId(id)) < 0) {
 188                     // failed to serialize, abort
 189                     return;
 190                 }
 191             }
 192             boxIdx++;                 // count active boxes
 193         }
 194     }
 195 }
 196
 197 void initActiveBoxIds(void)
 198 {
 199     // calculate used boxes based on features and set corresponding activeBoxIds bits
 200     boxBitmask_t ena;  // temporary variable to collect result
 201     memset(&ena, 0, sizeof(ena));
 202
 203     // macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only
 204 #define BME(boxId) do { bitArraySet(&ena, boxId); } while (0)
 205     BME(BOXARM);
 206     BME(BOXPREARM);
 207     if (!featureIsEnabled(FEATURE_AIRMODE)) {
 208         BME(BOXAIRMODE);
 209     }
 210
 211     bool acceleratorGainsEnabled = false;
 212     for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
 213         if (pidProfiles(i)->anti_gravity_gain != ITERM_ACCELERATOR_GAIN_OFF) {
 214             acceleratorGainsEnabled = true;
 215         }
 216     }
 217     if (acceleratorGainsEnabled && !featureIsEnabled(FEATURE_ANTI_GRAVITY)) {
 218         BME(BOXANTIGRAVITY);
 219     }
 220
 221     if (sensors(SENSOR_ACC)) {
 222         BME(BOXANGLE);
 223         BME(BOXHORIZON);
 224 #ifdef USE_ALTITUDE_HOLD
 225         BME(BOXALTHOLD);
 226 #endif
 227 #ifdef USE_POSITION_HOLD
 228         BME(BOXPOSHOLD);
 229 #endif
 230         BME(BOXHEADFREE);
 231         BME(BOXHEADADJ);
 232         BME(BOXFPVANGLEMIX);
 233         if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) {
 234             BME(BOXCALIB);
 235         }
 236 #if defined(USE_ACRO_TRAINER) && defined(USE_ACC)
 237         BME(BOXACROTRAINER);
 238 #endif // USE_ACRO_TRAINER
 239     }
 240
 241 #ifdef USE_MAG
 242     if (sensors(SENSOR_MAG)) {
 243         BME(BOXMAG);
 244     }
 245 #endif
 246
 247 #ifdef USE_GPS
 248     if (featureIsEnabled(FEATURE_GPS)) {
 249 #ifdef USE_GPS_RESCUE
 250         if (!featureIsEnabled(FEATURE_3D) && !isFixedWing()) {
 251             BME(BOXGPSRESCUE);
 252         }
 253 #endif
 254         BME(BOXBEEPGPSCOUNT);
 255     }
 256 #endif
 257
 258     BME(BOXFAILSAFE);
 259
 260     if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
 261         BME(BOXPASSTHRU);
 262     }
 263
 264     BME(BOXBEEPERON);
 265     BME(BOXBEEPERMUTE);
 266
 267 #ifdef USE_LED_STRIP
 268     if (featureIsEnabled(FEATURE_LED_STRIP)) {
 269         BME(BOXLEDLOW);
 270     }
 271 #endif
 272
 273 #ifdef USE_BLACKBOX
 274     BME(BOXBLACKBOX);
 275 #ifdef USE_FLASHFS
 276     BME(BOXBLACKBOXERASE);
 277 #endif
 278 #endif
 279
 280     if (featureIsEnabled(FEATURE_3D)) {
 281         BME(BOX3D);
 282     }
 283
 284 #ifdef USE_DSHOT
 285     bool configuredMotorProtocolDshot;
 286     checkMotorProtocolEnabled(&motorConfig()->dev, &configuredMotorProtocolDshot);
 287     if (configuredMotorProtocolDshot) {
 288         BME(BOXCRASHFLIP);
 289     }
 290 #endif
 291
 292     if (featureIsEnabled(FEATURE_SERVO_TILT)) {
 293         BME(BOXCAMSTAB);
 294     }
 295
 296     BME(BOXOSD);
 297
 298 #ifdef USE_TELEMETRY
 299     if (featureIsEnabled(FEATURE_TELEMETRY)) {
 300         BME(BOXTELEMETRY);
 301     }
 302 #endif
 303
 304 #ifdef USE_SERVOS
 305     if (mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
 306         BME(BOXSERVO1);
 307         BME(BOXSERVO2);
 308         BME(BOXSERVO3);
 309     }
 310 #endif
 311
 312 #ifdef USE_RCDEVICE
 313     BME(BOXCAMERA1);
 314     BME(BOXCAMERA2);
 315     BME(BOXCAMERA3);
 316 #endif
 317
 318 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
 319     BME(BOXVTXPITMODE);
 320     BME(BOXVTXCONTROLDISABLE);
 321 #endif
 322
 323     BME(BOXPARALYZE);
 324
 325 #ifdef USE_PINIOBOX
 326     // Turn BOXUSERx only if pinioBox facility monitors them, as the facility is the only BOXUSERx observer.
 327     // Note that pinioBoxConfig can be set to monitor any box.
 328     for (int i = 0; i < PINIO_COUNT; i++) {
 329         if (pinioBoxConfig()->permanentId[i] != PERMANENT_ID_NONE) {
 330             const box_t *box = findBoxByPermanentId(pinioBoxConfig()->permanentId[i]);
 331             if (box) {
 332                 switch(box->boxId) {
 333                 case BOXUSER1:
 334                 case BOXUSER2:
 335                 case BOXUSER3:
 336                 case BOXUSER4:
 337                     BME(box->boxId);
 338                     break;
 339                 default:
 340                     break;
 341                 }
 342             }
 343         }
 344     }
 345 #endif
 346
 347 #if defined(USE_PID_AUDIO)
 348     BME(BOXPIDAUDIO);
 349 #endif
 350
 351 #ifdef USE_LAUNCH_CONTROL
 352     BME(BOXLAUNCHCONTROL);
 353 #endif
 354
 355 #if defined(USE_RX_MSP_OVERRIDE)
 356     if (rxConfig()->msp_override_channels_mask) {
 357         BME(BOXMSPOVERRIDE);
 358     }
 359 #endif
 360
 361     BME(BOXSTICKCOMMANDDISABLE);
 362     BME(BOXREADY);
 363
 364 #if defined(USE_GPS_LAP_TIMER)
 365     BME(BOXLAPTIMERRESET);
 366 #endif
 367
 368 #undef BME
 369     // check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
 370     for (boxId_e boxId = 0;  boxId < CHECKBOX_ITEM_COUNT; boxId++)
 371         if (bitArrayGet(&ena, boxId)
 372             && findBoxByBoxId(boxId) == NULL)
 373             bitArrayClr(&ena, boxId);                 // this should not happen, but handle it gracefully
 374
 375     activeBoxIds = ena;                               // set global variable
 376 }
 377
 378 // return state of given boxId box, handling ARM and FLIGHT_MODE
 379 bool getBoxIdState(boxId_e boxid)
 380 {
 381     const uint8_t boxIdToFlightModeMap[] = BOXID_TO_FLIGHT_MODE_MAP_INITIALIZER;
 382
 383     // we assume that all boxId below BOXID_FLIGHTMODE_LAST except BOXARM are mapped to flightmode
 384     STATIC_ASSERT(ARRAYLEN(boxIdToFlightModeMap) == BOXID_FLIGHTMODE_LAST + 1, FLIGHT_MODE_BOXID_MAP_INITIALIZER_does_not_match_boxId_e);
 385
 386     if (boxid == BOXARM) {
 387         return ARMING_FLAG(ARMED);
 388     } else if (boxid <= BOXID_FLIGHTMODE_LAST) {
 389         return FLIGHT_MODE(1 << boxIdToFlightModeMap[boxid]);
 390     } else {
 391         return IS_RC_MODE_ACTIVE(boxid);
 392     }
 393 }
 394
 395 // pack used flightModeFlags into supplied array
 396 // returns number of bits used
 397 int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags)
 398 {
 399     // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
 400     memset(mspFlightModeFlags, 0, sizeof(boxBitmask_t));
 401     // map boxId_e enabled bits to MSP status indexes
 402     // only active boxIds are sent in status over MSP, other bits are not counted
 403     unsigned mspBoxIdx = 0;           // index of active boxId (matches sent permanentId and boxNames)
 404     for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) {
 405         if (activeBoxIdGet(boxId)) {
 406             if (getBoxIdState(boxId))
 407                 bitArraySet(mspFlightModeFlags, mspBoxIdx);       // box is enabled
 408             mspBoxIdx++;                                          // box is active, count it
 409         }
 410     }
 411     // return count of used bits
 412     return mspBoxIdx;
 413 }