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