mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-27 02:05:26 +03:00
Split box code out of MSP
This commit is contained in:
Martin Budden
parent
40f2a85439
commit
8c36df3ea1
5 changed files with 350 additions and 273 deletions
1
Makefile
1
Makefile
|
|
@ -591,6 +591,7 @@ COMMON_SRC = \
|
||||||
fc/fc_tasks.c \
|
fc/fc_tasks.c \
|
||||||
fc/fc_hardfaults.c \
|
fc/fc_hardfaults.c \
|
||||||
fc/fc_msp.c \
|
fc/fc_msp.c \
|
||||||
|
fc/fc_msp_box.c \
|
||||||
fc/rc_adjustments.c \
|
fc/rc_adjustments.c \
|
||||||
fc/rc_controls.c \
|
fc/rc_controls.c \
|
||||||
fc/rc_curves.c \
|
fc/rc_curves.c \
|
||||||
|
|
|
||||||
|
|
@ -48,6 +48,7 @@
|
||||||
#include "fc/config.h"
|
#include "fc/config.h"
|
||||||
#include "fc/controlrate_profile.h"
|
#include "fc/controlrate_profile.h"
|
||||||
#include "fc/fc_msp.h"
|
#include "fc/fc_msp.h"
|
||||||
|
#include "fc/fc_msp_box.h"
|
||||||
#include "fc/rc_adjustments.h"
|
#include "fc/rc_adjustments.h"
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
#include "fc/rc_modes.h"
|
#include "fc/rc_modes.h"
|
||||||
|
|
@ -106,54 +107,6 @@ extern uint16_t rssi; // FIXME dependency on mw.c
|
||||||
static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
|
static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
|
||||||
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
||||||
|
|
||||||
typedef struct box_e {
|
|
||||||
const uint8_t boxId; // see boxId_e
|
|
||||||
const char *boxName; // GUI-readable box name
|
|
||||||
const uint8_t permanentId; //
|
|
||||||
} box_t;
|
|
||||||
|
|
||||||
// FIXME remove ;'s
|
|
||||||
static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
|
||||||
{ BOXARM, "ARM;", 0 },
|
|
||||||
{ BOXANGLE, "ANGLE;", 1 },
|
|
||||||
{ BOXHORIZON, "HORIZON;", 2 },
|
|
||||||
{ BOXNAVALTHOLD, "NAV ALTHOLD;", 3 }, // old BARO
|
|
||||||
{ BOXHEADINGHOLD, "HEADING HOLD;", 5 },
|
|
||||||
{ BOXHEADFREE, "HEADFREE;", 6 },
|
|
||||||
{ BOXHEADADJ, "HEADADJ;", 7 },
|
|
||||||
{ BOXCAMSTAB, "CAMSTAB;", 8 },
|
|
||||||
{ BOXNAVRTH, "NAV RTH;", 10 }, // old GPS HOME
|
|
||||||
{ BOXNAVPOSHOLD, "NAV POSHOLD;", 11 }, // old GPS HOLD
|
|
||||||
{ BOXPASSTHRU, "PASSTHRU;", 12 },
|
|
||||||
{ BOXBEEPERON, "BEEPER;", 13 },
|
|
||||||
{ BOXLEDLOW, "LEDLOW;", 15 },
|
|
||||||
{ BOXLLIGHTS, "LLIGHTS;", 16 },
|
|
||||||
{ BOXOSD, "OSD SW;", 19 },
|
|
||||||
{ BOXTELEMETRY, "TELEMETRY;", 20 },
|
|
||||||
{ BOXAUTOTUNE, "AUTO TUNE;", 21 },
|
|
||||||
{ BOXBLACKBOX, "BLACKBOX;", 26 },
|
|
||||||
{ BOXFAILSAFE, "FAILSAFE;", 27 },
|
|
||||||
{ BOXNAVWP, "NAV WP;", 28 },
|
|
||||||
{ BOXAIRMODE, "AIR MODE;", 29 },
|
|
||||||
{ BOXHOMERESET, "HOME RESET;", 30 },
|
|
||||||
{ BOXGCSNAV, "GCS NAV;", 31 },
|
|
||||||
//{ BOXHEADINGLOCK, "HEADING LOCK;", 32 },
|
|
||||||
{ BOXSURFACE, "SURFACE;", 33 },
|
|
||||||
{ BOXFLAPERON, "FLAPERON;", 34 },
|
|
||||||
{ BOXTURNASSIST, "TURN ASSIST;", 35 },
|
|
||||||
{ BOXNAVLAUNCH, "NAV LAUNCH;", 36 },
|
|
||||||
{ BOXAUTOTRIM, "SERVO AUTOTRIM;", 37 },
|
|
||||||
{ BOXKILLSWITCH, "KILLSWITCH;", 38 },
|
|
||||||
{ BOXCAMERA1, "CAMERA CONTROL 1;", 39 },
|
|
||||||
{ BOXCAMERA2, "CAMERA CONTROL 2;", 40 },
|
|
||||||
{ BOXCAMERA3, "CAMERA CONTROL 3;", 41 },
|
|
||||||
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
|
|
||||||
};
|
|
||||||
|
|
||||||
// this is calculated at startup based on enabled features.
|
|
||||||
static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
|
|
||||||
// this is the number of filled indexes in above array
|
|
||||||
static uint8_t activeBoxIdCount = 0;
|
|
||||||
// from mixer.c
|
// from mixer.c
|
||||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||||
|
|
||||||
|
|
@ -238,224 +191,6 @@ static void mspRebootFn(serialPort_t *serialPort)
|
||||||
while (true) ;
|
while (true) ;
|
||||||
}
|
}
|
||||||
|
|
||||||
static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
|
|
||||||
{
|
|
||||||
for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
|
|
||||||
const box_t *candidate = &boxes[boxIndex];
|
|
||||||
if (candidate->boxId == activeBoxId) {
|
|
||||||
return candidate;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return NULL;
|
|
||||||
}
|
|
||||||
|
|
||||||
static const box_t *findBoxByPermenantId(uint8_t permenantId)
|
|
||||||
{
|
|
||||||
for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
|
|
||||||
const box_t *candidate = &boxes[boxIndex];
|
|
||||||
if (candidate->permanentId == permenantId) {
|
|
||||||
return candidate;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return NULL;
|
|
||||||
}
|
|
||||||
|
|
||||||
static bool serializeBoxNamesReply(sbuf_t *dst)
|
|
||||||
{
|
|
||||||
// First run of the loop - calculate total length of the reply
|
|
||||||
int replyLengthTotal = 0;
|
|
||||||
for (int i = 0; i < activeBoxIdCount; i++) {
|
|
||||||
const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
|
|
||||||
if (box) {
|
|
||||||
replyLengthTotal += strlen(box->boxName);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check if we have enough space to send a reply
|
|
||||||
if (sbufBytesRemaining(dst) < replyLengthTotal) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (int i = 0; i < activeBoxIdCount; i++) {
|
|
||||||
const int activeBoxId = activeBoxIds[i];
|
|
||||||
const box_t *box = findBoxByActiveBoxId(activeBoxId);
|
|
||||||
if (box) {
|
|
||||||
const int len = strlen(box->boxName);
|
|
||||||
sbufWriteData(dst, box->boxName, len);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void initActiveBoxIds(void)
|
|
||||||
{
|
|
||||||
// calculate used boxes based on features and fill availableBoxes[] array
|
|
||||||
memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
|
|
||||||
|
|
||||||
activeBoxIdCount = 0;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXARM;
|
|
||||||
|
|
||||||
if (sensors(SENSOR_ACC)) {
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXANGLE;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
|
|
||||||
|
|
||||||
#ifdef USE_FLM_TURN_ASSIST
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXTURNASSIST;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!feature(FEATURE_AIRMODE)) {
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
|
|
||||||
}
|
|
||||||
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXHEADINGHOLD;
|
|
||||||
|
|
||||||
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (feature(FEATURE_SERVO_TILT))
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
|
|
||||||
|
|
||||||
#ifdef GPS
|
|
||||||
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) {
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
|
|
||||||
}
|
|
||||||
if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (STATE(FIXED_WING)) {
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
|
|
||||||
#if defined(AUTOTUNE_FIXED_WING)
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
/*
|
|
||||||
* FLAPERON mode active only in case of airplane and custom airplane. Activating on
|
|
||||||
* flying wing can cause bad thing
|
|
||||||
*/
|
|
||||||
if (STATE(FLAPERON_AVAILABLE)) {
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXFLAPERON;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
|
|
||||||
|
|
||||||
#ifdef LED_STRIP
|
|
||||||
if (feature(FEATURE_LED_STRIP)) {
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXLEDLOW;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXOSD;
|
|
||||||
|
|
||||||
#ifdef TELEMETRY
|
|
||||||
if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch)
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef BLACKBOX
|
|
||||||
if (feature(FEATURE_BLACKBOX)){
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXKILLSWITCH;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
|
|
||||||
|
|
||||||
#ifdef USE_RCSPLIT
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXCAMERA1;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXCAMERA2;
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXCAMERA3;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
#define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
|
|
||||||
#define CHECK_ACTIVE_BOX(condition, index) do { if (IS_ENABLED(condition)) { activeBoxes[index] = 1; } } while(0)
|
|
||||||
|
|
||||||
static void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
|
|
||||||
{
|
|
||||||
uint8_t activeBoxes[CHECKBOX_ITEM_COUNT];
|
|
||||||
memset(activeBoxes, 0, sizeof(activeBoxes));
|
|
||||||
|
|
||||||
// Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
|
|
||||||
// Requires new Multiwii protocol version to fix
|
|
||||||
// It would be preferable to setting the enabled bits based on BOXINDEX.
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)), BOXANGLE);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)), BOXHORIZON);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADING_MODE)), BOXHEADINGHOLD);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)), BOXHEADFREE);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)), BOXHEADADJ);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)), BOXCAMSTAB);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)), BOXPASSTHRU);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)), BOXLLIGHTS);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)), BOXOSD);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)), BOXTELEMETRY);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(ARMING_FLAG(ARMED)), BOXARM);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)), BOXBLACKBOX);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)), BOXSURFACE);
|
|
||||||
#ifdef USE_FLM_FLAPERON
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)), BOXFLAPERON);
|
|
||||||
#endif
|
|
||||||
#ifdef USE_FLM_TURN_ASSIST
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)), BOXTURNASSIST);
|
|
||||||
#endif
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)), BOXNAVLAUNCH);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(AUTO_TUNE)), BOXAUTOTUNE);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTRIM)), BOXAUTOTRIM);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKILLSWITCH)), BOXKILLSWITCH);
|
|
||||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)), BOXHOMERESET);
|
|
||||||
|
|
||||||
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
|
|
||||||
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
|
|
||||||
if (activeBoxes[activeBoxIds[i]]) {
|
|
||||||
bitArraySet(mspBoxModeFlags->bits, i);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static uint16_t packSensorStatus(void)
|
|
||||||
{
|
|
||||||
// Sensor bits
|
|
||||||
uint16_t sensorStatus =
|
|
||||||
IS_ENABLED(sensors(SENSOR_ACC)) << 0 |
|
|
||||||
IS_ENABLED(sensors(SENSOR_BARO)) << 1 |
|
|
||||||
IS_ENABLED(sensors(SENSOR_MAG)) << 2 |
|
|
||||||
IS_ENABLED(sensors(SENSOR_GPS)) << 3 |
|
|
||||||
IS_ENABLED(sensors(SENSOR_RANGEFINDER)) << 4 |
|
|
||||||
//IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 |
|
|
||||||
IS_ENABLED(sensors(SENSOR_PITOT)) << 6;
|
|
||||||
|
|
||||||
// Hardware failure indication bit
|
|
||||||
if (!isHardwareHealthy()) {
|
|
||||||
sensorStatus |= 1 << 15; // Bit 15 of sensor bit field indicates hardware failure
|
|
||||||
}
|
|
||||||
|
|
||||||
return sensorStatus;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void serializeSDCardSummaryReply(sbuf_t *dst)
|
static void serializeSDCardSummaryReply(sbuf_t *dst)
|
||||||
{
|
{
|
||||||
#ifdef USE_SDCARD
|
#ifdef USE_SDCARD
|
||||||
|
|
@ -807,13 +542,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_BOXIDS:
|
case MSP_BOXIDS:
|
||||||
for (int i = 0; i < activeBoxIdCount; i++) {
|
serializeBoxReply(dst);
|
||||||
const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
|
|
||||||
if (!box) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
sbufWriteU8(dst, box->permanentId);
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_MISC:
|
case MSP_MISC:
|
||||||
|
|
|
||||||
308
src/main/fc/fc_msp_box.c
Normal file
308
src/main/fc/fc_msp_box.c
Normal file
|
|
@ -0,0 +1,308 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Cleanflight.
|
||||||
|
*
|
||||||
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
|
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "common/streambuf.h"
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "config/feature.h"
|
||||||
|
|
||||||
|
#include "fc/config.h"
|
||||||
|
#include "fc/fc_msp_box.h"
|
||||||
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
|
#include "sensors/diagnostics.h"
|
||||||
|
#include "sensors/sensors.h"
|
||||||
|
|
||||||
|
#include "telemetry/telemetry.h"
|
||||||
|
|
||||||
|
// FIXME remove ;'s
|
||||||
|
static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
||||||
|
{ BOXARM, "ARM;", 0 },
|
||||||
|
{ BOXANGLE, "ANGLE;", 1 },
|
||||||
|
{ BOXHORIZON, "HORIZON;", 2 },
|
||||||
|
{ BOXNAVALTHOLD, "NAV ALTHOLD;", 3 }, // old BARO
|
||||||
|
{ BOXHEADINGHOLD, "HEADING HOLD;", 5 },
|
||||||
|
{ BOXHEADFREE, "HEADFREE;", 6 },
|
||||||
|
{ BOXHEADADJ, "HEADADJ;", 7 },
|
||||||
|
{ BOXCAMSTAB, "CAMSTAB;", 8 },
|
||||||
|
{ BOXNAVRTH, "NAV RTH;", 10 }, // old GPS HOME
|
||||||
|
{ BOXNAVPOSHOLD, "NAV POSHOLD;", 11 }, // old GPS HOLD
|
||||||
|
{ BOXPASSTHRU, "PASSTHRU;", 12 },
|
||||||
|
{ BOXBEEPERON, "BEEPER;", 13 },
|
||||||
|
{ BOXLEDLOW, "LEDLOW;", 15 },
|
||||||
|
{ BOXLLIGHTS, "LLIGHTS;", 16 },
|
||||||
|
{ BOXOSD, "OSD SW;", 19 },
|
||||||
|
{ BOXTELEMETRY, "TELEMETRY;", 20 },
|
||||||
|
{ BOXAUTOTUNE, "AUTO TUNE;", 21 },
|
||||||
|
{ BOXBLACKBOX, "BLACKBOX;", 26 },
|
||||||
|
{ BOXFAILSAFE, "FAILSAFE;", 27 },
|
||||||
|
{ BOXNAVWP, "NAV WP;", 28 },
|
||||||
|
{ BOXAIRMODE, "AIR MODE;", 29 },
|
||||||
|
{ BOXHOMERESET, "HOME RESET;", 30 },
|
||||||
|
{ BOXGCSNAV, "GCS NAV;", 31 },
|
||||||
|
//{ BOXHEADINGLOCK, "HEADING LOCK;", 32 },
|
||||||
|
{ BOXSURFACE, "SURFACE;", 33 },
|
||||||
|
{ BOXFLAPERON, "FLAPERON;", 34 },
|
||||||
|
{ BOXTURNASSIST, "TURN ASSIST;", 35 },
|
||||||
|
{ BOXNAVLAUNCH, "NAV LAUNCH;", 36 },
|
||||||
|
{ BOXAUTOTRIM, "SERVO AUTOTRIM;", 37 },
|
||||||
|
{ BOXKILLSWITCH, "KILLSWITCH;", 38 },
|
||||||
|
{ BOXCAMERA1, "CAMERA CONTROL 1;", 39 },
|
||||||
|
{ BOXCAMERA2, "CAMERA CONTROL 2;", 40 },
|
||||||
|
{ BOXCAMERA3, "CAMERA CONTROL 3;", 41 },
|
||||||
|
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
|
||||||
|
};
|
||||||
|
|
||||||
|
// this is calculated at startup based on enabled features.
|
||||||
|
static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
|
||||||
|
// this is the number of filled indexes in above array
|
||||||
|
uint8_t activeBoxIdCount = 0;
|
||||||
|
|
||||||
|
const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
|
||||||
|
{
|
||||||
|
for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
|
||||||
|
const box_t *candidate = &boxes[boxIndex];
|
||||||
|
if (candidate->boxId == activeBoxId) {
|
||||||
|
return candidate;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
const box_t *findBoxByPermenantId(uint8_t permenantId)
|
||||||
|
{
|
||||||
|
for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
|
||||||
|
const box_t *candidate = &boxes[boxIndex];
|
||||||
|
if (candidate->permanentId == permenantId) {
|
||||||
|
return candidate;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool serializeBoxNamesReply(sbuf_t *dst)
|
||||||
|
{
|
||||||
|
// First run of the loop - calculate total length of the reply
|
||||||
|
int replyLengthTotal = 0;
|
||||||
|
for (int i = 0; i < activeBoxIdCount; i++) {
|
||||||
|
const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
|
||||||
|
if (box) {
|
||||||
|
replyLengthTotal += strlen(box->boxName);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check if we have enough space to send a reply
|
||||||
|
if (sbufBytesRemaining(dst) < replyLengthTotal) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; i < activeBoxIdCount; i++) {
|
||||||
|
const int activeBoxId = activeBoxIds[i];
|
||||||
|
const box_t *box = findBoxByActiveBoxId(activeBoxId);
|
||||||
|
if (box) {
|
||||||
|
const int len = strlen(box->boxName);
|
||||||
|
sbufWriteData(dst, box->boxName, len);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void serializeBoxReply(sbuf_t *dst)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < activeBoxIdCount; i++) {
|
||||||
|
const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]);
|
||||||
|
if (!box) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
sbufWriteU8(dst, box->permanentId);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void initActiveBoxIds(void)
|
||||||
|
{
|
||||||
|
// calculate used boxes based on features and fill availableBoxes[] array
|
||||||
|
memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
|
||||||
|
|
||||||
|
activeBoxIdCount = 0;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXARM;
|
||||||
|
|
||||||
|
if (sensors(SENSOR_ACC)) {
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXANGLE;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
|
||||||
|
|
||||||
|
#ifdef USE_FLM_TURN_ASSIST
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXTURNASSIST;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!feature(FEATURE_AIRMODE)) {
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
|
||||||
|
}
|
||||||
|
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXHEADINGHOLD;
|
||||||
|
|
||||||
|
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (feature(FEATURE_SERVO_TILT))
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
|
||||||
|
|
||||||
|
#ifdef GPS
|
||||||
|
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) {
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
|
||||||
|
}
|
||||||
|
if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
if (STATE(FIXED_WING)) {
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
|
||||||
|
#if defined(AUTOTUNE_FIXED_WING)
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef USE_SERVOS
|
||||||
|
/*
|
||||||
|
* FLAPERON mode active only in case of airplane and custom airplane. Activating on
|
||||||
|
* flying wing can cause bad thing
|
||||||
|
*/
|
||||||
|
if (STATE(FLAPERON_AVAILABLE)) {
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXFLAPERON;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
|
||||||
|
|
||||||
|
#ifdef LED_STRIP
|
||||||
|
if (feature(FEATURE_LED_STRIP)) {
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXLEDLOW;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXOSD;
|
||||||
|
|
||||||
|
#ifdef TELEMETRY
|
||||||
|
if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch)
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef BLACKBOX
|
||||||
|
if (feature(FEATURE_BLACKBOX)){
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXKILLSWITCH;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
|
||||||
|
|
||||||
|
#ifdef USE_RCSPLIT
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXCAMERA1;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXCAMERA2;
|
||||||
|
activeBoxIds[activeBoxIdCount++] = BOXCAMERA3;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
#define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
|
||||||
|
#define CHECK_ACTIVE_BOX(condition, index) do { if (IS_ENABLED(condition)) { activeBoxes[index] = 1; } } while(0)
|
||||||
|
|
||||||
|
void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
|
||||||
|
{
|
||||||
|
uint8_t activeBoxes[CHECKBOX_ITEM_COUNT];
|
||||||
|
memset(activeBoxes, 0, sizeof(activeBoxes));
|
||||||
|
|
||||||
|
// Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
|
||||||
|
// Requires new Multiwii protocol version to fix
|
||||||
|
// It would be preferable to setting the enabled bits based on BOXINDEX.
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)), BOXANGLE);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)), BOXHORIZON);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADING_MODE)), BOXHEADINGHOLD);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)), BOXHEADFREE);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)), BOXHEADADJ);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)), BOXCAMSTAB);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)), BOXPASSTHRU);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)), BOXLLIGHTS);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)), BOXOSD);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)), BOXTELEMETRY);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(ARMING_FLAG(ARMED)), BOXARM);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)), BOXBLACKBOX);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)), BOXSURFACE);
|
||||||
|
#ifdef USE_FLM_FLAPERON
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)), BOXFLAPERON);
|
||||||
|
#endif
|
||||||
|
#ifdef USE_FLM_TURN_ASSIST
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)), BOXTURNASSIST);
|
||||||
|
#endif
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)), BOXNAVLAUNCH);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(AUTO_TUNE)), BOXAUTOTUNE);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTRIM)), BOXAUTOTRIM);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKILLSWITCH)), BOXKILLSWITCH);
|
||||||
|
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)), BOXHOMERESET);
|
||||||
|
|
||||||
|
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
|
||||||
|
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
|
||||||
|
if (activeBoxes[activeBoxIds[i]]) {
|
||||||
|
bitArraySet(mspBoxModeFlags->bits, i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t packSensorStatus(void)
|
||||||
|
{
|
||||||
|
// Sensor bits
|
||||||
|
uint16_t sensorStatus =
|
||||||
|
IS_ENABLED(sensors(SENSOR_ACC)) << 0 |
|
||||||
|
IS_ENABLED(sensors(SENSOR_BARO)) << 1 |
|
||||||
|
IS_ENABLED(sensors(SENSOR_MAG)) << 2 |
|
||||||
|
IS_ENABLED(sensors(SENSOR_GPS)) << 3 |
|
||||||
|
IS_ENABLED(sensors(SENSOR_RANGEFINDER)) << 4 |
|
||||||
|
//IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 |
|
||||||
|
IS_ENABLED(sensors(SENSOR_PITOT)) << 6;
|
||||||
|
|
||||||
|
// Hardware failure indication bit
|
||||||
|
if (!isHardwareHealthy()) {
|
||||||
|
sensorStatus |= 1 << 15; // Bit 15 of sensor bit field indicates hardware failure
|
||||||
|
}
|
||||||
|
|
||||||
|
return sensorStatus;
|
||||||
|
}
|
||||||
37
src/main/fc/fc_msp_box.h
Normal file
37
src/main/fc/fc_msp_box.h
Normal file
|
|
@ -0,0 +1,37 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Cleanflight.
|
||||||
|
*
|
||||||
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
|
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "fc/rc_modes.h"
|
||||||
|
|
||||||
|
typedef struct box_s {
|
||||||
|
const uint8_t boxId; // see boxId_e
|
||||||
|
const char *boxName; // GUI-readable box name
|
||||||
|
const uint8_t permanentId; // permanent ID used to identify BOX. This ID is unique for one function, DO NOT REUSE IT
|
||||||
|
} box_t;
|
||||||
|
|
||||||
|
const box_t *findBoxByActiveBoxId(uint8_t activeBoxId);
|
||||||
|
const box_t *findBoxByPermanentId(uint8_t permanentId);
|
||||||
|
|
||||||
|
struct boxBitmask_s;
|
||||||
|
void packBoxModeFlags(struct boxBitmask_s * mspBoxModeFlags);
|
||||||
|
uint16_t packSensorStatus(void);
|
||||||
|
struct sbuf_s;
|
||||||
|
bool serializeBoxNamesReply(struct sbuf_s *dst);
|
||||||
|
void serializeBoxReply(struct sbuf_s *dst);
|
||||||
|
void initActiveBoxIds(void);
|
||||||
|
|
@ -25,6 +25,8 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
|
#include "io/serial.h"
|
||||||
|
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
FRSKY_FORMAT_DMS = 0,
|
FRSKY_FORMAT_DMS = 0,
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue