Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +03:00
Code Issues Wiki Activity

Merge pull request #1042 from martinbudden/inav_msp_tidy

Browse source 
Tidied fc_msp.c to be more aligned with betaflight
This commit is contained in:
Martin Budden 2017-01-05 23:18:01 +01:00 • committed by GitHub
commit 8d15d3a299
2 changed files with 93 additions and 94 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/config/config_master.h
View file

@ -65,6 +65,7 @@
#define boardAlignment(x) (&masterConfig.boardAlignment) #define boardAlignment(x) (&masterConfig.boardAlignment)
#define imuConfig(x) (&masterConfig.imuConfig) #define imuConfig(x) (&masterConfig.imuConfig)
#define batteryConfig(x) (&masterConfig.batteryConfig) #define batteryConfig(x) (&masterConfig.batteryConfig)
#define rcControlsConfig(x) (&masterConfig.rcControlsConfig)
#define gpsConfig(x) (&masterConfig.gpsConfig) #define gpsConfig(x) (&masterConfig.gpsConfig)
#define navConfig(x) (&masterConfig.navConfig) #define navConfig(x) (&masterConfig.navConfig)
#define armingConfig(x) (&masterConfig.armingConfig) #define armingConfig(x) (&masterConfig.armingConfig)

146
src/main/fc/fc_msp.c
View file

@ -33,28 +33,43 @@
#include "common/streambuf.h" #include "common/streambuf.h"
#include "common/utils.h" #include "common/utils.h"
#include "drivers/system.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/pwm_mapping.h"
#include "drivers/serial.h"
#include "drivers/bus_i2c.h" #include "drivers/bus_i2c.h"
#include "drivers/sdcard.h" #include "drivers/compass.h"
#include "drivers/max7456.h" #include "drivers/max7456.h"
#include "drivers/pwm_mapping.h"
#include "drivers/sdcard.h"
#include "drivers/serial.h"
#include "drivers/system.h"
#include "fc/config.h"
#include "fc/fc_msp.h" #include "fc/fc_msp.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "io/motors.h" #include "flight/failsafe.h"
#include "io/servos.h" #include "flight/imu.h"
#include "flight/hil.h"
#include "flight/mixer.h"
#include "flight/navigation_rewrite.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/serial.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"
#include "io/flashfs.h" #include "io/motors.h"
#include "io/asyncfatfs/asyncfatfs.h" #include "io/serial.h"
#include "io/serial_4way.h" #include "io/serial_4way.h"
#include "io/servos.h"
#include "msp/msp.h" #include "msp/msp.h"
#include "msp/msp_protocol.h" #include "msp/msp_protocol.h"
@ -78,21 +93,6 @@
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "fc/config.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/hil.h"
#include "flight/failsafe.h"
#include "flight/navigation_rewrite.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef NAZE #ifdef NAZE
#include "hardware_revision.h" #include "hardware_revision.h"
#endif #endif
@ -171,12 +171,12 @@ typedef enum {
} mspSDCardState_e; } mspSDCardState_e;
typedef enum { typedef enum {
MSP_SDCARD_FLAG_SUPPORTTED = 1, MSP_SDCARD_FLAG_SUPPORTTED = 1
} mspSDCardFlags_e; } mspSDCardFlags_e;
typedef enum { typedef enum {
MSP_FLASHFS_BIT_READY = 1, MSP_FLASHFS_BIT_READY = 1,
MSP_FLASHFS_BIT_SUPPORTED = 2, MSP_FLASHFS_BIT_SUPPORTED = 2
} mspFlashfsFlags_e; } mspFlashfsFlags_e;
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
@ -208,10 +208,8 @@ static void mspRebootFn(serialPort_t *serialPort)
static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
{ {
uint8_t boxIndex; for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
const box_t *candidate; const box_t *candidate = &boxes[boxIndex];
for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
candidate = &boxes[boxIndex];
if (candidate->boxId == activeBoxId) { if (candidate->boxId == activeBoxId) {
return candidate; return candidate;
} }
@ -221,10 +219,8 @@ static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
static const box_t *findBoxByPermenantId(uint8_t permenantId) static const box_t *findBoxByPermenantId(uint8_t permenantId)
{ {
uint8_t boxIndex; for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
const box_t *candidate; const box_t *candidate = &boxes[boxIndex];
for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
candidate = &boxes[boxIndex];
if (candidate->permanentId == permenantId) { if (candidate->permanentId == permenantId) {
return candidate; return candidate;
} }
@ -501,7 +497,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
switch (cmdMSP) { switch (cmdMSP) {
case MSP_API_VERSION: case MSP_API_VERSION:
sbufWriteU8(dst, MSP_PROTOCOL_VERSION); sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
sbufWriteU8(dst, API_VERSION_MAJOR); sbufWriteU8(dst, API_VERSION_MAJOR);
sbufWriteU8(dst, API_VERSION_MINOR); sbufWriteU8(dst, API_VERSION_MINOR);
break; break;
@ -620,12 +615,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_SERVO_MIX_RULES: case MSP_SERVO_MIX_RULES:
for (int i = 0; i < MAX_SERVO_RULES; i++) { for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, masterConfig.customServoMixer[i].targetChannel); sbufWriteU8(dst, customServoMixer(i)->targetChannel);
sbufWriteU8(dst, masterConfig.customServoMixer[i].inputSource); sbufWriteU8(dst, customServoMixer(i)->inputSource);
sbufWriteU8(dst, masterConfig.customServoMixer[i].rate); sbufWriteU8(dst, customServoMixer(i)->rate);
sbufWriteU8(dst, masterConfig.customServoMixer[i].speed); sbufWriteU8(dst, customServoMixer(i)->speed);
sbufWriteU8(dst, masterConfig.customServoMixer[i].min); sbufWriteU8(dst, customServoMixer(i)->min);
sbufWriteU8(dst, masterConfig.customServoMixer[i].max); sbufWriteU8(dst, customServoMixer(i)->max);
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
} }
break; break;
@ -786,8 +781,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_MOTOR_PINS: case MSP_MOTOR_PINS:
// FIXME This is hardcoded and should not be. // FIXME This is hardcoded and should not be.
for (int i = 0; i < 8; i++) for (int i = 0; i < 8; i++) {
sbufWriteU8(dst, i + 1); sbufWriteU8(dst, i + 1);
}
break; break;
#ifdef GPS #ifdef GPS
@ -1010,22 +1006,25 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_OSD_CONFIG: case MSP_OSD_CONFIG:
#ifdef OSD #ifdef OSD
sbufWriteU8(dst, 1); // OSD supported sbufWriteU8(dst, 1); // OSD supported
// send video system (AUTO/PAL/NTSC) // send video system (AUTO/PAL/NTSC)
#ifdef USE_MAX7456
sbufWriteU8(dst, osdProfile()->video_system); sbufWriteU8(dst, osdProfile()->video_system);
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, osdProfile()->units); sbufWriteU8(dst, osdProfile()->units);
sbufWriteU8(dst, osdProfile()->rssi_alarm); sbufWriteU8(dst, osdProfile()->rssi_alarm);
sbufWriteU16(dst, osdProfile()->cap_alarm); sbufWriteU16(dst, osdProfile()->cap_alarm);
sbufWriteU16(dst, osdProfile()->time_alarm); sbufWriteU16(dst, osdProfile()->time_alarm);
sbufWriteU16(dst, osdProfile()->alt_alarm); sbufWriteU16(dst, osdProfile()->alt_alarm);
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
for (uint8_t i = 0; i < OSD_ITEM_COUNT; i++) {
sbufWriteU16(dst, osdProfile()->item_pos[i]); sbufWriteU16(dst, osdProfile()->item_pos[i]);
} }
#else #else
sbufWriteU8(dst, 0); // OSD not supported sbufWriteU8(dst, 0); // OSD not supported
#endif #endif
break; break;
case MSP_BF_BUILD_INFO: case MSP_BF_BUILD_INFO:
@ -1041,9 +1040,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_RC_DEADBAND: case MSP_RC_DEADBAND:
sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband); sbufWriteU8(dst, rcControlsConfig()->deadband);
sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband); sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband); sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle); sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
break; break;
@ -1205,11 +1204,10 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
} }
#ifdef NAV #ifdef NAV
static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src) static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
{ {
navWaypoint_t msp_wp;
const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number
navWaypoint_t msp_wp;
getWaypoint(msp_wp_no, &msp_wp); getWaypoint(msp_wp_no, &msp_wp);
sbufWriteU8(dst, msp_wp_no); // wp_no sbufWriteU8(dst, msp_wp_no); // wp_no
sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT) sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT)
@ -1238,10 +1236,6 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
uint8_t rate; uint8_t rate;
const unsigned int dataSize = sbufBytesRemaining(src); const unsigned int dataSize = sbufBytesRemaining(src);
#ifdef NAV
uint8_t msp_wp_no;
navWaypoint_t msp_wp;
#endif
switch (cmdMSP) { switch (cmdMSP) {
#ifdef HIL #ifdef HIL
@ -1431,7 +1425,6 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef USE_SERVOS #ifdef USE_SERVOS
if (dataSize != 1 + sizeof(servoParam_t)) { if (dataSize != 1 + sizeof(servoParam_t)) {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break;
} }
i = sbufReadU8(src); i = sbufReadU8(src);
if (i >= MAX_SUPPORTED_SERVOS) { if (i >= MAX_SUPPORTED_SERVOS) {
@ -1455,12 +1448,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= MAX_SERVO_RULES) { if (i >= MAX_SERVO_RULES) {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} else { } else {
masterConfig.customServoMixer[i].targetChannel = sbufReadU8(src); customServoMixer(i)->targetChannel = sbufReadU8(src);
masterConfig.customServoMixer[i].inputSource = sbufReadU8(src); customServoMixer(i)->inputSource = sbufReadU8(src);
masterConfig.customServoMixer[i].rate = sbufReadU8(src); customServoMixer(i)->rate = sbufReadU8(src);
masterConfig.customServoMixer[i].speed = sbufReadU8(src); customServoMixer(i)->speed = sbufReadU8(src);
masterConfig.customServoMixer[i].min = sbufReadU8(src); customServoMixer(i)->min = sbufReadU8(src);
masterConfig.customServoMixer[i].max = sbufReadU8(src); customServoMixer(i)->max = sbufReadU8(src);
sbufReadU8(src); //Read 1 byte for `box` and ignore it sbufReadU8(src); //Read 1 byte for `box` and ignore it
loadCustomServoMixer(); loadCustomServoMixer();
} }
@ -1475,9 +1468,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break; break;
case MSP_SET_RC_DEADBAND: case MSP_SET_RC_DEADBAND:
masterConfig.rcControlsConfig.deadband = sbufReadU8(src); rcControlsConfig()->deadband = sbufReadU8(src);
masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src); rcControlsConfig()->yaw_deadband = sbufReadU8(src);
masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src); rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
break; break;
case MSP_SET_RESET_CURR_PID: case MSP_SET_RESET_CURR_PID:
@ -1534,7 +1527,6 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break; break;
case MSP_SET_PID_ADVANCED: case MSP_SET_PID_ADVANCED:
currentProfile->pidProfile.rollPitchItermIgnoreRate = sbufReadU16(src); currentProfile->pidProfile.rollPitchItermIgnoreRate = sbufReadU16(src);
currentProfile->pidProfile.yawItermIgnoreRate = sbufReadU16(src); currentProfile->pidProfile.yawItermIgnoreRate = sbufReadU16(src);
currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src); currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src);
@ -1643,11 +1635,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef BLACKBOX #ifdef BLACKBOX
case MSP_SET_BLACKBOX_CONFIG: case MSP_SET_BLACKBOX_CONFIG:
// Don't allow config to be updated while Blackbox is logging // Don't allow config to be updated while Blackbox is logging
if (!blackboxMayEditConfig()) if (blackboxMayEditConfig()) {
return false;
blackboxConfig()->device = sbufReadU8(src); blackboxConfig()->device = sbufReadU8(src);
blackboxConfig()->rate_num = sbufReadU8(src); blackboxConfig()->rate_num = sbufReadU8(src);
blackboxConfig()->rate_denom = sbufReadU8(src); blackboxConfig()->rate_denom = sbufReadU8(src);
}
break; break;
#endif #endif
@ -1669,7 +1661,10 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
osdProfile()->alt_alarm = sbufReadU16(src); osdProfile()->alt_alarm = sbufReadU16(src);
} else { } else {
// set a position setting // set a position setting
osdProfile()->item_pos[addr] = sbufReadU16(src); const uint16_t pos = sbufReadU16(src);
if (addr < OSD_ITEM_COUNT) {
osdProfile()->item_pos[addr] = pos;
}
} }
} }
break; break;
@ -1727,7 +1722,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#endif #endif
#ifdef NAV #ifdef NAV
case MSP_SET_WP: case MSP_SET_WP:
msp_wp_no = sbufReadU8(src); // get the wp number {
const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number
navWaypoint_t msp_wp;
msp_wp.action = sbufReadU8(src); // action msp_wp.action = sbufReadU8(src); // action
msp_wp.lat = sbufReadU32(src); // lat msp_wp.lat = sbufReadU32(src); // lat
msp_wp.lon = sbufReadU32(src); // lon msp_wp.lon = sbufReadU32(src); // lon
@ -1737,6 +1734,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
msp_wp.p3 = sbufReadU16(src); // P3 msp_wp.p3 = sbufReadU16(src); // P3
msp_wp.flag = sbufReadU8(src); // future: to set nav flag msp_wp.flag = sbufReadU8(src); // future: to set nav flag
setWaypoint(msp_wp_no, &msp_wp); setWaypoint(msp_wp_no, &msp_wp);
}
break; break;
#endif #endif
case MSP_SET_FEATURE: case MSP_SET_FEATURE:
@ -1933,7 +1931,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
ret = MSP_RESULT_ACK; ret = MSP_RESULT_ACK;
#ifdef NAV #ifdef NAV
} else if (cmdMSP == MSP_WP) { } else if (cmdMSP == MSP_WP) {
mspFcWpCommand(dst, src); mspFcWaypointOutCommand(dst, src);
ret = MSP_RESULT_ACK; ret = MSP_RESULT_ACK;
#endif #endif
#ifdef USE_FLASHFS #ifdef USE_FLASHFS