Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

refactoring

Browse source 
implemented beeper muting in simulator mode
implemented debug output in simulator mode
adjusted MSP_SIMULATOR command id
implemented battery emulation in simulator mode
This commit is contained in:
Roman Lut 2022-06-26 02:20:22 +03:00
parent c5d32983aa
commit 1f41db14a5
7 changed files with 88 additions and 48 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/drivers/sound_beeper.c
View file

@ -27,6 +27,7 @@
#include "drivers/pwm_mapping.h"
#include "drivers/pwm_output.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "sound_beeper.h"
@ -45,6 +46,10 @@ void systemBeep(bool onoff)
UNUSED(onoff);
#else
if (simulatorData.flags & SIMU_MUTE_BEEPER) {
onoff = false;
}
if (beeperConfig()->pwmMode) {
pwmWriteBeeper(onoff);
beeperState = onoff;

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

@ -3421,17 +3421,21 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (tmp_u8 != 1) break;
tmp_u8 = sbufReadU8(src);
if (tmp_u8 & 1) {
if ((tmp_u8 & SIMU_ENABLE) == 0) {
DISABLE_ARMING_FLAG(SIMULATOR_MODE);
baroStartCalibration(); // just once
simulatorData.flags = 0;
}
else {
simulatorData.flags = tmp_u8;
if (!ARMING_FLAG(SIMULATOR_MODE)) {
baroStartCalibration(); // just once
ENABLE_ARMING_FLAG(SIMULATOR_MODE);
LOG_D(SYSTEM, "Simulator enabled");
}
} else {
DISABLE_ARMING_FLAG(SIMULATOR_MODE);
baroStartCalibration(); // just once
}
if (dataSize >= 14) {
#ifdef USE_GPS
gpsSol.fixType = sbufReadU8(src);
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSol.flags.hasNewData = true;
@ -3451,9 +3455,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
gpsSol.epv = 100;
// Feed data to navigation
gpsProcessNewSolutionData();
#endif
attitude.values.roll = sbufReadU16(src)-1800;
attitude.values.pitch = sbufReadU16(src)-1800;
attitude.values.roll = sbufReadU16(src) - 1800;
attitude.values.pitch = sbufReadU16(src) - 1800;
attitude.values.yaw = sbufReadU16(src);
acc.accADCf[X] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
@ -3465,7 +3469,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
gyro.gyroADCf[Z] = (float)sbufReadU16(src) / 100 - 320;
baro.baroPressure = sbufReadU32(src);
} else {
}
else {
DISABLE_STATE(GPS_FIX);
}
@ -3474,8 +3479,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
sbufWriteU16(dst, input[INPUT_STABILIZED_YAW] + 500);
sbufWriteU16(dst, input[INPUT_STABILIZED_THROTTLE] + 500);
mspWriteSimulatorOSD(dst);
simulatorData.debugIndex++;
if (simulatorData.debugIndex == 8){
simulatorData.debugIndex = 0;
}
sbufWriteU8(dst, simulatorData.debugIndex);
sbufWriteU32(dst, debug[simulatorData.debugIndex]);
mspWriteSimulatorOSD(dst);
}
*ret = MSP_RESULT_ACK;
break;
default:

2
src/main/fc/runtime_config.c
View file

@ -176,3 +176,5 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
}
simulatorData_t simulatorData = { flags: 0, debugIndex: 0 };

14
src/main/fc/runtime_config.h
View file

@ -165,6 +165,20 @@ typedef enum {
flightModeForTelemetry_e getFlightModeForTelemetry(void);
typedef enum {
SIMU_ENABLE = (1 << 0),
SIMU_SIMULATE_BATTERY = (1 << 1),
SIMU_MUTE_BEEPER = (1 << 2)
} simulatorFlags_t;
typedef struct {
simulatorFlags_t flags;
uint8_t debugIndex;
} simulatorData_t;
extern simulatorData_t simulatorData;
uint32_t enableFlightMode(flightModeFlags_e mask);
uint32_t disableFlightMode(flightModeFlags_e mask);

1
src/main/msp/msp_protocol.h
View file

@ -288,7 +288,6 @@
#define MSP_DEBUGMSG 253 //out message debug string buffer
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
#define MSP_V2_FRAME 255 //MSPv2 payload indicator
#define MSP_SIMULATOR 256 //inout Set simulator state
// Additional commands that are not compatible with MultiWii
#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc

2
src/main/msp/msp_protocol_v2_inav.h
View file

@ -53,6 +53,8 @@
#define MSP2_INAV_SET_TEMP_SENSOR_CONFIG 0x201D
#define MSP2_INAV_TEMPERATURES 0x201E
#define MSP_SIMULATOR 0x201F
#define MSP2_INAV_SERVO_MIXER 0x2020
#define MSP2_INAV_SET_SERVO_MIXER 0x2021
#define MSP2_INAV_LOGIC_CONDITIONS 0x2022

6
src/main/sensors/battery.c
View file

@ -291,6 +291,12 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
break;
}
if (simulatorData.flags & SIMU_SIMULATE_BATTERY) {
vbat = 1260;
batteryFullVoltage = 1210;
batteryWarningVoltage = 1020;
batteryCriticalVoltage = 960;
}
if (justConnected) {
pt1FilterReset(&vbatFilterState, vbat);
} else {