mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 03:20:00 +03:00
393 lines
14 KiB
C
393 lines
14 KiB
C
/*
|
|
* This file is part of Cleanflight and Betaflight.
|
|
*
|
|
* Cleanflight and Betaflight are free software. You can redistribute
|
|
* this software and/or modify this software under the terms of the
|
|
* GNU General Public License as published by the Free Software
|
|
* Foundation, either version 3 of the License, or (at your option)
|
|
* any later version.
|
|
*
|
|
* Cleanflight and Betaflight are distributed in the hope that they
|
|
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
|
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
* See the GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this software.
|
|
*
|
|
* If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#include "common/bitarray.h"
|
|
#include "common/streambuf.h"
|
|
#include "common/utils.h"
|
|
|
|
#include "config/feature.h"
|
|
|
|
#include "config/config.h"
|
|
#include "fc/runtime_config.h"
|
|
|
|
#include "flight/mixer.h"
|
|
#include "flight/pid.h"
|
|
|
|
#include "sensors/sensors.h"
|
|
|
|
#include "telemetry/telemetry.h"
|
|
|
|
#include "pg/piniobox.h"
|
|
|
|
#include "msp_box.h"
|
|
|
|
|
|
// permanent IDs must uniquely identify BOX meaning, DO NOT REUSE THEM!
|
|
static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
|
|
{ .boxId = BOXARM, .boxName = "ARM", .permanentId = 0 },
|
|
{ .boxId = BOXANGLE, .boxName = "ANGLE", .permanentId = 1 },
|
|
{ .boxId = BOXHORIZON, .boxName = "HORIZON", .permanentId = 2 },
|
|
// { .boxId = BOXBARO, .boxName = "BARO", .permanentId = 3 },
|
|
{ .boxId = BOXANTIGRAVITY, .boxName = "ANTI GRAVITY", .permanentId = 4 },
|
|
{ .boxId = BOXMAG, .boxName = "MAG", .permanentId = 5 },
|
|
{ .boxId = BOXHEADFREE, .boxName = "HEADFREE", .permanentId = 6 },
|
|
{ .boxId = BOXHEADADJ, .boxName = "HEADADJ", .permanentId = 7 },
|
|
{ .boxId = BOXCAMSTAB, .boxName = "CAMSTAB", .permanentId = 8 },
|
|
// { .boxId = BOXCAMTRIG, .boxName = "CAMTRIG", .permanentId = 9 },
|
|
// { .boxId = BOXGPSHOME, .boxName = "GPS HOME", .permanentId = 10 },
|
|
// { .boxId = BOXGPSHOLD, .boxName = "GPS HOLD", .permanentId = 11 },
|
|
{ .boxId = BOXPASSTHRU, .boxName = "PASSTHRU", .permanentId = 12 },
|
|
{ .boxId = BOXBEEPERON, .boxName = "BEEPER", .permanentId = 13 },
|
|
// { .boxId = BOXLEDMAX, .boxName = "LEDMAX", .permanentId = 14 }, (removed)
|
|
{ .boxId = BOXLEDLOW, .boxName = "LEDLOW", .permanentId = 15 },
|
|
// { .boxId = BOXLLIGHTS, .boxName = "LLIGHTS", .permanentId = 16 }, (removed)
|
|
{ .boxId = BOXCALIB, .boxName = "CALIB", .permanentId = 17 },
|
|
// { .boxId = BOXGOV, .boxName = "GOVERNOR", .permanentId = 18 }, (removed)
|
|
{ .boxId = BOXOSD, .boxName = "OSD DISABLE", .permanentId = 19 },
|
|
{ .boxId = BOXTELEMETRY, .boxName = "TELEMETRY", .permanentId = 20 },
|
|
// { .boxId = BOXGTUNE, .boxName = "GTUNE", .permanentId = 21 }, (removed)
|
|
// { .boxId = BOXRANGEFINDER, .boxName = "RANGEFINDER", .permanentId = 22 }, (removed)
|
|
{ .boxId = BOXSERVO1, .boxName = "SERVO1", .permanentId = 23 },
|
|
{ .boxId = BOXSERVO2, .boxName = "SERVO2", .permanentId = 24 },
|
|
{ .boxId = BOXSERVO3, .boxName = "SERVO3", .permanentId = 25 },
|
|
{ .boxId = BOXBLACKBOX, .boxName = "BLACKBOX", .permanentId = 26 },
|
|
{ .boxId = BOXFAILSAFE, .boxName = "FAILSAFE", .permanentId = 27 },
|
|
{ .boxId = BOXAIRMODE, .boxName = "AIR MODE", .permanentId = 28 },
|
|
{ .boxId = BOX3D, .boxName = "3D DISABLE / SWITCH", .permanentId = 29},
|
|
{ .boxId = BOXFPVANGLEMIX, .boxName = "FPV ANGLE MIX", .permanentId = 30},
|
|
{ .boxId = BOXBLACKBOXERASE, .boxName = "BLACKBOX ERASE", .permanentId = 31 },
|
|
{ .boxId = BOXCAMERA1, .boxName = "CAMERA CONTROL 1", .permanentId = 32},
|
|
{ .boxId = BOXCAMERA2, .boxName = "CAMERA CONTROL 2", .permanentId = 33},
|
|
{ .boxId = BOXCAMERA3, .boxName = "CAMERA CONTROL 3", .permanentId = 34 },
|
|
{ .boxId = BOXFLIPOVERAFTERCRASH, .boxName = "FLIP OVER AFTER CRASH", .permanentId = 35 },
|
|
{ .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 36 },
|
|
{ .boxId = BOXBEEPGPSCOUNT, .boxName = "GPS BEEP SATELLITE COUNT", .permanentId = 37 },
|
|
// { .boxId = BOX3DONASWITCH, .boxName = "3D ON A SWITCH", .permanentId = 38 }, (removed)
|
|
{ .boxId = BOXVTXPITMODE, .boxName = "VTX PIT MODE", .permanentId = 39 },
|
|
{ .boxId = BOXUSER1, .boxName = "USER1", .permanentId = 40 },
|
|
{ .boxId = BOXUSER2, .boxName = "USER2", .permanentId = 41 },
|
|
{ .boxId = BOXUSER3, .boxName = "USER3", .permanentId = 42 },
|
|
{ .boxId = BOXUSER4, .boxName = "USER4", .permanentId = 43 },
|
|
{ .boxId = BOXPIDAUDIO, .boxName = "PID AUDIO", .permanentId = 44 },
|
|
{ .boxId = BOXPARALYZE, .boxName = "PARALYZE", .permanentId = 45 },
|
|
{ .boxId = BOXGPSRESCUE, .boxName = "GPS RESCUE", .permanentId = 46 },
|
|
{ .boxId = BOXACROTRAINER, .boxName = "ACRO TRAINER", .permanentId = 47 },
|
|
{ .boxId = BOXVTXCONTROLDISABLE, .boxName = "VTX CONTROL DISABLE", .permanentId = 48},
|
|
{ .boxId = BOXLAUNCHCONTROL, .boxName = "LAUNCH CONTROL", .permanentId = 49 },
|
|
{ .boxId = BOXMSPOVERRIDE, .boxName = "MSP OVERRIDE", .permanentId = 50},
|
|
{ .boxId = BOXSTICKCOMMANDDISABLE, .boxName = "STICK COMMANDS DISABLE", .permanentId = 51},
|
|
{ .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 52},
|
|
{ .boxId = BOXREADY, .boxName = "READY", .permanentId = 53},
|
|
{ .boxId = BOXLAPTIMERRESET, .boxName = "LAP TIMER RESET", .permanentId = 54},
|
|
{ .boxId = BOXLEDNOBLINK, .boxName = "LED NO BLINK", .permanentId = 101},
|
|
};
|
|
|
|
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
|
|
|
|
static boxBitmask_t activeBoxIds;
|
|
|
|
const box_t *findBoxByBoxId(boxId_e boxId)
|
|
{
|
|
for (unsigned i = 0; i < ARRAYLEN(boxes); i++) {
|
|
const box_t *candidate = &boxes[i];
|
|
if (candidate->boxId == boxId)
|
|
return candidate;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
const box_t *findBoxByPermanentId(uint8_t permanentId)
|
|
{
|
|
for (unsigned i = 0; i < ARRAYLEN(boxes); i++) {
|
|
const box_t *candidate = &boxes[i];
|
|
if (candidate->permanentId == permanentId)
|
|
return candidate;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
static bool activeBoxIdGet(boxId_e boxId)
|
|
{
|
|
if (boxId > sizeof(activeBoxIds) * 8) {
|
|
return false;
|
|
}
|
|
|
|
return bitArrayGet(&activeBoxIds, boxId);
|
|
}
|
|
|
|
void serializeBoxNameFn(sbuf_t *dst, const box_t *box)
|
|
{
|
|
#if defined(USE_CUSTOM_BOX_NAMES)
|
|
if (box->boxId == BOXUSER1 && strlen(modeActivationConfig()->box_user_1_name) > 0) {
|
|
sbufWriteString(dst, modeActivationConfig()->box_user_1_name);
|
|
} else if (box->boxId == BOXUSER2 && strlen(modeActivationConfig()->box_user_2_name) > 0) {
|
|
sbufWriteString(dst, modeActivationConfig()->box_user_2_name);
|
|
} else if (box->boxId == BOXUSER3 && strlen(modeActivationConfig()->box_user_3_name) > 0) {
|
|
sbufWriteString(dst, modeActivationConfig()->box_user_3_name);
|
|
} else if (box->boxId == BOXUSER4 && strlen(modeActivationConfig()->box_user_4_name) > 0) {
|
|
sbufWriteString(dst, modeActivationConfig()->box_user_4_name);
|
|
} else
|
|
#endif
|
|
{
|
|
sbufWriteString(dst, box->boxName);
|
|
}
|
|
sbufWriteU8(dst, ';');
|
|
}
|
|
|
|
void serializeBoxPermanentIdFn(sbuf_t *dst, const box_t *box)
|
|
{
|
|
sbufWriteU8(dst, box->permanentId);
|
|
}
|
|
|
|
// serialize 'page' of boxNames.
|
|
// Each page contains at most 32 boxes
|
|
void serializeBoxReply(sbuf_t *dst, int page, serializeBoxFn *serializeBox)
|
|
{
|
|
unsigned boxIdx = 0;
|
|
unsigned pageStart = page * 32;
|
|
unsigned pageEnd = pageStart + 32;
|
|
for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) {
|
|
if (activeBoxIdGet(id)) {
|
|
if (boxIdx >= pageStart && boxIdx < pageEnd) {
|
|
(*serializeBox)(dst, findBoxByBoxId(id));
|
|
}
|
|
boxIdx++; // count active boxes
|
|
}
|
|
}
|
|
}
|
|
|
|
void initActiveBoxIds(void)
|
|
{
|
|
// calculate used boxes based on features and set corresponding activeBoxIds bits
|
|
boxBitmask_t ena; // temporary variable to collect result
|
|
memset(&ena, 0, sizeof(ena));
|
|
|
|
// macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only
|
|
#define BME(boxId) do { bitArraySet(&ena, boxId); } while (0)
|
|
BME(BOXARM);
|
|
BME(BOXPREARM);
|
|
if (!featureIsEnabled(FEATURE_AIRMODE)) {
|
|
BME(BOXAIRMODE);
|
|
}
|
|
|
|
bool acceleratorGainsEnabled = false;
|
|
for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
|
|
if (pidProfiles(i)->anti_gravity_gain != ITERM_ACCELERATOR_GAIN_OFF) {
|
|
acceleratorGainsEnabled = true;
|
|
}
|
|
}
|
|
if (acceleratorGainsEnabled && !featureIsEnabled(FEATURE_ANTI_GRAVITY)) {
|
|
BME(BOXANTIGRAVITY);
|
|
}
|
|
|
|
if (sensors(SENSOR_ACC)) {
|
|
BME(BOXANGLE);
|
|
BME(BOXHORIZON);
|
|
BME(BOXHEADFREE);
|
|
BME(BOXHEADADJ);
|
|
BME(BOXFPVANGLEMIX);
|
|
if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) {
|
|
BME(BOXCALIB);
|
|
}
|
|
#if defined(USE_ACRO_TRAINER) && defined(USE_ACC)
|
|
BME(BOXACROTRAINER);
|
|
#endif // USE_ACRO_TRAINER
|
|
}
|
|
|
|
#ifdef USE_MAG
|
|
if (sensors(SENSOR_MAG)) {
|
|
BME(BOXMAG);
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_GPS
|
|
if (featureIsEnabled(FEATURE_GPS)) {
|
|
#ifdef USE_GPS_RESCUE
|
|
if (!featureIsEnabled(FEATURE_3D) && !isFixedWing()) {
|
|
BME(BOXGPSRESCUE);
|
|
}
|
|
#endif
|
|
BME(BOXBEEPGPSCOUNT);
|
|
}
|
|
#endif
|
|
|
|
BME(BOXFAILSAFE);
|
|
|
|
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
|
|
BME(BOXPASSTHRU);
|
|
}
|
|
|
|
BME(BOXBEEPERON);
|
|
BME(BOXBEEPERMUTE);
|
|
|
|
#ifdef USE_LED_STRIP
|
|
if (featureIsEnabled(FEATURE_LED_STRIP)) {
|
|
BME(BOXLEDLOW);
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_BLACKBOX
|
|
BME(BOXBLACKBOX);
|
|
#ifdef USE_FLASHFS
|
|
BME(BOXBLACKBOXERASE);
|
|
#endif
|
|
#endif
|
|
|
|
if (featureIsEnabled(FEATURE_3D)) {
|
|
BME(BOX3D);
|
|
}
|
|
|
|
#ifdef USE_DSHOT
|
|
bool configuredMotorProtocolDshot;
|
|
checkMotorProtocolEnabled(&motorConfig()->dev, &configuredMotorProtocolDshot);
|
|
if (configuredMotorProtocolDshot) {
|
|
BME(BOXFLIPOVERAFTERCRASH);
|
|
}
|
|
#endif
|
|
|
|
if (featureIsEnabled(FEATURE_SERVO_TILT)) {
|
|
BME(BOXCAMSTAB);
|
|
}
|
|
|
|
BME(BOXOSD);
|
|
|
|
#ifdef USE_TELEMETRY
|
|
if (featureIsEnabled(FEATURE_TELEMETRY)) {
|
|
BME(BOXTELEMETRY);
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_SERVOS
|
|
if (mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
|
|
BME(BOXSERVO1);
|
|
BME(BOXSERVO2);
|
|
BME(BOXSERVO3);
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_RCDEVICE
|
|
BME(BOXCAMERA1);
|
|
BME(BOXCAMERA2);
|
|
BME(BOXCAMERA3);
|
|
#endif
|
|
|
|
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
|
|
BME(BOXVTXPITMODE);
|
|
BME(BOXVTXCONTROLDISABLE);
|
|
#endif
|
|
|
|
BME(BOXPARALYZE);
|
|
|
|
#ifdef USE_PINIOBOX
|
|
// Turn BOXUSERx only if pinioBox facility monitors them, as the facility is the only BOXUSERx observer.
|
|
// Note that pinioBoxConfig can be set to monitor any box.
|
|
for (int i = 0; i < PINIO_COUNT; i++) {
|
|
if (pinioBoxConfig()->permanentId[i] != PERMANENT_ID_NONE) {
|
|
const box_t *box = findBoxByPermanentId(pinioBoxConfig()->permanentId[i]);
|
|
if (box) {
|
|
switch(box->boxId) {
|
|
case BOXUSER1:
|
|
case BOXUSER2:
|
|
case BOXUSER3:
|
|
case BOXUSER4:
|
|
BME(box->boxId);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#if defined(USE_PID_AUDIO)
|
|
BME(BOXPIDAUDIO);
|
|
#endif
|
|
|
|
#ifdef USE_LAUNCH_CONTROL
|
|
BME(BOXLAUNCHCONTROL);
|
|
#endif
|
|
|
|
#if defined(USE_RX_MSP_OVERRIDE)
|
|
if (rxConfig()->msp_override_channels_mask) {
|
|
BME(BOXMSPOVERRIDE);
|
|
}
|
|
#endif
|
|
|
|
BME(BOXSTICKCOMMANDDISABLE);
|
|
BME(BOXREADY);
|
|
BME(BOXLEDNOBLINK);
|
|
|
|
#if defined(USE_GPS_LAP_TIMER)
|
|
BME(BOXLAPTIMERRESET);
|
|
#endif
|
|
|
|
#undef BME
|
|
// check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
|
|
for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++)
|
|
if (bitArrayGet(&ena, boxId)
|
|
&& findBoxByBoxId(boxId) == NULL)
|
|
bitArrayClr(&ena, boxId); // this should not happen, but handle it gracefully
|
|
|
|
activeBoxIds = ena; // set global variable
|
|
}
|
|
|
|
// return state of given boxId box, handling ARM and FLIGHT_MODE
|
|
bool getBoxIdState(boxId_e boxid)
|
|
{
|
|
const uint8_t boxIdToFlightModeMap[] = BOXID_TO_FLIGHT_MODE_MAP_INITIALIZER;
|
|
|
|
// we assume that all boxId below BOXID_FLIGHTMODE_LAST except BOXARM are mapped to flightmode
|
|
STATIC_ASSERT(ARRAYLEN(boxIdToFlightModeMap) == BOXID_FLIGHTMODE_LAST + 1, FLIGHT_MODE_BOXID_MAP_INITIALIZER_does_not_match_boxId_e);
|
|
|
|
if (boxid == BOXARM) {
|
|
return ARMING_FLAG(ARMED);
|
|
} else if (boxid <= BOXID_FLIGHTMODE_LAST) {
|
|
return FLIGHT_MODE(1 << boxIdToFlightModeMap[boxid]);
|
|
} else {
|
|
return IS_RC_MODE_ACTIVE(boxid);
|
|
}
|
|
}
|
|
|
|
// pack used flightModeFlags into supplied array
|
|
// returns number of bits used
|
|
int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags)
|
|
{
|
|
// Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
|
|
memset(mspFlightModeFlags, 0, sizeof(boxBitmask_t));
|
|
// map boxId_e enabled bits to MSP status indexes
|
|
// only active boxIds are sent in status over MSP, other bits are not counted
|
|
unsigned mspBoxIdx = 0; // index of active boxId (matches sent permanentId and boxNames)
|
|
for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) {
|
|
if (activeBoxIdGet(boxId)) {
|
|
if (getBoxIdState(boxId))
|
|
bitArraySet(mspFlightModeFlags, mspBoxIdx); // box is enabled
|
|
mspBoxIdx++; // box is active, count it
|
|
}
|
|
}
|
|
// return count of used bits
|
|
return mspBoxIdx;
|
|
}
|