2 * This file is part of Cleanflight and Betaflight.
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)
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.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
27 #include "common/bitarray.h"
28 #include "common/streambuf.h"
29 #include "common/utils.h"
31 #include "config/feature.h"
33 #include "config/config.h"
34 #include "fc/runtime_config.h"
36 #include "flight/mixer.h"
37 #include "flight/pid.h"
39 #include "sensors/sensors.h"
41 #include "telemetry/telemetry.h"
43 #include "pg/piniobox.h"
48 // permanent IDs must uniquely identify BOX meaning, DO NOT REUSE THEM!
49 static const box_t boxes
[CHECKBOX_ITEM_COUNT
] = {
51 { BOXANGLE
, "ANGLE", 1 },
52 { BOXHORIZON
, "HORIZON", 2 },
53 // { BOXBARO, "BARO", 3 },
54 { BOXANTIGRAVITY
, "ANTI GRAVITY", 4 },
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 SW", 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
, "DISABLE / SWITCH 3D", 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
, "BEEP GPS 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
, "DISABLE VTX CONTROL", 48},
99 { BOXLAUNCHCONTROL
, "LAUNCH CONTROL", 49 },
102 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
104 static boxBitmask_t activeBoxIds
;
106 const box_t
*findBoxByBoxId(boxId_e boxId
)
108 for (unsigned i
= 0; i
< ARRAYLEN(boxes
); i
++) {
109 const box_t
*candidate
= &boxes
[i
];
110 if (candidate
->boxId
== boxId
)
116 const box_t
*findBoxByPermanentId(uint8_t permanentId
)
118 for (unsigned i
= 0; i
< ARRAYLEN(boxes
); i
++) {
119 const box_t
*candidate
= &boxes
[i
];
120 if (candidate
->permanentId
== permanentId
)
126 static bool activeBoxIdGet(boxId_e boxId
)
128 if (boxId
> sizeof(activeBoxIds
) * 8) {
132 return bitArrayGet(&activeBoxIds
, boxId
);
135 void serializeBoxNameFn(sbuf_t
*dst
, const box_t
*box
)
137 #if defined(USE_CUSTOM_BOX_NAMES)
138 if (box
->boxId
== BOXUSER1
&& strlen(modeActivationConfig()->box_user_1_name
) > 0) {
139 sbufWriteString(dst
, modeActivationConfig()->box_user_1_name
);
140 } else if (box
->boxId
== BOXUSER2
&& strlen(modeActivationConfig()->box_user_2_name
) > 0) {
141 sbufWriteString(dst
, modeActivationConfig()->box_user_2_name
);
142 } else if (box
->boxId
== BOXUSER3
&& strlen(modeActivationConfig()->box_user_3_name
) > 0) {
143 sbufWriteString(dst
, modeActivationConfig()->box_user_3_name
);
144 } else if (box
->boxId
== BOXUSER4
&& strlen(modeActivationConfig()->box_user_4_name
) > 0) {
145 sbufWriteString(dst
, modeActivationConfig()->box_user_4_name
);
149 sbufWriteString(dst
, box
->boxName
);
151 sbufWriteU8(dst
, ';');
154 void serializeBoxPermanentIdFn(sbuf_t
*dst
, const box_t
*box
)
156 sbufWriteU8(dst
, box
->permanentId
);
159 // serialize 'page' of boxNames.
160 // Each page contains at most 32 boxes
161 void serializeBoxReply(sbuf_t
*dst
, int page
, serializeBoxFn
*serializeBox
)
164 unsigned pageStart
= page
* 32;
165 unsigned pageEnd
= pageStart
+ 32;
166 for (boxId_e id
= 0; id
< CHECKBOX_ITEM_COUNT
; id
++) {
167 if (activeBoxIdGet(id
)) {
168 if (boxIdx
>= pageStart
&& boxIdx
< pageEnd
) {
169 (*serializeBox
)(dst
, findBoxByBoxId(id
));
171 boxIdx
++; // count active boxes
176 void initActiveBoxIds(void)
178 // calculate used boxes based on features and set corresponding activeBoxIds bits
179 boxBitmask_t ena
; // temporary variable to collect result
180 memset(&ena
, 0, sizeof(ena
));
182 // macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only
183 #define BME(boxId) do { bitArraySet(&ena, boxId); } while (0)
186 if (!featureIsEnabled(FEATURE_AIRMODE
)) {
190 bool acceleratorGainsEnabled
= false;
191 for (unsigned i
= 0; i
< PID_PROFILE_COUNT
; i
++) {
192 if (pidProfiles(i
)->itermAcceleratorGain
!= ITERM_ACCELERATOR_GAIN_OFF
) {
193 acceleratorGainsEnabled
= true;
196 if (acceleratorGainsEnabled
&& !featureIsEnabled(FEATURE_ANTI_GRAVITY
)) {
200 if (sensors(SENSOR_ACC
)) {
208 if (sensors(SENSOR_MAG
)) {
214 if (featureIsEnabled(FEATURE_GPS
)) {
215 #ifdef USE_GPS_RESCUE
216 if (!featureIsEnabled(FEATURE_3D
)) {
220 BME(BOXBEEPGPSCOUNT
);
226 if (mixerConfig()->mixerMode
== MIXER_FLYING_WING
|| mixerConfig()->mixerMode
== MIXER_AIRPLANE
|| mixerConfig()->mixerMode
== MIXER_CUSTOM_AIRPLANE
) {
233 if (featureIsEnabled(FEATURE_LED_STRIP
)) {
241 BME(BOXBLACKBOXERASE
);
247 if (featureIsEnabled(FEATURE_3D
)) {
252 bool configuredMotorProtocolDshot
;
253 checkMotorProtocolEnabled(&motorConfig()->dev
, &configuredMotorProtocolDshot
);
254 if (configuredMotorProtocolDshot
) {
255 BME(BOXFLIPOVERAFTERCRASH
);
259 if (featureIsEnabled(FEATURE_SERVO_TILT
)) {
263 if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL
)) {
270 if (featureIsEnabled(FEATURE_TELEMETRY
)) {
276 if (mixerConfig()->mixerMode
== MIXER_CUSTOM_AIRPLANE
) {
289 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
291 BME(BOXVTXCONTROLDISABLE
);
297 // Turn BOXUSERx only if pinioBox facility monitors them, as the facility is the only BOXUSERx observer.
298 // Note that pinioBoxConfig can be set to monitor any box.
299 for (int i
= 0; i
< PINIO_COUNT
; i
++) {
300 if (pinioBoxConfig()->permanentId
[i
] != PERMANENT_ID_NONE
) {
301 const box_t
*box
= findBoxByPermanentId(pinioBoxConfig()->permanentId
[i
]);
318 #if defined(USE_PID_AUDIO)
322 #if defined(USE_ACRO_TRAINER) && defined(USE_ACC)
323 if (sensors(SENSOR_ACC
)) {
326 #endif // USE_ACRO_TRAINER
328 #ifdef USE_LAUNCH_CONTROL
329 BME(BOXLAUNCHCONTROL
);
333 // check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
334 for (boxId_e boxId
= 0; boxId
< CHECKBOX_ITEM_COUNT
; boxId
++)
335 if (bitArrayGet(&ena
, boxId
)
336 && findBoxByBoxId(boxId
) == NULL
)
337 bitArrayClr(&ena
, boxId
); // this should not happen, but handle it gracefully
339 activeBoxIds
= ena
; // set global variable
342 // return state of given boxId box, handling ARM and FLIGHT_MODE
343 bool getBoxIdState(boxId_e boxid
)
345 const uint8_t boxIdToFlightModeMap
[] = BOXID_TO_FLIGHT_MODE_MAP_INITIALIZER
;
347 // we assume that all boxId below BOXID_FLIGHTMODE_LAST except BOXARM are mapped to flightmode
348 STATIC_ASSERT(ARRAYLEN(boxIdToFlightModeMap
) == BOXID_FLIGHTMODE_LAST
+ 1, FLIGHT_MODE_BOXID_MAP_INITIALIZER_does_not_match_boxId_e
);
350 if (boxid
== BOXARM
) {
351 return ARMING_FLAG(ARMED
);
352 } else if (boxid
<= BOXID_FLIGHTMODE_LAST
) {
353 return FLIGHT_MODE(1 << boxIdToFlightModeMap
[boxid
]);
355 return IS_RC_MODE_ACTIVE(boxid
);
359 // pack used flightModeFlags into supplied array
360 // returns number of bits used
361 int packFlightModeFlags(boxBitmask_t
*mspFlightModeFlags
)
363 // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
364 memset(mspFlightModeFlags
, 0, sizeof(boxBitmask_t
));
365 // map boxId_e enabled bits to MSP status indexes
366 // only active boxIds are sent in status over MSP, other bits are not counted
367 unsigned mspBoxIdx
= 0; // index of active boxId (matches sent permanentId and boxNames)
368 for (boxId_e boxId
= 0; boxId
< CHECKBOX_ITEM_COUNT
; boxId
++) {
369 if (activeBoxIdGet(boxId
)) {
370 if (getBoxIdState(boxId
))
371 bitArraySet(mspFlightModeFlags
, mspBoxIdx
); // box is enabled
372 mspBoxIdx
++; // box is active, count it
375 // return count of used bits