Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Merge branch 'master' into patch_v3.1.6

Browse source
This commit is contained in:
borisbstyle 2017-02-20 14:40:43 +01:00 • committed by GitHub
commit de3d1d527d
197 changed files with 4476 additions and 4008 deletions
Download patch file Download diff file Expand all files Collapse all files

40
Makefile
View file

@ -561,6 +561,7 @@ COMMON_SRC = \
config/config_eeprom.c \
config/feature.c \
config/parameter_group.c \
config/config_streamer.c \
drivers/adc.c \
drivers/buf_writer.c \
drivers/bus_i2c_soft.c \
@ -594,6 +595,7 @@ COMMON_SRC = \
fc/fc_rc.c \
fc/fc_msp.c \
fc/fc_tasks.c \
fc/rc_adjustments.c \
fc/rc_controls.c \
fc/runtime_config.c \
fc/cli.c \
@ -634,10 +636,6 @@ COMMON_SRC = \
sensors/compass.c \
sensors/gyro.c \
sensors/initialisation.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC)
HIGHEND_SRC = \
blackbox/blackbox.c \
blackbox/blackbox_io.c \
cms/cms.c \
@ -649,13 +647,13 @@ HIGHEND_SRC = \
cms/cms_menu_osd.c \
cms/cms_menu_vtx.c \
common/colorconversion.c \
common/gps_conversion.c \
drivers/display_ug2864hsweg01.c \
drivers/light_ws2811strip.c \
drivers/serial_escserial.c \
drivers/sonar_hcsr04.c \
drivers/vtx_common.c \
flight/navigation.c \
flight/gps_conversion.c \
io/dashboard.c \
io/displayport_max7456.c \
io/displayport_msp.c \
@ -677,7 +675,10 @@ HIGHEND_SRC = \
sensors/esc_sensor.c \
io/vtx_string.c \
io/vtx_smartaudio.c \
io/vtx_tramp.c
io/vtx_tramp.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC)
SPEED_OPTIMISED_SRC := ""
SIZE_OPTIMISED_SRC := ""
@ -707,25 +708,20 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
drivers/serial.c \
drivers/serial_uart.c \
drivers/sound_beeper.c \
drivers/stack_check.c \
drivers/system.c \
drivers/timer.c \
fc/fc_core.c \
fc/fc_tasks.c \
fc/fc_rc.c \
fc/rc_controls.c \
fc/runtime_config.c \
flight/altitudehold.c \
flight/failsafe.c \
flight/imu.c \
flight/mixer.c \
flight/pid.c \
flight/servos.c \
io/beeper.c \
io/serial.c \
io/statusindicator.c \
rx/ibus.c \
rx/jetiexbus.c \
rx/msp.c \
rx/nrf24_cx10.c \
rx/nrf24_inav.c \
rx/nrf24_h8_3d.c \
@ -746,25 +742,12 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
sensors/gyro.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC) \
blackbox/blackbox.c \
blackbox/blackbox_io.c \
drivers/display_ug2864hsweg01.c \
drivers/light_ws2811strip.c \
drivers/serial_softserial.c \
io/dashboard.c \
io/displayport_max7456.c \
io/displayport_msp.c \
io/displayport_oled.c \
io/ledstrip.c \
io/osd.c \
telemetry/telemetry.c \
telemetry/crsf.c \
telemetry/frsky.c \
telemetry/hott.c \
telemetry/smartport.c \
telemetry/ltm.c \
telemetry/mavlink.c \
telemetry/esc_telemetry.c \
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/serial_escserial.c \
@ -888,13 +871,8 @@ TARGET_SRC += \
io/flashfs.c
endif
ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS) $(F4_TARGETS) $(F3_TARGETS)))
TARGET_SRC += $(HIGHEND_SRC)
else ifneq ($(filter HIGHEND,$(FEATURES)),)
TARGET_SRC += $(HIGHEND_SRC)
endif
TARGET_SRC += $(COMMON_SRC)
#excludes
ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS)))
TARGET_SRC := $(filter-out ${F7EXCLUDES}, $(TARGET_SRC))

BIN
reference/spektrum/Bi-Directional SRXL.pdf
View file

Binary file not shown.

822
reference/spektrum/spektrumTelemetrySensors.h
View file

@ -1,822 +0,0 @@
//////////////////////////////////////////////////////////////////
//
// Copyright 2013 by Horizon Hobby, Inc.
// Released to Public Domain
//
// This header file may be incorporated into non-Horizon
// products.
//
//////////////////////////////////////////////////////////////////
#ifndef TELEMETRY_H
#define TELEMETRY_H
//////////////////////////////////////////////////////////////////
//
// Assigned I2C Addresses and Device Types
//
//////////////////////////////////////////////////////////////////
#define TELE_DEVICE_NODATA (0x00) // No data in packet
#define TELE_DEVICE_VOLTAGE (0x01) // High-Voltage sensor (INTERNAL)
#define TELE_DEVICE_TEMPERATURE (0x02) // Temperature Sensor (INTERNAL)
#define TELE_DEVICE_RSV_03 (0x03) // Reserved
#define TELE_DEVICE_RSV_04 (0x04) // Reserved
#define TELE_DEVICE_RSV_05 (0x05) // Reserved
#define TELE_DEVICE_RSV_06 (0x06) // Reserved
#define TELE_DEVICE_RSV_07 (0x07) // Reserved
#define TELE_DEVICE_RSV_08 (0x08) // Reserved
#define TELE_DEVICE_RSV_09 (0x09) // Reserved
#define TELE_DEVICE_PBOX (0x0A) // PowerBox
#define TELE_DEVICE_LAPTIMER (0x0B) // Lap Timer
#define TELE_DEVICE_TEXTGEN (0x0C) // Text Generator
#define TELE_DEVICE_AIRSPEED (0x11) // Air Speed
#define TELE_DEVICE_ALTITUDE (0x12) // Altitude
#define TELE_DEVICE_GMETER (0x14) // GForce
#define TELE_DEVICE_JETCAT (0x15) // JetCat interface
#define TELE_DEVICE_GPS_LOC (0x16) // GPS Location Data
#define TELE_DEVICE_GPS_STATS (0x17) // GPS Status
#define TELE_DEVICE_RX_MAH (0x18) // Receiver Pack Capacity (Dual)
#define TELE_DEVICE_JETCAT_2 (0x19) // JetCat interface, msg 2
#define TELE_DEVICE_GYRO (0x1A) // 3-axis gyro
#define TELE_DEVICE_ATTMAG (0x1B) // Attitude and Magnetic Compass
#define TELE_DEVICE_AS3X_LEGACYGAIN (0x1F) // Active AS3X Gains for legacy mode
#define TELE_DEVICE_ESC (0x20) // ESC
#define TELE_DEVICE_FUEL (0x22) // Fuel Flow Meter
#define TELE_DEVICE_ALPHA6 (0x24) // Alpha6 Stabilizer
// DO NOT USE (0x30) // Reserved for internal use
// DO NOT USE (0x32) // Reserved for internal use
#define TELE_DEVICE_MAH (0x34) // Battery Gauge (mAh) (Dual)
#define TELE_DEVICE_DIGITAL_AIR (0x36) // Digital Inputs & Tank Pressure
#define TELE_DEVICE_STRAIN (0x38) // Thrust/Strain Gauge
#define TELE_DEVICE_LIPOMON (0x3A) // 6S Cell Monitor (LiPo taps)
#define TELE_DEVICE_LIPOMON_14 (0x3F) // 14S Cell Monitor (LiPo taps)
#define TELE_DEVICE_VARIO_S (0x40) // Vario
#define TELE_DEVICE_RSV_43 (0x43) // Reserved
#define TELE_DEVICE_USER_16SU (0x50) // User-Def, STRU_TELE_USER_16SU
#define TELE_DEVICE_USER_16SU32U (0x52) // User-Def, STRU_TELE_USER_16SU32U
#define TELE_DEVICE_USER_16SU32S (0x54) // User-Def, STRU_TELE_USER_16SU32S
#define TELE_DEVICE_USER_16U32SU (0x56) // User-Def, STRU_TELE_USER_16U32SU
#define TELE_DEVICE_RSV_60 (0x60) // Reserved
#define TELE_DEVICE_RSV_68 (0x68) // Reserved
#define TELE_DEVICE_RSV_69 (0x69) // Reserved
#define TELE_DEVICE_RSV_6A (0x6A) // Reserved
#define TELE_DEVICE_RSV_6B (0x6B) // Reserved
#define TELE_DEVICE_RSV_6C (0x6C) // Reserved
#define TELE_DEVICE_RSV_6D (0x6D) // Reserved
#define TELE_DEVICE_RSV_6E (0x6E) // Reserved
#define TELE_DEVICE_RSV_6F (0x6F) // Reserved
#define TELE_DEVICE_RSV_70 (0x70) // Reserved
#define TELE_DEVICE_RTC (0x7C) // Pseudo-device giving timestamp
#define TELE_DEVICE_FRAMEDATA (0x7D) // Transmitter frame data
#define TELE_DEVICE_RPM (0x7E) // RPM sensor
#define TELE_DEVICE_QOS (0x7F) // RxV + flight log data
#define TELE_DEVICE_MAX (0x7F) // Last address available
#define TELE_DEVICE_SHORTRANGE (0x80) // Data is from a TM1100
//////////////////////////////////////////////////////////////////
//
// Message Data Structures
//
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
//
// Electronic Speed Control
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x20
UINT8 sID; // Secondary ID
UINT16 RPM; // RPM, 10RPM (0-655340 RPM). 0xFFFF --> "No data"
UINT16 voltsInput; // Volts, 0.01v (0-655.34V). 0xFFFF --> "No data"
UINT16 tempFET; // Temperature, 0.1C (0-999.8C) 0xFFFF --> "No data"
UINT16 currentMotor; // Current, 10mA (0-655.34A). 0xFFFF --> "No data"
UINT16 tempBEC; // Temperature, 0.1C (0-999.8C) 0x7FFF --> "No data"
UINT8 currentBEC; // BEC Current, 100mA (0-25.4A). 0xFF ----> "No data"
UINT8 voltsBEC; // BEC Volts, 0.05V (0-12.70V). 0xFF ----> "No data"
UINT8 throttle; // 0.5% (0-127%). 0xFF ----> "No data"
UINT8 powerOut; // Power Output, 0.5% (0-127%). 0xFF ----> "No data"} STRU_TELE_ESC;
//////////////////////////////////////////////////////////////////
//
// LAP TIMER
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x0B
UINT8 sID; // Secondary ID
UINT8 lapNumber; // Lap last finished
UINT8 gateNumber; // Last gate passed
UINT32 lastLapTime; // Time of lap in 1ms increments (NOT duration)
UINT32 gateTime; // Duration between last 2 gates
UINT8 unused[4];
} STRU_TELE_LAPTIMER;
//////////////////////////////////////////////////////////////////
//
// TEXT GENERATOR
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x0C
UINT8 sID; // Secondary ID
UINT8 lineNumber; // Line number to display (0 = title, 1-8 for general,
// 255 = Erase all text on screen)
char text[13]; // 0-terminated text
} STRU_TELE_TEXTGEN;
//////////////////////////////////////////////////////////////////
//
// (Liquid) Fuel Flow/Capacity (Two Tanks/Engines)
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 id; // Source device = 0x22
UINT8 sID; // Secondary ID
UINT16 fuelConsumed_A; // Integrated fuel consumption, 0.1mL
UINT16 flowRate_A; // Instantaneous consumption, 0.01mL/min
UINT16 temp_A; // Temperature, 0.1C (0-655.34C)
UINT16 fuelConsumed_B; // Integrated fuel consumption, 0.1mL
UINT16 flowRate_B; // Instantaneous consumption, 0.01mL/min
UINT16 temp_B; // Temperature, 0.1C (0-655.34C)
UINT16 spare; // Not used
} STRU_TELE_FUEL;
//////////////////////////////////////////////////////////////////
//
// Battery Current/Capacity (Dual Batteries)
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 id; // Source device = 0x34
UINT8 sID; // Secondary ID
INT16 current_A; // Instantaneous current, 0.1A (0-3276.8A)
INT16 chargeUsed_A; // Integrated mAh used, 1mAh (0-32.766Ah)
UINT16 temp_A; // Temperature, 0.1C (0-150.0C,
// 0x7FFF indicates not populated)
INT16 current_B; // Instantaneous current, 0.1A (0-6553.4A)
INT16 chargeUsed_B; // Integrated mAh used, 1mAh (0-65.534Ah)
UINT16 temp_B; // Temperature, 0.1C (0-150.0C,
// 0x7FFF indicates not populated)
UINT16 spare; // Not used
} STRU_TELE_MAH;
//////////////////////////////////////////////////////////////////
//
// Digital Input Status (Retract Status) and Tank Pressure
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 id; // Source device = 0x36
UINT8 sID; // Secondary ID
UINT16 digital; // Digital inputs (bit per input)
UINT16 pressure; // Tank pressure, 0.1PSI (0-6553.4PSI)
} STRU_TELE_DIGITAL_AIR;
//////////////////////////////////////////////////////////////////
//
// Thrust/Strain Gauge
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 id; // Source device = 0x38
UINT8 sID; // Secondary ID
UINT16 strain_A, // Strain sensor A
strain_B, // Strain sensor B
strain_C, // Strain sensor D
strain_D; // Strain sensor C
} STRU_TELE_STRAIN;
//////////////////////////////////////////////////////////////////
//
// THIRD-PARTY 16-BIT DATA SIGNED/UNSIGNED
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 id; // Source device = 0x50
UINT8 sID; // Secondary ID
INT16 sField1, // Signed 16-bit data fields
sField2,
sField3;
UINT16 uField1, // Unsigned 16-bit data fields
uField2,
uField3,
uField4;
} STRU_TELE_USER_16SU;
//////////////////////////////////////////////////////////////////
//
// THIRD-PARTY 16-BIT SIGNED/UNSIGNED AND 32-BIT UNSIGNED
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 id; // Source device = 0x52
UINT8 sID; // Secondary ID
INT16 sField1, // Signed 16-bit data fields
sField2;
UINT16 uField1, // Unsigned 16-bit data fields
uField2,
uField3;
UINT32 u32Field; // Unsigned 32-bit data field
} STRU_TELE_USER_16SU32U;
//////////////////////////////////////////////////////////////////
//
// THIRD-PARTY 16-BIT SIGNED/UNSIGNED AND 32-BIT SIGNED
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 id; // Source device = 0x54
UINT8 sID; // Secondary ID
INT16 sField1, // Signed 16-bit data fields
sField2;
UINT16 uField1, // Unsigned 16-bit data fields
uField2,
uField3;
INT32 u32Field; // Signed 32-bit data field
} STRU_TELE_USER_16U32SU;
//////////////////////////////////////////////////////////////////
//
// THIRD-PARTY 16-BIT UNSIGNED AND 32-BIT SIGNED/UNSIGNED
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 id; // Source device = 0x56
UINT8 sID; // Secondary ID
UINT16 uField1; // Unsigned 16-bit data field
INT32 u32Field; // Signed 32-bit data field
INT32 u32Field1, // Signed 32-bit data fields
u32Field2;
} STRU_TELE_USER_16U32SU;
//////////////////////////////////////////////////////////////////
//
// POWERBOX
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x0A
UINT8 sID; // Secondary ID
UINT16 volt1; // Volts, 0v01v
UINT16 volt2; // Volts, 0.01v
UINT16 capacity1; // mAh, 1mAh
UINT16 capacity2; // mAh, 1mAh
UINT16 spare16_1;
UINT16 spare16_2;
UINT8 spare;
UINT8 alarms; // Alarm bitmask (see below)
} STRU_TELE_POWERBOX;
#define TELE_PBOX_ALARM_VOLTAGE_1 (0x01)
#define TELE_PBOX_ALARM_VOLTAGE_2 (0x02)
#define TELE_PBOX_ALARM_CAPACITY_1 (0x04)
#define TELE_PBOX_ALARM_CAPACITY_2 (0x08)
//#define TELE_PBOX_ALARM_RPM (0x10)
//#define TELE_PBOX_ALARM_TEMPERATURE (0x20)
#define TELE_PBOX_ALARM_RESERVED_1 (0x40)
#define TELE_PBOX_ALARM_RESERVED_2 (0x80)
//////////////////////////////////////////////////////////////////
//
// DUAL ENERGY
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 id; // Source device = 0x18
UINT8 sID; // Secondary ID
INT16 current_A; // Instantaneous current, 0.01A (0-328.7A)
INT16 chargeUsed_A; // Integrated mAh used, 0.1mAh (0-3276.6mAh)
UINT16 volts_A; // Voltage, 0.01VC (0-16.00V)
INT16 current_B; // Instantaneous current, 0.1A (0-3276.8A)
INT16 chargeUsed_B; // Integrated mAh used, 1mAh (0-32.766Ah)
UINT16 volts_B; // Voltage, 0.01VC (0-16.00V)
UINT16 spare; // Not used
} STRU_TELE_ENERGY_DUAL;
//////////////////////////////////////////////////////////////////
//
// HIGH-CURRENT
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x03
UINT8 sID; // Secondary ID
INT16 current, // Range: +/- 150A
// Resolution: 300A/2048 = 0.196791 A/tick
dummy; // TBD
} STRU_TELE_IHIGH;
#define IHIGH_RESOLUTION_FACTOR ((FP32)(0.196791))
//////////////////////////////////////////////////////////////////
//
// VARIO-S
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x40
UINT8 sID; // Secondary ID
INT16 altitude; // .1m increments
INT16 delta_0250ms, // delta last 250ms, 0.1m/s increments
delta_0500ms, // delta last 500ms, 0.1m/s increments
delta_1000ms, // delta last 1.0 seconds
delta_1500ms, // delta last 1.5 seconds
delta_2000ms, // delta last 2.0 seconds
delta_3000ms; // delta last 3.0 seconds
} STRU_TELE_VARIO_S;
//////////////////////////////////////////////////////////////////
//
// ALTIMETER
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier;
UINT8 sID; // Secondary ID
INT16 altitude; // .1m increments
INT16 maxAltitude; // .1m increments
} STRU_TELE_ALT;
//////////////////////////////////////////////////////////////////
//
// AIRSPEED
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier;
UINT8 sID; // Secondary ID
UINT16 airspeed; // 1 km/h increments
UINT16 maxAirspeed; // 1 km/h increments
} STRU_TELE_SPEED;
//////////////////////////////////////////////////////////////////
//
// GFORCE
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x14
UINT8 sID; // Secondary ID
INT16 GForceX; // force is reported as .01G increments
INT16 GForceY; // Range = +/-4000 (+/- 40G) in Pro model
INT16 GForceZ; // Range = +/-800 (+/- 8G) in Standard model
INT16 maxGForceX; // abs(max G X-axis) FORE/AFT
INT16 maxGForceY; // abs (max G Y-axis) LEFT/RIGHT
INT16 maxGForceZ; // max G Z-axis WING SPAR LOAD
INT16 minGForceZ; // min G Z-axis WING SPAR LOAD
} STRU_TELE_G_METER;
//////////////////////////////////////////////////////////////////
//
// JETCAT/TURBINE
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x15
UINT8 sID; // Secondary ID
UINT8 status; // See table below
UINT8 throttle; // (BCD) xx Percent
UINT16 packVoltage; // (BCD) xx.yy
UINT16 pumpVoltage; // (BCD) xx.yy
UINT32 RPM; // (BCD)
UINT16 EGT; // (BCD) Temperature, Celsius
UINT8 offCondition; // (BCD) See table below
UINT8 spare;
} STRU_TELE_JETCAT;
enum JETCAT_ECU_TURBINE_STATE { // ECU Status definitions
JETCAT_ECU_STATE_OFF = 0x00,
JETCAT_ECU_STATE_WAIT_for_RPM = 0x01, // (Stby/Start)
JETCAT_ECU_STATE_Ignite = 0x02,
JETCAT_ECU_STATE_Accelerate = 0x03,
JETCAT_ECU_STATE_Stabilise = 0x04,
JETCAT_ECU_STATE_Learn_HI = 0x05,
JETCAT_ECU_STATE_Learn_LO = 0x06,
JETCAT_ECU_STATE_UNDEFINED = 0x07,
JETCAT_ECU_STATE_Slow_Down = 0x08,
JETCAT_ECU_STATE_Manual = 0x09,
JETCAT_ECU_STATE_AutoOff = 0x10,
JETCAT_ECU_STATE_Run = 0x11, // (reg.)
JETCAT_ECU_STATE_Accleleration_delay = 0x12,
JETCAT_ECU_STATE_SpeedReg = 0x13, // (Speed Ctrl)
JETCAT_ECU_STATE_Two_Shaft_Regulate = 0x14, // (only for secondary shaft)
JETCAT_ECU_STATE_PreHeat1 = 0x15,
JETCAT_ECU_STATE_PreHeat2 = 0x16,
JETCAT_ECU_STATE_MainFStart = 0x17,
JETCAT_ECU_STATE_NotUsed = 0x18,
JETCAT_ECU_STATE_KeroFullOn = 0x19,
// undefined states 0x1A-0x1F
EVOJET_ECU_STATE_off = 0x20,
EVOJET_ECU_STATE_ignt = 0x21,
EVOJET_ECU_STATE_acce = 0x22,
EVOJET_ECU_STATE_run = 0x23,
EVOJET_ECU_STATE_cal = 0x24,
EVOJET_ECU_STATE_cool = 0x25,
EVOJET_ECU_STATE_fire = 0x26,
EVOJET_ECU_STATE_glow = 0x27,
EVOJET_ECU_STATE_heat = 0x28,
EVOJET_ECU_STATE_idle = 0x29,
EVOJET_ECU_STATE_lock = 0x2A,
EVOJET_ECU_STATE_rel = 0x2B,
EVOJET_ECU_STATE_spin = 0x2C,
EVOJET_ECU_STATE_stop = 0x2D,
// undefined states 0x2E-0x2F
HORNET_ECU_STATE_OFF = 0x30,
HORNET_ECU_STATE_SLOWDOWN = 0x31,
HORNET_ECU_STATE_COOL_DOWN = 0x32,
HORNET_ECU_STATE_AUTO = 0x33,
HORNET_ECU_STATE_AUTO_HC = 0x34,
HORNET_ECU_STATE_BURNER_ON = 0x35,
HORNET_ECU_STATE_CAL_IDLE = 0x36,
HORNET_ECU_STATE_CALIBRATE = 0x37,
HORNET_ECU_STATE_DEV_DELAY = 0x38,
HORNET_ECU_STATE_EMERGENCY = 0x39,
HORNET_ECU_STATE_FUEL_HEAT = 0x3A,
HORNET_ECU_STATE_FUEL_IGNITE = 0x3B,
HORNET_ECU_STATE_GO_IDLE = 0x3C,
HORNET_ECU_STATE_PROP_IGNITE = 0x3D,
HORNET_ECU_STATE_RAMP_DELAY = 0x3E,
HORNET_ECU_STATE_RAMP_UP = 0x3F,
HORNET_ECU_STATE_STANDBY = 0x40,
HORNET_ECU_STATE_STEADY = 0x41,
HORNET_ECU_STATE_WAIT_ACC = 0x42,
HORNET_ECU_STATE_ERROR = 0x43,
// undefined states 0x44-0x4F
XICOY_ECU_STATE_Temp_High = 0x50,
XICOY_ECU_STATE_Trim_Low = 0x51,
XICOY_ECU_STATE_Set_Idle = 0x52,
XICOY_ECU_STATE_Ready = 0x53,
XICOY_ECU_STATE_Ignition = 0x54,
XICOY_ECU_STATE_Fuel_Ramp = 0x55,
XICOY_ECU_STATE_Glow_Test = 0x56,
XICOY_ECU_STATE_Running = 0x57,
XICOY_ECU_STATE_Stop = 0x58,
XICOY_ECU_STATE_Flameout = 0x59,
XICOY_ECU_STATE_Speed_Low = 0x5A,
XICOY_ECU_STATE_Cooling = 0x5B,
XICOY_ECU_STATE_Igniter_Bad = 0x5C,
XICOY_ECU_STATE_Starter_F = 0x5D,
XICOY_ECU_STATE_Weak_Fuel = 0x5E,
XICOY_ECU_STATE_Start_On = 0x5F,
XICOY_ECU_STATE_Pre_Heat = 0x60,
XICOY_ECU_STATE_Battery = 0x61,
XICOY_ECU_STATE_Time_Out = 0x62,
XICOY_ECU_STATE_Overload = 0x63,
XICOY_ECU_STATE_Igniter_Fail = 0x64,
XICOY_ECU_STATE_Burner_On = 0x65,
XICOY_ECU_STATE_Starting = 0x66,
XICOY_ECU_STATE_SwitchOver = 0x67,
XICOY_ECU_STATE_Cal_Pump = 0x68,
XICOY_ECU_STATE_Pump_Limit = 0x69,
XICOY_ECU_STATE_No_Engine = 0x6A,
XICOY_ECU_STATE_Pwr_Boost = 0x6B,
XICOY_ECU_STATE_Run_Idle = 0x6C,
XICOY_ECU_STATE_Run_Max = 0x6D,
TURBINE_ECU_MAX_STATE = 0x74
};
enum JETCAT_ECU_OFF_CONDITIONS { // ECU off conditions. Valid only when the ECUStatus = JETCAT_ECU_STATE_OFF
JETCAT_ECU_OFF_No_Off_Condition_defined = 0,
JETCAT_ECU_OFF_Shut_down_via_RC,
JETCAT_ECU_OFF_Overtemperature,
JETCAT_ECU_OFF_Ignition_timeout,
JETCAT_ECU_OFF_Acceleration_time_out,
JETCAT_ECU_OFF_Acceleration_too_slow,
JETCAT_ECU_OFF_Over_RPM,
JETCAT_ECU_OFF_Low_Rpm_Off,
JETCAT_ECU_OFF_Low_Battery,
JETCAT_ECU_OFF_Auto_Off,
JETCAT_ECU_OFF_Low_temperature_Off,
JETCAT_ECU_OFF_Hi_Temp_Off,
JETCAT_ECU_OFF_Glow_Plug_defective,
JETCAT_ECU_OFF_Watch_Dog_Timer,
JETCAT_ECU_OFF_Fail_Safe_Off,
JETCAT_ECU_OFF_Manual_Off, // (via GSU)
JETCAT_ECU_OFF_Power_fail, // (Battery fail)
JETCAT_ECU_OFF_Temp_Sensor_fail, // (only during startup)
JETCAT_ECU_OFF_Fuel_fail,
JETCAT_ECU_OFF_Prop_fail,
JETCAT_ECU_OFF_2nd_Engine_fail,
JETCAT_ECU_OFF_2nd_Engine_Diff_Too_High,
JETCAT_ECU_OFF_2nd_Engine_No_Comm,
JETCAT_ECU_MAX_OFF_COND
};
typedef struct
{
UINT8 identifier; // Source device = 0x19
UINT8 sID; // Secondary ID
UINT16 FuelFlowRateMLMin; // (BCD) mL per Minute
UINT32 RestFuelVolumeInTankML; // (BCD) mL remaining in tank
// 8 bytes left
} STRU_TELE_JETCAT2;
//////////////////////////////////////////////////////////////////
//
// GPS
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x16
UINT8 sID; // Secondary ID
UINT16 altitudeLow; // BCD, meters, format 3.1 (Low bits of alt)
UINT32 latitude; // BCD, format 4.4,
// Degrees * 100 + minutes, < 100 degrees
UINT32 longitude; // BCD, format 4.4,
// Degrees * 100 + minutes, flag --> > 99deg
UINT16 course; // BCD, 3.1
UINT8 HDOP; // BCD, format 1.1
UINT8 GPSflags; // see definitions below
} STRU_TELE_GPS_LOC;
typedef struct
{
UINT8 identifier; // Source device = 0x17
UINT8 sID; // Secondary ID
UINT16 speed; // BCD, knots, format 3.1
UINT32 UTC; // BCD, format HH:MM:SS.S, format 6.1
UINT8 numSats; // BCD, 0-99
UINT8 altitudeHigh; // BCD, meters, format 2.0 (High bits alt)
} STRU_TELE_GPS_STAT;
// GPS flags definitions:
#define GPS_INFO_FLAGS_IS_NORTH_BIT (0)
#define GPS_INFO_FLAGS_IS_NORTH (1 << GPS_INFO_FLAGS_IS_NORTH_BIT)
#define GPS_INFO_FLAGS_IS_EAST_BIT (1)
#define GPS_INFO_FLAGS_IS_EAST (1 << GPS_INFO_FLAGS_IS_EAST_BIT)
#define GPS_INFO_FLAGS_LONG_GREATER_99_BIT (2)
#define GPS_INFO_FLAGS_LONG_GREATER_99 (1 << GPS_INFO_FLAGS_LONG_GREATER_99_BIT)
#define GPS_INFO_FLAGS_GPS_FIX_VALID_BIT (3)
#define GPS_INFO_FLAGS_GPS_FIX_VALID (1 << GPS_INFO_FLAGS_GPS_FIX_VALID_BIT)
#define GPS_INFO_FLAGS_GPS_DATA_RECEIVED_BIT (4)
#define GPS_INFO_FLAGS_GPS_DATA_RECEIVED (1 << GPS_INFO_FLAGS_GPS_DATA_RECEIVED_BIT)
#define GPS_INFO_FLAGS_3D_FIX_BIT (5)
#define GPS_INFO_FLAGS_3D_FIX (1 << GPS_INFO_FLAGS_3D_FIX_BIT)
#define GPS_INFO_FLAGS_NEGATIVE_ALT_BIT (7)
#define GPS_INFO_FLAGS_NEGATIVE_ALT (1 << GPS_INFO_FLAGS_NEGATIVE_ALT_BIT)
//////////////////////////////////////////////////////////////////
//
// GYRO
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x1A
UINT8 sID; // Secondary ID
INT16 gyroX; // Rotation rates of the body - Rate
// is about the X Axis which is
// defined out the nose of the
// vehicle.
INT16 gyroY; // Units are 0.1 deg/sec - Rate is
// about the Y Axis which is defined
// out the right wing of the vehicle.
INT16 gyroZ; // Rate is about the Z axis which is
// defined down from the vehicle.
INT16 maxGyroX; // Max rates (absolute value)
INT16 maxGyroY;
INT16 maxGyroZ;
} STRU_TELE_GYRO;
//////////////////////////////////////////////////////////////////
//
// Alpha6 Stabilizer
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x24
UINT8 sID; // Secondary ID
UINT16 volts; // 0.01V increments
UINT8 state_FM; // Flight Mode and System State
// (see below)
UINT8 gainRoll, // Roll Gain, high bit -->
// Heading Hold
gainPitch, // Pitch Gain
gainYaw; // Yaw Gain
INT16 attRoll, // Roll Attitude, 0.1degree, RHR
attPitch, // Pitch Attitude
attYaw; // Yaw Attitude
UINT16 spare;
} STRU_TELE_ALPHA6;
#define GBOX_STATE_BOOT (0x00) // Alpha6 State - Boot
#define GBOX_STATE_INIT (0x01) // Init
#define GBOX_STATE_READY (0x02) // Ready
#define GBOX_STATE_SENSORFAULT (0x03) // Sensor Fault
#define GBOX_STATE_POWERFAULT (0x04) // Power Fault
#define GBOX_STATE_MASK (0x0F)
#define GBOX_FMODE_FM0 (0x00) // FM0 through FM4
#define GBOX_FMODE_FM1 (0x10)
#define GBOX_FMODE_FM2 (0x20)
#define GBOX_FMODE_FM3 (0x30)
#define GBOX_FMODE_FM4 (0x40)
#define GBOX_FMODE_PANIC (0x50)
#define GBOX_FMODE_MASK (0xF0)
//////////////////////////////////////////////////////////////////
//
// 6S LiPo Cell Monitor
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x3A
UINT8 sID; // Secondary ID
UINT16 cell[6]; // Voltage across cell 1, .01V steps
// 0x7FFF --> cell not present
UINT16 temp; // Temperature, 0.1C (0-655.34C)
} STRU_TELE_LIPOMON;
//////////////////////////////////////////////////////////////////
//
// 14S LiPo Cell Monitor
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x3F
UINT8 sID; // Secondary ID
UINT8 cell[14]; // Voltage across cell 1, .01V steps,
// excess of 2.56V (ie, 3.00V would
// report 300-256 = 44)
// 0xFF --> cell not present
} STRU_TELE_LIPOMON_14;
//////////////////////////////////////////////////////////////////
//
// ATTITUDE & MAG COMPASS
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x1B
UINT8 sID; // Secondary ID
INT16 attRoll; // Attitude, 3 axes. Roll is a
// rotation about the X Axis of
// the vehicle using the RHR.
INT16 attPitch; // Units are 0.1 deg - Pitch is a
// rotation about the Y Axis of the
// vehicle using the RHR.
INT16 attYaw; // Yaw is a rotation about the Z
// Axis of the vehicle using the RHR.
INT16 magX; // Magnetic Compass, 3 axes
INT16 magY; // Units are TBD
INT16 magZ; //
} STRU_TELE_ATTMAG;
//////////////////////////////////////////////////////////////////
//
// Real-Time Clock
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x7C
UINT8 sID; // Secondary ID
UINT8 spare[6];
UINT64 UTC64; // Linux 64-bit time_t for
// post-2038 date compatibility
} STRU_TELE_RTC;
//////////////////////////////////////////////////////////////////
//
// RPM/Volts/Temperature
//
//////////////////////////////////////////////////////////////////
//
typedef struct
{
UINT8 identifier; // Source device = 0x7E
UINT8 sID; // Secondary ID
UINT16 microseconds; // microseconds between pulse leading edges
UINT16 volts; // 0.01V increments
INT16 temperature; // degrees F
INT8 dBm_A, // Average signal for A antenna in dBm
dBm_B; // Average signal for B antenna in dBm.
// If only 1 antenna, set B = A
} STRU_TELE_RPM;
//////////////////////////////////////////////////////////////////
//
// QoS DATA
//
//////////////////////////////////////////////////////////////////
//
// NOTE: AR6410-series send:
// id = 7F
// sID = 0
// A = 0
// B = 0
// L = 0
// R = 0
// F = fades
// H = holds
// rxV = 0xFFFF
//
typedef struct
{
UINT8 identifier; // Source device = 0x7F
UINT8 sID; // Secondary ID
UINT16 A;
UINT16 B;
UINT16 L;
UINT16 R;
UINT16 F;
UINT16 H;
UINT16 rxVoltage; // Volts, 0.01V increments
} STRU_TELE_QOS;
//////////////////////////////////////////////////////////////////
//
// UNION OF ALL DEVICE MESSAGES
//
//////////////////////////////////////////////////////////////////
//
typedef union
{
UINT16 raw[8];
STRU_TELE_RTC rtc;
STRU_TELE_QOS qos;
STRU_TELE_RPM rpm;
STRU_TELE_FRAMEDATA frame;
STRU_TELE_ALT alt;
STRU_TELE_SPEED speed;
STRU_TELE_ENERGY_DUAL eDual;
STRU_TELE_VARIO_S varioS;
STRU_TELE_G_METER accel;
STRU_TELE_JETCAT jetcat;
STRU_TELE_JETCAT2 jetcat2;
STRU_TELE_GPS_LOC gpsloc;
STRU_TELE_GPS_STAT gpsstat;
STRU_TELE_GYRO gyro;
STRU_TELE_ATTMAG attMag;
STRU_TELE_POWERBOX powerBox;
STRU_TELE_ESC escGeneric;
STRU_TELE_LAPTIMER lapTimer;
STRU_TELE_TEXTGEN textgen;
STRU_TELE_FUEL fuel;
STRU_TELE_MAH mAh;
STRU_TELE_DIGITAL_AIR digAir;
STRU_TELE_STRAIN strain;
STRU_TELE_LIPOMON lipomon;
STRU_TELE_LIPOMON_14 lipomon14;
STRU_TELE_USER_16SU user_16SU;
STRU_TELE_USER_16SU32U user_16SU32U;
STRU_TELE_USER_16SU32S user_16SU32S;
STRU_TELE_USER_16U32SU user_16U32SU;
} UN_TELEMETRY; // All telemetry messages

197
src/main/blackbox/blackbox.c
View file

@ -24,15 +24,22 @@
#ifdef BLACKBOX
#include "blackbox.h"
#include "blackbox_io.h"
#include "build/debug.h"
#include "build/version.h"
#include "common/axis.h"
#include "common/encoding.h"
#include "common/maths.h"
#include "common/utils.h"
#include "blackbox.h"
#include "blackbox_io.h"
#include "config/config_master.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/compass.h"
@ -43,18 +50,22 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "sensors/sensors.h"
#include "rx/rx.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/compass.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sonar.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#define BLACKBOX_I_INTERVAL 32
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
#define SLOW_FRAME_INTERVAL 4096
@ -232,21 +243,22 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
typedef enum BlackboxState {
BLACKBOX_STATE_DISABLED = 0,
BLACKBOX_STATE_STOPPED,
BLACKBOX_STATE_PREPARE_LOG_FILE,
BLACKBOX_STATE_SEND_HEADER,
BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
BLACKBOX_STATE_SEND_GPS_H_HEADER,
BLACKBOX_STATE_SEND_GPS_G_HEADER,
BLACKBOX_STATE_SEND_SLOW_HEADER,
BLACKBOX_STATE_SEND_SYSINFO,
BLACKBOX_STATE_PAUSED,
BLACKBOX_STATE_RUNNING,
BLACKBOX_STATE_SHUTTING_DOWN
BLACKBOX_STATE_STOPPED, //1
BLACKBOX_STATE_PREPARE_LOG_FILE, //2
BLACKBOX_STATE_SEND_HEADER, //3
BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, //4
BLACKBOX_STATE_SEND_GPS_H_HEADER, //5
BLACKBOX_STATE_SEND_GPS_G_HEADER, //6
BLACKBOX_STATE_SEND_SLOW_HEADER, //7
BLACKBOX_STATE_SEND_SYSINFO, //8
BLACKBOX_STATE_PAUSED, //9
BLACKBOX_STATE_RUNNING, //10
BLACKBOX_STATE_SHUTTING_DOWN, //11
BLACKBOX_STATE_START_ERASE, //12
BLACKBOX_STATE_ERASING, //13
BLACKBOX_STATE_ERASED //14
} BlackboxState;
#define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER
#define BLACKBOX_LAST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_SYSINFO
typedef struct blackboxMainState_s {
uint32_t time;
@ -761,16 +773,16 @@ void validateBlackboxConfig()
if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0
|| blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) {
blackboxConfig()->rate_num = 1;
blackboxConfig()->rate_denom = 1;
blackboxConfigMutable()->rate_num = 1;
blackboxConfigMutable()->rate_denom = 1;
} else {
/* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
* itself more frequently)
*/
div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
blackboxConfig()->rate_num /= div;
blackboxConfig()->rate_denom /= div;
blackboxConfigMutable()->rate_num /= div;
blackboxConfigMutable()->rate_denom /= div;
}
// If we've chosen an unsupported device, change the device to serial
@ -786,7 +798,7 @@ void validateBlackboxConfig()
break;
default:
blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL;
blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL;
}
}
@ -795,47 +807,44 @@ void validateBlackboxConfig()
*/
void startBlackbox(void)
{
if (blackboxState == BLACKBOX_STATE_STOPPED) {
validateBlackboxConfig();
validateBlackboxConfig();
if (!blackboxDeviceOpen()) {
blackboxSetState(BLACKBOX_STATE_DISABLED);
return;
}
memset(&gpsHistory, 0, sizeof(gpsHistory));
blackboxHistory[0] = &blackboxHistoryRing[0];
blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2];
vbatReference = vbatLatest;
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
/*
* We use conditional tests to decide whether or not certain fields should be logged. Since our headers
* must always agree with the logged data, the results of these tests must not change during logging. So
* cache those now.
*/
blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationProfile()->modeActivationConditions, BOXBLACKBOX);
blackboxIteration = 0;
blackboxPFrameIndex = 0;
blackboxIFrameIndex = 0;
/*
* Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
* it finally plays the beep for this arming event.
*/
blackboxLastArmingBeep = getArmingBeepTimeMicros();
blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status
blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
if (!blackboxDeviceOpen()) {
blackboxSetState(BLACKBOX_STATE_DISABLED);
return;
}
memset(&gpsHistory, 0, sizeof(gpsHistory));
blackboxHistory[0] = &blackboxHistoryRing[0];
blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2];
vbatReference = vbatLatest;
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
/*
* We use conditional tests to decide whether or not certain fields should be logged. Since our headers
* must always agree with the logged data, the results of these tests must not change during logging. So
* cache those now.
*/
blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX);
blackboxIteration = 0;
blackboxPFrameIndex = 0;
blackboxIFrameIndex = 0;
/*
* Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
* it finally plays the beep for this arming event.
*/
blackboxLastArmingBeep = getArmingBeepTimeMicros();
blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status
blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
}
/**
@ -1175,7 +1184,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight");
BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime);
BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name);
BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", systemConfig()->name);
BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle);
@ -1283,12 +1292,12 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", rxConfig()->rcInterpolationInterval);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", rxConfig()->airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d", rxConfig()->serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", motorConfig()->useUnsyncedPwm);
BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", motorConfig()->motorPwmProtocol);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", motorConfig()->motorPwmRate);
BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", motorConfig()->dev.useUnsyncedPwm);
BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", motorConfig()->dev.motorPwmProtocol);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", motorConfig()->dev.motorPwmRate);
BLACKBOX_PRINT_HEADER_LINE("digitalIdleOffset:%d", (int)(motorConfig()->digitalIdleOffsetPercent * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d", masterConfig.debug_mode);
BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures);
BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d", systemConfig()->debug_mode);
BLACKBOX_PRINT_HEADER_LINE("features:%d", featureConfig()->enabledFeatures);
default:
return true;
@ -1461,17 +1470,23 @@ void handleBlackbox(timeUs_t currentTimeUs)
{
int i;
if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) {
blackboxReplenishHeaderBudget();
}
switch (blackboxState) {
case BLACKBOX_STATE_STOPPED:
if (ARMING_FLAG(ARMED)) {
blackboxOpen();
startBlackbox();
}
if (IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
blackboxSetState(BLACKBOX_STATE_START_ERASE);
}
break;
case BLACKBOX_STATE_PREPARE_LOG_FILE:
if (blackboxDeviceBeginLog()) {
blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
}
break;
case BLACKBOX_STATE_SEND_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
/*
@ -1492,6 +1507,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
}
break;
case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields),
&blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
@ -1505,6 +1521,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
break;
#ifdef GPS
case BLACKBOX_STATE_SEND_GPS_H_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAY_LENGTH(blackboxGpsHFields),
NULL, NULL)) {
@ -1512,6 +1529,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
}
break;
case BLACKBOX_STATE_SEND_GPS_G_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAY_LENGTH(blackboxGpsGFields),
&blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) {
@ -1520,6 +1538,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
break;
#endif
case BLACKBOX_STATE_SEND_SLOW_HEADER:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAY_LENGTH(blackboxSlowFields),
NULL, NULL)) {
@ -1527,6 +1546,7 @@ void handleBlackbox(timeUs_t currentTimeUs)
}
break;
case BLACKBOX_STATE_SEND_SYSINFO:
blackboxReplenishHeaderBudget();
//On entry of this state, xmitState.headerIndex is 0
//Keep writing chunks of the system info headers until it returns true to signal completion
@ -1556,7 +1576,6 @@ void handleBlackbox(timeUs_t currentTimeUs)
blackboxLogIteration(currentTimeUs);
}
// Keep the logging timers ticking so our log iteration continues to advance
blackboxAdvanceIterationTimers();
break;
@ -1568,7 +1587,6 @@ void handleBlackbox(timeUs_t currentTimeUs)
} else {
blackboxLogIteration(currentTimeUs);
}
blackboxAdvanceIterationTimers();
break;
case BLACKBOX_STATE_SHUTTING_DOWN:
@ -1585,15 +1603,36 @@ void handleBlackbox(timeUs_t currentTimeUs)
blackboxSetState(BLACKBOX_STATE_STOPPED);
}
break;
case BLACKBOX_STATE_START_ERASE:
blackboxEraseAll();
blackboxSetState(BLACKBOX_STATE_ERASING);
beeper(BEEPER_BLACKBOX_ERASE);
break;
case BLACKBOX_STATE_ERASING:
if (isBlackboxErased()) {
//Done eraseing
blackboxSetState(BLACKBOX_STATE_ERASED);
beeper(BEEPER_BLACKBOX_ERASE);
}
break;
case BLACKBOX_STATE_ERASED:
if (!IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
blackboxSetState(BLACKBOX_STATE_STOPPED);
}
break;
default:
break;
}
// Did we run out of room on the device? Stop!
if (isBlackboxDeviceFull()) {
blackboxSetState(BLACKBOX_STATE_STOPPED);
// ensure we reset the test mode flag if we stop due to full memory card
if (startedLoggingInTestMode) startedLoggingInTestMode = false;
if (blackboxState != BLACKBOX_STATE_ERASING
&& blackboxState != BLACKBOX_STATE_START_ERASE
&& blackboxState != BLACKBOX_STATE_ERASED) {
blackboxSetState(BLACKBOX_STATE_STOPPED);
// ensure we reset the test mode flag if we stop due to full memory card
if (startedLoggingInTestMode) startedLoggingInTestMode = false;
}
} else { // Only log in test mode if there is room!
if(blackboxConfig()->on_motor_test) {

4
src/main/blackbox/blackbox.h
View file

@ -21,6 +21,8 @@
#include "common/time.h"
#include "config/parameter_group.h"
typedef struct blackboxConfig_s {
uint8_t rate_num;
uint8_t rate_denom;
@ -28,6 +30,8 @@ typedef struct blackboxConfig_s {
uint8_t on_motor_test;
} blackboxConfig_t;
PG_DECLARE(blackboxConfig_t, blackboxConfig);
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
void initBlackbox(void);

55
src/main/blackbox/blackbox_io.c
View file

@ -8,14 +8,20 @@
#ifdef BLACKBOX
#include "blackbox.h"
#include "blackbox_io.h"
#include "build/version.h"
#include "build/build_config.h"
#include "common/encoding.h"
#include "common/maths.h"
#include "common/printf.h"
#include "config/config_profile.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
@ -23,12 +29,10 @@
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/serial.h"
#include "msp/msp_serial.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#define BLACKBOX_SERIAL_PORT_MODE MODE_TX
// How many bytes can we transmit per loop iteration when writing headers?
@ -63,6 +67,14 @@ static struct {
#endif
void blackboxOpen()
{
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
}
void blackboxWrite(uint8_t value)
{
switch (blackboxConfig()->device) {
@ -599,6 +611,43 @@ bool blackboxDeviceOpen(void)
}
}
/**
* Erase all blackbox logs
*/
void blackboxEraseAll(void)
{
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
flashfsEraseCompletely();
break;
#endif
default:
//not supported
break;
}
}
/**
* Check to see if erasing is done
*/
bool isBlackboxErased(void)
{
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
return flashfsIsReady();
break;
#endif
default:
//not supported
return true;
break;
}
}
/**
* Close the Blackbox logging device immediately without attempting to flush any remaining data.
*/

4
src/main/blackbox/blackbox_io.h
View file

@ -49,6 +49,7 @@ typedef enum {
extern int32_t blackboxHeaderBudget;
void blackboxOpen(void);
void blackboxWrite(uint8_t value);
int blackboxPrintf(const char *fmt, ...);
@ -71,6 +72,9 @@ bool blackboxDeviceFlushForce(void);
bool blackboxDeviceOpen(void);
void blackboxDeviceClose(void);
void blackboxEraseAll(void);
bool isBlackboxErased(void);
bool blackboxDeviceBeginLog(void);
bool blackboxDeviceEndLog(bool retainLog);

24
src/main/build/atomic.h
View file

@ -69,25 +69,25 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
__ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 )
// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier.
// Be carefull when using this, you must use some method to prevent optimizer form breaking things
// - lto is used for baseflight compillation, so function call is not memory barrier
// - use ATOMIC_BARRIER or propper volatile to protect used variables
// - gcc 4.8.4 does write all values in registes to memory before 'asm volatile', so this optimization does not help much
// but that can change in future versions
// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create memory barrier.
// Be careful when using this, you must use some method to prevent optimizer form breaking things
// - lto is used for Cleanflight compilation, so function call is not memory barrier
// - use ATOMIC_BARRIER or volatile to protect used variables
// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
// - gcc 5 and later works as intended, generating quite optimal code
#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
__ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
// ATOMIC_BARRIER
// Create memory barrier
// - at the beginning (all data must be reread from memory)
// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use)
// ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier
// - at the beginning of containing block (value of parameter must be reread from memory)
// - at exit of block (all exit paths) (parameter value if written into memory, but may be cached in register for subsequent use)
// On gcc 5 and higher, this protects only memory passed as parameter (any type should work)
// this macro can be used only ONCE PER LINE, but multiple uses per block are fine
#if (__GNUC__ > 6)
#warning "Please verify that ATOMIC_BARRIER works as intended"
// increment version number is BARRIER works
// increment version number if BARRIER works
// TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead
// you should check that local variable scope with cleanup spans entire block
#endif
@ -101,10 +101,10 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
// this macro uses local function for cleanup. CLang block can be substituded
#define ATOMIC_BARRIER(data) \
__extension__ void __UNIQL(__barrierEnd)(typeof(data) **__d) { \
__asm__ volatile ("\t# barier(" #data ") end\n" : : "m" (**__d)); \
__asm__ volatile ("\t# barrier(" #data ") end\n" : : "m" (**__d)); \
} \
typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \
__asm__ volatile ("\t# barier (" #data ") start\n" : "=m" (*__UNIQL(__barrier)))
__asm__ volatile ("\t# barrier (" #data ") start\n" : "+m" (*__UNIQL(__barrier)))
// define these wrappers for atomic operations, use gcc buildins

4
src/main/build/version.h
View file

@ -17,8 +17,8 @@
#define FC_FIRMWARE_NAME "Betaflight"
#define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 6 // increment when a bug is fixed
#define FC_VERSION_MINOR 2 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)

15
src/main/cms/cms.c
View file

@ -44,19 +44,24 @@
#include "drivers/system.h"
// For rcData, stopAllMotors, stopPwmAllMotors
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
// For 'ARM' related
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
// For rcData, stopAllMotors, stopPwmAllMotors
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "flight/mixer.h"
// For VISIBLE* (Actually, included by config_master.h)
// For VISIBLE*
#include "io/osd.h"
#include "rx/rx.h"
// DisplayPort management
#ifndef CMS_MAX_DEVICE

24
src/main/cms/cms_menu_blackbox.c
View file

@ -27,11 +27,12 @@
#include "platform.h"
#include "build/version.h"
#ifdef CMS
#include "drivers/system.h"
#include "build/version.h"
#include "blackbox/blackbox.h"
#include "blackbox/blackbox_io.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
@ -40,13 +41,16 @@
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/beeper.h"
#include "blackbox/blackbox_io.h"
#ifdef USE_FLASHFS
static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
@ -62,6 +66,7 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
delay(100);
}
beeper(BEEPER_BLACKBOX_ERASE);
displayClearScreen(pDisplay);
displayResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI?
@ -78,6 +83,7 @@ static const char * const cmsx_BlackboxDeviceNames[] = {
static bool featureRead = false;
static uint8_t cmsx_FeatureBlackbox;
static uint8_t blackboxConfig_rate_denom;
static uint8_t cmsx_BlackboxDevice;
static OSD_TAB_t cmsx_BlackboxDeviceTable = { &cmsx_BlackboxDevice, 2, cmsx_BlackboxDeviceNames };
@ -167,7 +173,7 @@ static long cmsx_Blackbox_onEnter(void)
cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0;
featureRead = true;
}
blackboxConfig_rate_denom = blackboxConfig()->rate_denom;
return 0;
}
@ -176,10 +182,10 @@ static long cmsx_Blackbox_onExit(const OSD_Entry *self)
UNUSED(self);
if (blackboxMayEditConfig()) {
blackboxConfig()->device = cmsx_BlackboxDevice;
blackboxConfigMutable()->device = cmsx_BlackboxDevice;
validateBlackboxConfig();
}
blackboxConfigMutable()->rate_denom = blackboxConfig_rate_denom;
return 0;
}
@ -203,7 +209,7 @@ static OSD_Entry cmsx_menuBlackboxEntries[] =
{ "(STATUS)", OME_String, NULL, &cmsx_BlackboxStatus, 0 },
{ "(USED)", OME_String, NULL, &cmsx_BlackboxDeviceStorageUsed, 0 },
{ "(FREE)", OME_String, NULL, &cmsx_BlackboxDeviceStorageFree, 0 },
{ "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &blackboxConfig()->rate_denom, 1, 32, 1 }, 0 },
{ "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &blackboxConfig_rate_denom, 1, 32, 1 }, 0 },
#ifdef USE_FLASHFS
{ "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 },

106
src/main/cms/cms_menu_imu.c
View file

@ -43,9 +43,11 @@
#include "flight/pid.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
//
// PID
@ -104,10 +106,11 @@ static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void
static long cmsx_PidRead(void)
{
const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
for (uint8_t i = 0; i < 3; i++) {
tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i];
tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i];
tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i];
tempPid[i][0] = pidProfile->P8[i];
tempPid[i][1] = pidProfile->I8[i];
tempPid[i][2] = pidProfile->D8[i];
}
return 0;
@ -125,10 +128,11 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
{
UNUSED(self);
pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
for (uint8_t i = 0; i < 3; i++) {
masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0];
masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1];
masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2];
pidProfile->P8[i] = tempPid[i][0];
pidProfile->I8[i] = tempPid[i][1];
pidProfile->D8[i] = tempPid[i][2];
}
pidInitConfig(&currentProfile->pidProfile);
@ -233,12 +237,13 @@ static long cmsx_profileOtherOnEnter(void)
{
profileIndexString[1] = '0' + tmpProfileIndex;
cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight;
cmsx_setpointRelaxRatio = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio;
const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight;
cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio;
cmsx_angleStrength = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL];
cmsx_horizonStrength = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL];
cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL];
cmsx_angleStrength = pidProfile->P8[PIDLEVEL];
cmsx_horizonStrength = pidProfile->I8[PIDLEVEL];
cmsx_horizonTransition = pidProfile->D8[PIDLEVEL];
return 0;
}
@ -247,13 +252,14 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
{
UNUSED(self);
masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight;
masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight;
pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidInitConfig(&currentProfile->pidProfile);
masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;
masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition;
pidProfile->P8[PIDLEVEL] = cmsx_angleStrength;
pidProfile->I8[PIDLEVEL] = cmsx_horizonStrength;
pidProfile->D8[PIDLEVEL] = cmsx_horizonTransition;
return 0;
}
@ -280,15 +286,45 @@ static CMS_Menu cmsx_menuProfileOther = {
.entries = cmsx_menuProfileOtherEntries,
};
static uint8_t gyroConfig_gyro_soft_lpf_hz;
static uint16_t gyroConfig_gyro_soft_notch_hz_1;
static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
static uint16_t gyroConfig_gyro_soft_notch_hz_2;
static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
static long cmsx_menuGyro_onEnter(void)
{
gyroConfig_gyro_soft_lpf_hz = gyroConfig()->gyro_soft_lpf_hz;
gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
return 0;
}
static long cmsx_menuGyro_onExit(const OSD_Entry *self)
{
UNUSED(self);
gyroConfigMutable()->gyro_soft_lpf_hz = gyroConfig_gyro_soft_lpf_hz;
gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
return 0;
}
static OSD_Entry cmsx_menuFilterGlobalEntries[] =
{
{ "-- FILTER GLB --", OME_Label, NULL, NULL, 0 },
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig()->gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
@ -297,8 +333,8 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] =
static CMS_Menu cmsx_menuFilterGlobal = {
.GUARD_text = "XFLTGLB",
.GUARD_type = OME_MENU,
.onEnter = NULL,
.onExit = NULL,
.onEnter = cmsx_menuGyro_onEnter,
.onExit = cmsx_menuGyro_onExit,
.onGlobalExit = NULL,
.entries = cmsx_menuFilterGlobalEntries,
};
@ -311,11 +347,12 @@ static uint16_t cmsx_yaw_p_limit;
static long cmsx_FilterPerProfileRead(void)
{
cmsx_dterm_lpf_hz = masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz;
cmsx_dterm_notch_hz = masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz;
cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff;
cmsx_yaw_lpf_hz = masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz;
cmsx_yaw_p_limit = masterConfig.profile[profileIndex].pidProfile.yaw_p_limit;
const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
cmsx_dterm_lpf_hz = pidProfile->dterm_lpf_hz;
cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz;
cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
cmsx_yaw_lpf_hz = pidProfile->yaw_lpf_hz;
cmsx_yaw_p_limit = pidProfile->yaw_p_limit;
return 0;
}
@ -324,11 +361,12 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
{
UNUSED(self);
masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz;
masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz = cmsx_dterm_notch_hz;
masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz;
masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit;
pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
pidProfile->dterm_lpf_hz = cmsx_dterm_lpf_hz;
pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz;
pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
pidProfile->yaw_lpf_hz = cmsx_yaw_lpf_hz;
pidProfile->yaw_p_limit = cmsx_yaw_p_limit;
return 0;
}

7
src/main/cms/cms_menu_ledstrip.c
View file

@ -22,15 +22,16 @@
#include "platform.h"
#include "build/version.h"
#ifdef CMS
#include "build/version.h"
#include "drivers/system.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "cms/cms.h"
#include "cms/cms_types.h"

52
src/main/cms/cms_menu_misc.c
View file

@ -22,20 +22,29 @@
#include "platform.h"
#include "build/version.h"
#ifdef CMS
#include "drivers/system.h"
#include "build/version.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "drivers/system.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "cms/cms_menu_ledstrip.h"
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "flight/mixer.h"
#include "rx/rx.h"
#include "sensors/battery.h"
//
// Misc
//
@ -78,14 +87,35 @@ CMS_Menu cmsx_menuRcPreview = {
.entries = cmsx_menuRcEntries
};
static uint16_t motorConfig_minthrottle;
static uint8_t batteryConfig_vbatscale;
static uint8_t batteryConfig_vbatmaxcellvoltage;
static long cmsx_menuMiscOnEnter(void)
{
motorConfig_minthrottle = motorConfig()->minthrottle;
batteryConfig_vbatscale = batteryConfig()->vbatscale;
batteryConfig_vbatmaxcellvoltage = batteryConfig()->vbatmaxcellvoltage;
return 0;
}
static long cmsx_menuMiscOnExit(const OSD_Entry *self)
{
UNUSED(self);
motorConfigMutable()->minthrottle = motorConfig_minthrottle;
batteryConfigMutable()->vbatscale = batteryConfig_vbatscale;
batteryConfigMutable()->vbatmaxcellvoltage = batteryConfig_vbatmaxcellvoltage;
return 0;
}
static OSD_Entry menuMiscEntries[]=
{
{ "-- MISC --", OME_Label, NULL, NULL, 0 },
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig()->minthrottle, 1000, 2000, 1 }, 0 },
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatscale, 1, 250, 1 }, 0 },
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig_minthrottle, 1000, 2000, 1 }, 0 },
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatscale, 1, 250, 1 }, 0 },
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0},
{ "BACK", OME_Back, NULL, NULL, 0},
@ -95,8 +125,8 @@ static OSD_Entry menuMiscEntries[]=
CMS_Menu cmsx_menuMisc = {
.GUARD_text = "XMISC",
.GUARD_type = OME_MENU,
.onEnter = NULL,
.onExit = NULL,
.onEnter = cmsx_menuMiscOnEnter,
.onExit = cmsx_menuMiscOnExit,
.onGlobalExit = NULL,
.entries = menuMiscEntries
};

108
src/main/cms/cms_menu_osd.c
View file

@ -17,34 +17,60 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <ctype.h>
#include "platform.h"
#if defined(OSD) && defined(CMS)
#include "build/version.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "cms/cms_menu_osd.h"
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#if defined(OSD) && defined(CMS)
#include "io/osd.h"
OSD_UINT8_t entryAlarmRssi = {&osdProfile()->rssi_alarm, 5, 90, 5};
OSD_UINT16_t entryAlarmCapacity = {&osdProfile()->cap_alarm, 50, 30000, 50};
OSD_UINT16_t enryAlarmFlyTime = {&osdProfile()->time_alarm, 1, 200, 1};
OSD_UINT16_t entryAlarmAltitude = {&osdProfile()->alt_alarm, 1, 200, 1};
static uint8_t osdConfig_rssi_alarm;
static uint16_t osdConfig_cap_alarm;
static uint16_t osdConfig_time_alarm;
static uint16_t osdConfig_alt_alarm;
static long cmsx_menuAlarmsOnEnter(void)
{
osdConfig_rssi_alarm = osdConfig()->rssi_alarm;
osdConfig_cap_alarm = osdConfig()->cap_alarm;
osdConfig_time_alarm = osdConfig()->time_alarm;
osdConfig_alt_alarm = osdConfig()->alt_alarm;
return 0;
}
static long cmsx_menuAlarmsOnExit(const OSD_Entry *self)
{
UNUSED(self);
osdConfigMutable()->rssi_alarm = osdConfig_rssi_alarm;
osdConfigMutable()->cap_alarm = osdConfig_cap_alarm;
osdConfigMutable()->time_alarm = osdConfig_time_alarm;
osdConfigMutable()->alt_alarm = osdConfig_alt_alarm;
return 0;
}
OSD_Entry cmsx_menuAlarmsEntries[] =
{
{"--- ALARMS ---", OME_Label, NULL, NULL, 0},
{"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0},
{"MAIN BAT", OME_UINT16, NULL, &entryAlarmCapacity, 0},
{"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, 0},
{"MAX ALT", OME_UINT16, NULL, &entryAlarmAltitude, 0},
{"RSSI", OME_UINT8, NULL, &(OSD_UINT8_t){&osdConfig_rssi_alarm, 5, 90, 5}, 0},
{"MAIN BAT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_cap_alarm, 50, 30000, 50}, 0},
{"FLY TIME", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_time_alarm, 1, 200, 1}, 0},
{"MAX ALT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_alt_alarm, 1, 200, 1}, 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
@ -52,38 +78,54 @@ OSD_Entry cmsx_menuAlarmsEntries[] =
CMS_Menu cmsx_menuAlarms = {
.GUARD_text = "MENUALARMS",
.GUARD_type = OME_MENU,
.onEnter = NULL,
.onExit = NULL,
.onEnter = cmsx_menuAlarmsOnEnter,
.onExit = cmsx_menuAlarmsOnExit,
.onGlobalExit = NULL,
.entries = cmsx_menuAlarmsEntries,
};
static uint16_t osdConfig_item_pos[OSD_ITEM_COUNT];
static long menuOsdActiveElemsOnEnter(void)
{
memcpy(&osdConfig_item_pos[0], &osdConfig()->item_pos[0], sizeof(uint16_t) * OSD_ITEM_COUNT);
return 0;
}
static long menuOsdActiveElemsOnExit(const OSD_Entry *self)
{
UNUSED(self);
memcpy(&osdConfigMutable()->item_pos[0], &osdConfig_item_pos[0], sizeof(uint16_t) * OSD_ITEM_COUNT);
return 0;
}
OSD_Entry menuOsdActiveElemsEntries[] =
{
{"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0},
{"RSSI", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_RSSI_VALUE], 0},
{"MAIN BATTERY", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], 0},
{"HORIZON", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], 0},
{"HORIZON SIDEBARS", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_HORIZON_SIDEBARS], 0},
{"UPTIME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ONTIME], 0},
{"FLY TIME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_FLYTIME], 0},
{"FLY MODE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_FLYMODE], 0},
{"NAME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CRAFT_NAME], 0},
{"THROTTLE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_THROTTLE_POS], 0},
{"RSSI", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_RSSI_VALUE], 0},
{"MAIN BATTERY", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_MAIN_BATT_VOLTAGE], 0},
{"HORIZON", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ARTIFICIAL_HORIZON], 0},
{"HORIZON SIDEBARS", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_HORIZON_SIDEBARS], 0},
{"UPTIME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ONTIME], 0},
{"FLY TIME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_FLYTIME], 0},
{"FLY MODE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_FLYMODE], 0},
{"NAME", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_CRAFT_NAME], 0},
{"THROTTLE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_THROTTLE_POS], 0},
#ifdef VTX
{"VTX CHAN", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_VTX_CHANNEL], 0},
{"VTX CHAN", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_VTX_CHANNEL], 0},
#endif // VTX
{"CURRENT (A)", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CURRENT_DRAW], 0},
{"USED MAH", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAH_DRAWN], 0},
{"CURRENT (A)", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_CURRENT_DRAW], 0},
{"USED MAH", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_MAH_DRAWN], 0},
#ifdef GPS
{"GPS SPEED", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_GPS_SPEED], 0},
{"GPS SATS.", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_GPS_SATS], 0},
{"GPS SPEED", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_GPS_SPEED], 0},
{"GPS SATS.", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_GPS_SATS], 0},
#endif // GPS
{"ALTITUDE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ALTITUDE], 0},
{"POWER", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_POWER], 0},
{"ROLL PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ROLL_PIDS], 0},
{"PITCH PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_PITCH_PIDS], 0},
{"YAW PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_YAW_PIDS], 0},
{"ALTITUDE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ALTITUDE], 0},
{"POWER", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_POWER], 0},
{"ROLL PID", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ROLL_PIDS], 0},
{"PITCH PID", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PITCH_PIDS], 0},
{"YAW PID", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_YAW_PIDS], 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
@ -91,8 +133,8 @@ OSD_Entry menuOsdActiveElemsEntries[] =
CMS_Menu menuOsdActiveElems = {
.GUARD_text = "MENUOSDACT",
.GUARD_type = OME_MENU,
.onEnter = NULL,
.onExit = NULL,
.onEnter = menuOsdActiveElemsOnEnter,
.onExit = menuOsdActiveElemsOnExit,
.onGlobalExit = NULL,
.entries = menuOsdActiveElemsEntries
};

11
src/main/cms/cms_menu_vtx.c
View file

@ -21,6 +21,10 @@
#include "platform.h"
#ifdef CMS
#if defined(VTX) || defined(USE_RTC6705)
#include "build/version.h"
#include "cms/cms.h"
@ -30,12 +34,9 @@
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef CMS
#if defined(VTX) || defined(USE_RTC6705)
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
static bool featureRead = false;
static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel;

0
src/main/flight/gps_conversion.c → src/main/common/gps_conversion.c
View file

0
src/main/flight/gps_conversion.h → src/main/common/gps_conversion.h
View file

4
src/main/common/maths.h
View file

@ -30,6 +30,10 @@
#define M_PIf 3.14159265358979323846f
#define RAD (M_PIf / 180.0f)
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))

471
src/main/config/config_eeprom.c
View file

@ -18,294 +18,285 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <stddef.h>
#include "platform.h"
#include "drivers/system.h"
#include "config/config_master.h"
#include "build/build_config.h"
#include "common/maths.h"
#include "config/config_eeprom.h"
#include "config/config_streamer.h"
#include "config/config_master.h"
#include "config/parameter_group.h"
#if !defined(FLASH_SIZE)
#error "Flash size not defined for target. (specify in KB)"
#endif
#include "drivers/system.h"
#include "fc/config.h"
#ifndef FLASH_PAGE_SIZE
#ifdef STM32F303xC
#define FLASH_PAGE_SIZE ((uint16_t)0x800)
#endif
// declare a dummy PG, since scanEEPROM assumes there is at least one PG
// !!TODO remove once first PG has been created out of masterConfg
typedef struct dummpConfig_s {
uint8_t dummy;
} dummyConfig_t;
PG_DECLARE(dummyConfig_t, dummyConfig);
#define PG_DUMMY_CONFIG 1
PG_REGISTER(dummyConfig_t, dummyConfig, PG_DUMMY_CONFIG, 0);
#ifdef STM32F10X_MD
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
#endif
extern uint8_t __config_start; // configured via linker script when building binaries.
extern uint8_t __config_end;
#ifdef STM32F10X_HD
#define FLASH_PAGE_SIZE ((uint16_t)0x800)
#endif
static uint16_t eepromConfigSize;
#if defined(STM32F745xx)
#define FLASH_PAGE_SIZE ((uint32_t)0x40000)
#endif
typedef enum {
CR_CLASSICATION_SYSTEM = 0,
CR_CLASSICATION_PROFILE1 = 1,
CR_CLASSICATION_PROFILE2 = 2,
CR_CLASSICATION_PROFILE3 = 3,
CR_CLASSICATION_PROFILE_LAST = CR_CLASSICATION_PROFILE3,
} configRecordFlags_e;
#if defined(STM32F746xx)
#define FLASH_PAGE_SIZE ((uint32_t)0x40000)
#endif
#define CR_CLASSIFICATION_MASK (0x3)
#if defined(STM32F722xx)
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#endif
// Header for the saved copy.
typedef struct {
uint8_t format;
} PG_PACKED configHeader_t;
#if defined(STM32F40_41xxx)
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#endif
// Header for each stored PG.
typedef struct {
// split up.
uint16_t size;
pgn_t pgn;
uint8_t version;
#if defined (STM32F411xE)
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#endif
// lower 2 bits used to indicate system or profile number, see CR_CLASSIFICATION_MASK
uint8_t flags;
#endif
uint8_t pg[];
} PG_PACKED configRecord_t;
#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT)
#ifdef STM32F10X_MD
#define FLASH_PAGE_COUNT 128
#endif
#ifdef STM32F10X_HD
#define FLASH_PAGE_COUNT 128
#endif
#endif
#if defined(FLASH_SIZE)
#if defined(STM32F40_41xxx)
#define FLASH_PAGE_COUNT 4
#elif defined (STM32F411xE)
#define FLASH_PAGE_COUNT 3
#elif defined (STM32F722xx)
#define FLASH_PAGE_COUNT 3
#elif defined (STM32F745xx)
#define FLASH_PAGE_COUNT 4
#elif defined (STM32F746xx)
#define FLASH_PAGE_COUNT 4
#else
#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE)
#endif
#endif
#if !defined(FLASH_PAGE_SIZE)
#error "Flash page size not defined for target."
#endif
#if !defined(FLASH_PAGE_COUNT)
#error "Flash page count not defined for target."
#endif
#if FLASH_SIZE <= 128
#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
#else
#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000
#endif
// use the last flash pages for storage
#ifdef CUSTOM_FLASH_MEMORY_ADDRESS
size_t custom_flash_memory_address = 0;
#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address)
#else
// use the last flash pages for storage
#ifndef CONFIG_START_FLASH_ADDRESS
#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
#endif
#endif
// Footer for the saved copy.
typedef struct {
uint16_t terminator;
} PG_PACKED configFooter_t;
// checksum is appended just after footer. It is not included in footer to make checksum calculation consistent
// Used to check the compiler packing at build time.
typedef struct {
uint8_t byte;
uint32_t word;
} PG_PACKED packingTest_t;
void initEEPROM(void)
{
// Verify that this architecture packs as expected.
BUILD_BUG_ON(offsetof(packingTest_t, byte) != 0);
BUILD_BUG_ON(offsetof(packingTest_t, word) != 1);
BUILD_BUG_ON(sizeof(packingTest_t) != 5);
BUILD_BUG_ON(sizeof(configHeader_t) != 1);
BUILD_BUG_ON(sizeof(configFooter_t) != 2);
BUILD_BUG_ON(sizeof(configRecord_t) != 6);
}
static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
static uint8_t updateChecksum(uint8_t chk, const void *data, uint32_t length)
{
uint8_t checksum = 0;
const uint8_t *byteOffset;
const uint8_t *p = (const uint8_t *)data;
const uint8_t *pend = p + length;
for (byteOffset = data; byteOffset < (data + length); byteOffset++)
checksum ^= *byteOffset;
return checksum;
for (; p != pend; p++) {
chk ^= *p;
}
return chk;
}
// Scan the EEPROM config. Returns true if the config is valid.
bool isEEPROMContentValid(void)
{
const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS;
uint8_t checksum = 0;
uint8_t chk = 0;
const uint8_t *p = &__config_start;
const configHeader_t *header = (const configHeader_t *)p;
// check version number
if (EEPROM_CONF_VERSION != temp->version)
if (header->format != EEPROM_CONF_VERSION) {
return false;
}
chk = updateChecksum(chk, header, sizeof(*header));
p += sizeof(*header);
// include the transitional masterConfig record
chk = updateChecksum(chk, p, sizeof(masterConfig));
p += sizeof(masterConfig);
// check size and magic numbers
if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
return false;
for (;;) {
const configRecord_t *record = (const configRecord_t *)p;
if (strncasecmp(temp->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER)))
return false;
if (record->size == 0) {
// Found the end. Stop scanning.
break;
}
if (p + record->size >= &__config_end
|| record->size < sizeof(*record)) {
// Too big or too small.
return false;
}
// verify integrity of temporary copy
checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
if (checksum != 0)
return false;
chk = updateChecksum(chk, p, record->size);
// looks good, let's roll!
p += record->size;
}
const configFooter_t *footer = (const configFooter_t *)p;
chk = updateChecksum(chk, footer, sizeof(*footer));
p += sizeof(*footer);
chk = ~chk;
const uint8_t checkSum = *p;
p += sizeof(checkSum);
eepromConfigSize = p - &__config_start;
return chk == checkSum;
}
uint16_t getEEPROMConfigSize(void)
{
return eepromConfigSize;
}
// find config record for reg + classification (profile info) in EEPROM
// return NULL when record is not found
// this function assumes that EEPROM content is valid
static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFlags_e classification)
{
const uint8_t *p = &__config_start;
p += sizeof(configHeader_t); // skip header
p += sizeof(master_t); // skip the transitional master_t record
while (true) {
const configRecord_t *record = (const configRecord_t *)p;
if (record->size == 0
|| p + record->size >= &__config_end
|| record->size < sizeof(*record))
break;
if (pgN(reg) == record->pgn
&& (record->flags & CR_CLASSIFICATION_MASK) == classification)
return record;
p += record->size;
}
// record not found
return NULL;
}
// Initialize all PG records from EEPROM.
// This functions processes all PGs sequentially, scanning EEPROM for each one. This is suboptimal,
// but each PG is loaded/initialized exactly once and in defined order.
bool loadEEPROM(void)
{
// read in the transitional masterConfig record
const uint8_t *p = &__config_start;
p += sizeof(configHeader_t); // skip header
masterConfig = *(master_t*)p;
PG_FOREACH(reg) {
configRecordFlags_e cls_start, cls_end;
if (pgIsSystem(reg)) {
cls_start = CR_CLASSICATION_SYSTEM;
cls_end = CR_CLASSICATION_SYSTEM;
} else {
cls_start = CR_CLASSICATION_PROFILE1;
cls_end = CR_CLASSICATION_PROFILE_LAST;
}
for (configRecordFlags_e cls = cls_start; cls <= cls_end; cls++) {
int profileIndex = cls - cls_start;
const configRecord_t *rec = findEEPROM(reg, cls);
if (rec) {
// config from EEPROM is available, use it to initialize PG. pgLoad will handle version mismatch
pgLoad(reg, profileIndex, rec->pg, rec->size - offsetof(configRecord_t, pg), rec->version);
} else {
pgReset(reg, profileIndex);
}
}
}
return true;
}
#if defined(STM32F7)
// FIXME: HAL for now this will only work for F4/F7 as flash layout is different
void writeEEPROM(void)
static bool writeSettingsToEEPROM(void)
{
// Generate compile time error if the config does not fit in the reserved area of flash.
BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
config_streamer_t streamer;
config_streamer_init(&streamer);
HAL_StatusTypeDef status;
uint32_t wordOffset;
int8_t attemptsRemaining = 3;
config_streamer_start(&streamer, (uintptr_t)&__config_start, &__config_end - &__config_start);
uint8_t chk = 0;
suspendRxSignal();
configHeader_t header = {
.format = EEPROM_CONF_VERSION,
};
// prepare checksum/version constants
masterConfig.version = EEPROM_CONF_VERSION;
masterConfig.size = sizeof(master_t);
masterConfig.magic_be = 0xBE;
masterConfig.magic_ef = 0xEF;
masterConfig.chk = 0; // erase checksum before recalculating
masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header));
chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header));
// write the transitional masterConfig record
config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig));
chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig));
PG_FOREACH(reg) {
const uint16_t regSize = pgSize(reg);
configRecord_t record = {
.size = sizeof(configRecord_t) + regSize,
.pgn = pgN(reg),
.version = pgVersion(reg),
.flags = 0
};
// write it
/* Unlock the Flash to enable the flash control register access *************/
HAL_FLASH_Unlock();
while (attemptsRemaining--)
{
/* Fill EraseInit structure*/
FLASH_EraseInitTypeDef EraseInitStruct = {0};
EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS;
EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V
EraseInitStruct.Sector = (FLASH_SECTOR_TOTAL-1);
EraseInitStruct.NbSectors = 1;
uint32_t SECTORError;
status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
if (status != HAL_OK)
{
continue;
}
else
{
for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4)
{
status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset));
if(status != HAL_OK)
{
break;
}
if (pgIsSystem(reg)) {
// write the only instance
record.flags |= CR_CLASSICATION_SYSTEM;
config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
config_streamer_write(&streamer, reg->address, regSize);
chk = updateChecksum(chk, reg->address, regSize);
} else {
// write one instance for each profile
for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) {
record.flags = 0;
record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK);
config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record));
const uint8_t *address = reg->address + (regSize * profileIndex);
config_streamer_write(&streamer, address, regSize);
chk = updateChecksum(chk, address, regSize);
}
}
if (status == HAL_OK) {
break;
}
configFooter_t footer = {
.terminator = 0,
};
config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer));
chk = updateChecksum(chk, (uint8_t *)&footer, sizeof(footer));
// append checksum now
chk = ~chk;
config_streamer_write(&streamer, &chk, sizeof(chk));
config_streamer_flush(&streamer);
bool success = config_streamer_finish(&streamer) == 0;
return success;
}
void writeConfigToEEPROM(void)
{
bool success = false;
// write it
for (int attempt = 0; attempt < 3 && !success; attempt++) {
if (writeSettingsToEEPROM()) {
success = true;
}
}
HAL_FLASH_Lock();
if (success && isEEPROMContentValid()) {
return;
}
// Flash write failed - just die now
if (status != HAL_OK || !isEEPROMContentValid()) {
failureMode(FAILURE_FLASH_WRITE_FAILED);
}
resumeRxSignal();
}
#else
void writeEEPROM(void)
{
// Generate compile time error if the config does not fit in the reserved area of flash.
BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
FLASH_Status status = 0;
uint32_t wordOffset;
int8_t attemptsRemaining = 3;
suspendRxSignal();
// prepare checksum/version constants
masterConfig.version = EEPROM_CONF_VERSION;
masterConfig.size = sizeof(master_t);
masterConfig.magic_be = 0xBE;
masterConfig.magic_ef = 0xEF;
masterConfig.chk = 0; // erase checksum before recalculating
masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
// write it
FLASH_Unlock();
while (attemptsRemaining--) {
#if defined(STM32F4)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
#elif defined(STM32F303)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
#elif defined(STM32F10X)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
#endif
for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) {
if (wordOffset % FLASH_PAGE_SIZE == 0) {
#if defined(STM32F40_41xxx)
status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000
#elif defined (STM32F411xE)
status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000
#else
status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
#endif
if (status != FLASH_COMPLETE) {
break;
}
}
status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset,
*(uint32_t *) ((char *) &masterConfig + wordOffset));
if (status != FLASH_COMPLETE) {
break;
}
}
if (status == FLASH_COMPLETE) {
break;
}
}
FLASH_Lock();
// Flash write failed - just die now
if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
failureMode(FAILURE_FLASH_WRITE_FAILED);
}
resumeRxSignal();
}
#endif
void readEEPROM(void)
{
// Sanity check
if (!isEEPROMContentValid())
failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
suspendRxSignal();
// Read flash
memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t));
if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check
masterConfig.current_profile_index = 0;
setProfile(masterConfig.current_profile_index);
validateAndFixConfig();
activateConfig();
resumeRxSignal();
failureMode(FAILURE_FLASH_WRITE_FAILED);
}

9
src/main/config/config_eeprom.h
View file

@ -17,9 +17,12 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#define EEPROM_CONF_VERSION 156
void initEEPROM(void);
void writeEEPROM();
void readEEPROM(void);
bool isEEPROMContentValid(void);
bool loadEEPROM(void);
void writeConfigToEEPROM(void);
uint16_t getEEPROMConfigSize(void);

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

@ -20,6 +20,7 @@
#include <stdlib.h>
#include "config/config_profile.h"
#include "config/feature.h"
#include "blackbox/blackbox.h"
@ -36,17 +37,20 @@
#include "drivers/display.h"
#include "drivers/serial.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/fc_core.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/gps.h"
#include "io/osd.h"
@ -65,6 +69,9 @@
#include "sensors/battery.h"
#include "sensors/compass.h"
#ifndef USE_PARAMETER_GROUPS
#define featureConfig(x) (&masterConfig.featureConfig)
#define systemConfig(x) (&masterConfig.systemConfig)
#define motorConfig(x) (&masterConfig.motorConfig)
#define flight3DConfig(x) (&masterConfig.flight3DConfig)
#define servoConfig(x) (&masterConfig.servoConfig)
@ -94,11 +101,11 @@
#define ppmConfig(x) (&masterConfig.ppmConfig)
#define pwmConfig(x) (&masterConfig.pwmConfig)
#define adcConfig(x) (&masterConfig.adcConfig)
#define beeperConfig(x) (&masterConfig.beeperConfig)
#define beeperDevConfig(x) (&masterConfig.beeperDevConfig)
#define sonarConfig(x) (&masterConfig.sonarConfig)
#define ledStripConfig(x) (&masterConfig.ledStripConfig)
#define statusLedConfig(x) (&masterConfig.statusLedConfig)
#define osdProfile(x) (&masterConfig.osdProfile)
#define osdConfig(x) (&masterConfig.osdProfile)
#define vcdProfile(x) (&masterConfig.vcdProfile)
#define sdcardConfig(x) (&masterConfig.sdcardConfig)
#define blackboxConfig(x) (&masterConfig.blackboxConfig)
@ -108,18 +115,82 @@
#define modeActivationProfile(x) (&masterConfig.modeActivationProfile)
#define servoProfile(x) (&masterConfig.servoProfile)
#define customMotorMixer(i) (&masterConfig.customMotorMixer[i])
#define customServoMixer(i) (&masterConfig.customServoMixer[i])
#define customServoMixers(i) (&masterConfig.customServoMixer[i])
#define displayPortProfileMsp(x) (&masterConfig.displayPortProfileMsp)
#define displayPortProfileMax7456(x) (&masterConfig.displayPortProfileMax7456)
#define displayPortProfileOled(x) (&masterConfig.displayPortProfileOled)
#define featureConfigMutable(x) (&masterConfig.featureConfig)
#define systemConfigMutable(x) (&masterConfig.systemConfig)
#define motorConfigMutable(x) (&masterConfig.motorConfig)
#define flight3DConfigMutable(x) (&masterConfig.flight3DConfig)
#define servoConfigMutable(x) (&masterConfig.servoConfig)
#define servoMixerConfigMutable(x) (&masterConfig.servoMixerConfig)
#define gimbalConfigMutable(x) (&masterConfig.gimbalConfig)
#define boardAlignmentMutable(x) (&masterConfig.boardAlignment)
#define imuConfigMutable(x) (&masterConfig.imuConfig)
#define gyroConfigMutable(x) (&masterConfig.gyroConfig)
#define compassConfigMutable(x) (&masterConfig.compassConfig)
#define accelerometerConfigMutable(x) (&masterConfig.accelerometerConfig)
#define barometerConfigMutable(x) (&masterConfig.barometerConfig)
#define throttleCorrectionConfigMutable(x) (&masterConfig.throttleCorrectionConfig)
#define batteryConfigMutable(x) (&masterConfig.batteryConfig)
#define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig)
#define gpsProfileMutable(x) (&masterConfig.gpsProfile)
#define gpsConfigMutable(x) (&masterConfig.gpsConfig)
#define rxConfigMutable(x) (&masterConfig.rxConfig)
#define armingConfigMutable(x) (&masterConfig.armingConfig)
#define mixerConfigMutable(x) (&masterConfig.mixerConfig)
#define airplaneConfigMutable(x) (&masterConfig.airplaneConfig)
#define failsafeConfigMutable(x) (&masterConfig.failsafeConfig)
#define serialConfigMutable(x) (&masterConfig.serialConfig)
#define telemetryConfigMutable(x) (&masterConfig.telemetryConfig)
#define ibusTelemetryConfigMutable(x) (&masterConfig.telemetryConfig)
#define ppmConfigMutable(x) (&masterConfig.ppmConfig)
#define pwmConfigMutable(x) (&masterConfig.pwmConfig)
#define adcConfigMutable(x) (&masterConfig.adcConfig)
#define beeperDevConfigMutable(x) (&masterConfig.beeperDevConfig)
#define sonarConfigMutable(x) (&masterConfig.sonarConfig)
#define ledStripConfigMutable(x) (&masterConfig.ledStripConfig)
#define statusLedConfigMutable(x) (&masterConfig.statusLedConfig)
#define osdProfileMutable(x) (&masterConfig.osdProfile)
#define vcdProfileMutable(x) (&masterConfig.vcdProfile)
#define sdcardConfigMutable(x) (&masterConfig.sdcardConfig)
#define blackboxConfigMutable(x) (&masterConfig.blackboxConfig)
#define flashConfigMutable(x) (&masterConfig.flashConfig)
#define pidConfigMutable(x) (&masterConfig.pidConfig)
#define adjustmentProfileMutable(x) (&masterConfig.adjustmentProfile)
#define modeActivationProfileMutable(x) (&masterConfig.modeActivationProfile)
#define servoProfileMutable(x) (&masterConfig.servoProfile)
#define customMotorMixerMutable(i) (&masterConfig.customMotorMixer[i])
#define customServoMixersMutable(i) (&masterConfig.customServoMixer[i])
#define displayPortProfileMspMutable(x) (&masterConfig.displayPortProfileMsp)
#define displayPortProfileMax7456Mutable(x) (&masterConfig.displayPortProfileMax7456)
#define displayPortProfileOledMutable(x) (&masterConfig.displayPortProfileOled)
#define servoParams(x) (&servoProfile()->servoConf[x])
#define adjustmentRanges(x) (&adjustmentProfile()->adjustmentRanges[x])
#define rxFailsafeChannelConfigs(x) (&masterConfig.rxConfig.failsafe_channel_configurations[x])
#define osdConfig(x) (&masterConfig.osdProfile)
#define modeActivationConditions(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x])
#define servoParamsMutable(x) (&servoProfile()->servoConf[x])
#define adjustmentRangesMutable(x) (&masterConfig.adjustmentProfile.adjustmentRanges[x])
#define rxFailsafeChannelConfigsMutable(x) (&masterConfig.rxConfig.>failsafe_channel_configurations[x])
#define osdConfigMutable(x) (&masterConfig.osdProfile)
#define modeActivationConditionsMutable(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x])
#endif
// System-wide
typedef struct master_s {
uint8_t version;
uint16_t size;
uint8_t magic_be; // magic number, should be 0xBE
uint32_t enabledFeatures;
featureConfig_t featureConfig;
systemConfig_t systemConfig;
// motor/esc/servo related stuff
motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
@ -128,7 +199,6 @@ typedef struct master_s {
#ifdef USE_SERVOS
servoConfig_t servoConfig;
servoMixerConfig_t servoMixerConfig;
servoMixer_t customServoMixer[MAX_SERVO_RULES];
// Servo-related stuff
servoProfile_t servoProfile;
@ -144,7 +214,6 @@ typedef struct master_s {
pidConfig_t pidConfig;
uint8_t debug_mode; // Processing denominator for PID controller vs gyro sampling rate
uint8_t task_statistics;
gyroConfig_t gyroConfig;
@ -194,7 +263,7 @@ typedef struct master_s {
#endif
#ifdef BEEPER
beeperConfig_t beeperConfig;
beeperDevConfig_t beeperDevConfig;
#endif
#ifdef SONAR
@ -258,7 +327,6 @@ typedef struct master_s {
uint32_t beeper_off_flags;
uint32_t preferred_beeper_off_flags;
char name[MAX_NAME_LENGTH + 1];
char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER)];
uint8_t magic_ef; // magic number, should be 0xEF
@ -270,7 +338,5 @@ typedef struct master_s {
} master_t;
extern master_t masterConfig;
extern profile_t *currentProfile;
extern controlRateConfig_t *currentControlRateProfile;
void createDefaultConfig(master_t *config);

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

@ -17,7 +17,6 @@
#pragma once
#include "common/axis.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "flight/pid.h"

40
src/main/config/config_reset.h Normal file
View file

@ -0,0 +1,40 @@
/*
* 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
#ifndef __UNIQL
# define __UNIQL_CONCAT2(x,y) x ## y
# define __UNIQL_CONCAT(x,y) __UNIQL_CONCAT2(x,y)
# define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__)
#endif
// overwrite _name with data passed as arguments. This version forces GCC to really copy data
// It is not possible to use multiple RESET_CONFIGs on single line (__UNIQL limitation)
#define RESET_CONFIG(_type, _name, ...) \
static const _type __UNIQL(_reset_template_) = { \
__VA_ARGS__ \
}; \
memcpy((_name), &__UNIQL(_reset_template_), sizeof(*(_name))); \
/**/
// overwrite _name with data passed as arguments. GCC is allowed to set structure field-by-field
#define RESET_CONFIG_2(_type, _name, ...) \
*(_name) = (_type) { \
__VA_ARGS__ \
}; \
/**/

263
src/main/config/config_streamer.c Normal file
View file

@ -0,0 +1,263 @@
/*
* 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 <string.h>
#include "platform.h"
#include "drivers/system.h"
#include "config/config_streamer.h"
extern uint8_t __config_start; // configured via linker script when building binaries.
extern uint8_t __config_end;
#if !defined(FLASH_PAGE_SIZE)
// F1
# if defined(STM32F10X_MD)
# define FLASH_PAGE_SIZE (0x400)
# elif defined(STM32F10X_HD)
# define FLASH_PAGE_SIZE (0x800)
// F3
# elif defined(STM32F303xC)
# define FLASH_PAGE_SIZE (0x800)
// F4
# elif defined(STM32F40_41xxx)
# define FLASH_PAGE_SIZE ((uint32_t)0x20000)
# elif defined (STM32F411xE)
# define FLASH_PAGE_SIZE ((uint32_t)0x20000)
# elif defined(STM32F427_437xx)
# define FLASH_PAGE_SIZE ((uint32_t)0x20000) // 128K sectors
// F7
#elif defined(STM32F722xx)
# define FLASH_PAGE_SIZE ((uint32_t)0x20000)
# elif defined(STM32F745xx)
# define FLASH_PAGE_SIZE ((uint32_t)0x40000)
# elif defined(STM32F746xx)
# define FLASH_PAGE_SIZE ((uint32_t)0x40000)
# elif defined(UNIT_TEST)
# define FLASH_PAGE_SIZE (0x400)
# else
# error "Flash page size not defined for target."
# endif
#endif
void config_streamer_init(config_streamer_t *c)
{
memset(c, 0, sizeof(*c));
}
void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
{
// base must start at FLASH_PAGE_SIZE boundary
c->address = base;
c->size = size;
if (!c->unlocked) {
#if defined(STM32F7)
HAL_FLASH_Unlock();
#else
FLASH_Unlock();
#endif
c->unlocked = true;
}
#if defined(STM32F10X)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
#elif defined(STM32F303)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
#elif defined(STM32F4)
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
#elif defined(STM32F7)
// NOP
#elif defined(UNIT_TEST)
// NOP
#else
# error "Unsupported CPU"
#endif
c->err = 0;
}
#if defined(STM32F7)
/*
Sector 0 0x08000000 - 0x08007FFF 32 Kbytes
Sector 1 0x08008000 - 0x0800FFFF 32 Kbytes
Sector 2 0x08010000 - 0x08017FFF 32 Kbytes
Sector 3 0x08018000 - 0x0801FFFF 32 Kbytes
Sector 4 0x08020000 - 0x0803FFFF 128 Kbytes
Sector 5 0x08040000 - 0x0807FFFF 256 Kbytes
Sector 6 0x08080000 - 0x080BFFFF 256 Kbytes
Sector 7 0x080C0000 - 0x080FFFFF 256 Kbytes
*/
static uint32_t getFLASHSectorForEEPROM(void)
{
if ((uint32_t)&__config_start <= 0x08007FFF)
return FLASH_SECTOR_0;
if ((uint32_t)&__config_start <= 0x0800FFFF)
return FLASH_SECTOR_1;
if ((uint32_t)&__config_start <= 0x08017FFF)
return FLASH_SECTOR_2;
if ((uint32_t)&__config_start <= 0x0801FFFF)
return FLASH_SECTOR_3;
if ((uint32_t)&__config_start <= 0x0803FFFF)
return FLASH_SECTOR_4;
if ((uint32_t)&__config_start <= 0x0807FFFF)
return FLASH_SECTOR_5;
if ((uint32_t)&__config_start <= 0x080BFFFF)
return FLASH_SECTOR_6;
if ((uint32_t)&__config_start <= 0x080FFFFF)
return FLASH_SECTOR_7;
// Not good
while (1) {
failureMode(FAILURE_FLASH_WRITE_FAILED);
}
}
#elif defined(STM32F4)
/*
Sector 0 0x08000000 - 0x08003FFF 16 Kbytes
Sector 1 0x08004000 - 0x08007FFF 16 Kbytes
Sector 2 0x08008000 - 0x0800BFFF 16 Kbytes
Sector 3 0x0800C000 - 0x0800FFFF 16 Kbytes
Sector 4 0x08010000 - 0x0801FFFF 64 Kbytes
Sector 5 0x08020000 - 0x0803FFFF 128 Kbytes
Sector 6 0x08040000 - 0x0805FFFF 128 Kbytes
Sector 7 0x08060000 - 0x0807FFFF 128 Kbytes
Sector 8 0x08080000 - 0x0809FFFF 128 Kbytes
Sector 9 0x080A0000 - 0x080BFFFF 128 Kbytes
Sector 10 0x080C0000 - 0x080DFFFF 128 Kbytes
Sector 11 0x080E0000 - 0x080FFFFF 128 Kbytes
*/
static uint32_t getFLASHSectorForEEPROM(void)
{
if ((uint32_t)&__config_start <= 0x08003FFF)
return FLASH_Sector_0;
if ((uint32_t)&__config_start <= 0x08007FFF)
return FLASH_Sector_1;
if ((uint32_t)&__config_start <= 0x0800BFFF)
return FLASH_Sector_2;
if ((uint32_t)&__config_start <= 0x0800FFFF)
return FLASH_Sector_3;
if ((uint32_t)&__config_start <= 0x0801FFFF)
return FLASH_Sector_4;
if ((uint32_t)&__config_start <= 0x0803FFFF)
return FLASH_Sector_5;
if ((uint32_t)&__config_start <= 0x0805FFFF)
return FLASH_Sector_6;
if ((uint32_t)&__config_start <= 0x0807FFFF)
return FLASH_Sector_7;
if ((uint32_t)&__config_start <= 0x0809FFFF)
return FLASH_Sector_8;
if ((uint32_t)&__config_start <= 0x080DFFFF)
return FLASH_Sector_9;
if ((uint32_t)&__config_start <= 0x080BFFFF)
return FLASH_Sector_10;
if ((uint32_t)&__config_start <= 0x080FFFFF)
return FLASH_Sector_11;
// Not good
while (1) {
failureMode(FAILURE_FLASH_WRITE_FAILED);
}
}
#endif
static int write_word(config_streamer_t *c, uint32_t value)
{
if (c->err != 0) {
return c->err;
}
#if defined(STM32F7)
if (c->address % FLASH_PAGE_SIZE == 0) {
FLASH_EraseInitTypeDef EraseInitStruct = {
.TypeErase = FLASH_TYPEERASE_SECTORS,
.VoltageRange = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V
.NbSectors = 1
};
EraseInitStruct.Sector = getFLASHSectorForEEPROM();
uint32_t SECTORError;
const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
if (status != HAL_OK){
return -1;
}
}
const HAL_StatusTypeDef status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value);
if (status != HAL_OK) {
return -2;
}
#else
if (c->address % FLASH_PAGE_SIZE == 0) {
#if defined(STM32F4)
const FLASH_Status status = FLASH_EraseSector(getFLASHSectorForEEPROM(), VoltageRange_3); //0x08080000 to 0x080A0000
#else
const FLASH_Status status = FLASH_ErasePage(c->address);
#endif
if (status != FLASH_COMPLETE) {
return -1;
}
}
const FLASH_Status status = FLASH_ProgramWord(c->address, value);
if (status != FLASH_COMPLETE) {
return -2;
}
#endif
c->address += sizeof(value);
return 0;
}
int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size)
{
for (const uint8_t *pat = p; pat != (uint8_t*)p + size; pat++) {
c->buffer.b[c->at++] = *pat;
if (c->at == sizeof(c->buffer)) {
c->err = write_word(c, c->buffer.w);
c->at = 0;
}
}
return c->err;
}
int config_streamer_status(config_streamer_t *c)
{
return c->err;
}
int config_streamer_flush(config_streamer_t *c)
{
if (c->at != 0) {
memset(c->buffer.b + c->at, 0, sizeof(c->buffer) - c->at);
c->err = write_word(c, c->buffer.w);
c->at = 0;
}
return c-> err;
}
int config_streamer_finish(config_streamer_t *c)
{
if (c->unlocked) {
#if defined(STM32F7)
HAL_FLASH_Lock();
#else
FLASH_Lock();
#endif
c->unlocked = false;
}
return c->err;
}

45
src/main/config/config_streamer.h Normal file
View file

@ -0,0 +1,45 @@
/*
* 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 <stdint.h>
#include <stdbool.h>
// Streams data out to the EEPROM, padding to the write size as
// needed, and updating the checksum as it goes.
typedef struct config_streamer_s {
uintptr_t address;
int size;
union {
uint8_t b[4];
uint32_t w;
} buffer;
int at;
int err;
bool unlocked;
} config_streamer_t;
void config_streamer_init(config_streamer_t *c);
void config_streamer_start(config_streamer_t *c, uintptr_t base, int size);
int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size);
int config_streamer_flush(config_streamer_t *c);
int config_streamer_finish(config_streamer_t *c);
int config_streamer_status(config_streamer_t *c);

14
src/main/config/feature.c
View file

@ -23,6 +23,8 @@
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
static uint32_t activeFeaturesLatch = 0;
@ -44,12 +46,12 @@ void intFeatureClearAll(uint32_t *features)
void latchActiveFeatures()
{
activeFeaturesLatch = masterConfig.enabledFeatures;
activeFeaturesLatch = featureConfig()->enabledFeatures;
}
bool featureConfigured(uint32_t mask)
{
return masterConfig.enabledFeatures & mask;
return featureConfig()->enabledFeatures & mask;
}
bool feature(uint32_t mask)
@ -59,22 +61,22 @@ bool feature(uint32_t mask)
void featureSet(uint32_t mask)
{
intFeatureSet(mask, &masterConfig.enabledFeatures);
intFeatureSet(mask, &featureConfigMutable()->enabledFeatures);
}
void featureClear(uint32_t mask)
{
intFeatureClear(mask, &masterConfig.enabledFeatures);
intFeatureClear(mask, &featureConfigMutable()->enabledFeatures);
}
void featureClearAll()
{
intFeatureClearAll(&masterConfig.enabledFeatures);
intFeatureClearAll(&featureConfigMutable()->enabledFeatures);
}
uint32_t featureMask(void)
{
return masterConfig.enabledFeatures;
return featureConfig()->enabledFeatures;
}

8
src/main/config/feature.h
View file

@ -17,6 +17,14 @@
#pragma once
#include "config/parameter_group.h"
typedef struct featureConfig_s {
uint32_t enabledFeatures;
} featureConfig_t;
PG_DECLARE(featureConfig_t, featureConfig);
void latchActiveFeatures(void);
bool featureConfigured(uint32_t mask);
bool feature(uint32_t mask);

3
src/main/config/parameter_group.c
View file

@ -19,6 +19,8 @@
#include <string.h>
#include <stdint.h>
#include "platform.h"
#include "parameter_group.h"
#include "common/maths.h"
@ -122,4 +124,3 @@ void pgActivateProfile(int profileIndex)
}
}
}

69
src/main/config/parameter_group.h
View file

@ -20,6 +20,8 @@
#include <stdint.h>
#include <stdbool.h>
#include "build/build_config.h"
typedef uint16_t pgn_t;
// parameter group registry flags
@ -54,6 +56,7 @@ static inline uint16_t pgN(const pgRegistry_t* reg) {return reg->pgn & PGR_PGN_M
static inline uint8_t pgVersion(const pgRegistry_t* reg) {return reg->pgn >> 12;}
static inline uint16_t pgSize(const pgRegistry_t* reg) {return reg->size & PGR_SIZE_MASK;}
static inline uint16_t pgIsSystem(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == 0;}
static inline uint16_t pgIsProfile(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == PGR_SIZE_PROFILE_FLAG;}
#define PG_PACKED __attribute__((packed))
@ -97,28 +100,47 @@ extern const uint8_t __pg_resetdata_end[];
} while(0) \
/**/
#ifdef USE_PARAMETER_GROUPS
// Declare system config
#define PG_DECLARE(_type, _name) \
extern _type _name ## _System; \
static inline _type* _name(void) { return &_name ## _System; } \
static inline const _type* _name(void) { return &_name ## _System; }\
static inline _type* _name ## Mutable(void) { return &_name ## _System; }\
struct _dummy \
/**/
// Declare system config array
#define PG_DECLARE_ARR(_type, _size, _name) \
#define PG_DECLARE_ARRAY(_type, _size, _name) \
extern _type _name ## _SystemArray[_size]; \
static inline _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \
static inline const _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
static inline _type* _name ## Mutable(int _index) { return &_name ## _SystemArray[_index]; } \
static inline _type (* _name ## _array(void))[_size] { return &_name ## _SystemArray; } \
struct _dummy \
/**/
// Declare profile config
#define PG_DECLARE_PROFILE(_type, _name) \
extern _type *_name ## _ProfileCurrent; \
static inline _type* _name(void) { return _name ## _ProfileCurrent; } \
static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \
static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \
struct _dummy \
/**/
#else
#define PG_DECLARE(_type, _name) \
extern _type _name ## _System
#define PG_DECLARE_ARRAY(_type, _size, _name) \
extern _type _name ## _SystemArray[_size]
// Declare profile config
#define PG_DECLARE_PROFILE(_type, _name) \
extern _type *_name ## _ProfileCurrent
#endif // USE_PARAMETER_GROUPS
#ifdef USE_PARAMETER_GROUPS
// Register system config
#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \
_type _name ## _System; \
@ -148,7 +170,7 @@ extern const uint8_t __pg_resetdata_end[];
/**/
// Register system config array
#define PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, _reset) \
#define PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, _reset) \
_type _name ## _SystemArray[_size]; \
extern const pgRegistry_t _name ##_Registry; \
const pgRegistry_t _name ## _Registry PG_REGISTER_ATTRIBUTES = { \
@ -160,20 +182,20 @@ extern const uint8_t __pg_resetdata_end[];
} \
/**/
#define PG_REGISTER_ARR(_type, _size, _name, _pgn, _version) \
PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \
#define PG_REGISTER_ARRAY(_type, _size, _name, _pgn, _version) \
PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \
/**/
#define PG_REGISTER_ARR_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \
#define PG_REGISTER_ARRAY_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \
extern void pgResetFn_ ## _name(_type *); \
PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \
PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \
/**/
#if 0
// ARRAY reset mechanism is not implemented yet, only few places in code would benefit from it - See pgResetInstance
#define PG_REGISTER_ARR_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \
#define PG_REGISTER_ARRAY_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \
extern const _type pgResetTemplate_ ## _name; \
PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
/**/
#endif
@ -221,6 +243,29 @@ extern const uint8_t __pg_resetdata_end[];
__VA_ARGS__ \
} \
/**/
#else
#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \
_type _name ## _System
#define PG_REGISTER(_type, _name, _pgn, _version) \
PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \
/**/
#define PG_REGISTER_WITH_RESET_FN(_type, _name, _pgn, _version) \
extern void pgResetFn_ ## _name(_type *); \
PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name }) \
/**/
#define PG_REGISTER_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \
extern const _type pgResetTemplate_ ## _name; \
PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \
/**/
#define PG_RESET_TEMPLATE(_type, _name, ...) \
const _type pgResetTemplate_ ## _name PG_RESETDATA_ATTRIBUTES = { \
__VA_ARGS__ \
} \
/**/
#endif
const pgRegistry_t* pgFind(pgn_t pgn);

29
src/main/config/parameter_group_ids.h
View file

@ -15,8 +15,12 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef USE_PARAMETER_GROUPS
#include "config/config_master.h"
#endif
// FC configuration
#define PG_FAILSAFE_CONFIG 1 // strruct OK
#define PG_FAILSAFE_CONFIG 1 // struct OK
#define PG_BOARD_ALIGNMENT 2 // struct OK
#define PG_GIMBAL_CONFIG 3 // struct OK
#define PG_MOTOR_MIXER 4 // two structs mixerConfig_t servoMixerConfig_t
@ -51,25 +55,38 @@
#define PG_NAVIGATION_CONFIG 34 // structs OK
#define PG_ACCELEROMETER_CONFIG 35 // no accelerometerConfig_t in betaflight
#define PG_RATE_PROFILE_SELECTION 36 // part of profile in betaflight
#define PG_ADJUSTMENT_PROFILE 37 // array needs to be made into struct
//#define PG_ADJUSTMENT_PROFILE 37 // array needs to be made into struct
#define PG_ADJUSTMENT_RANGE_CONFIG 37
#define PG_BAROMETER_CONFIG 38 // structs OK
#define PG_THROTTLE_CORRECTION_CONFIG 39
#define PG_COMPASS_CONFIGURATION 40 // structs OK
#define PG_COMPASS_CONFIG 40 // structs OK
#define PG_MODE_ACTIVATION_PROFILE 41 // array needs to be made into struct
#define PG_SERVO_PROFILE 42
#define PG_FAILSAFE_CHANNEL_CONFIG 43 // structs OK
#define PG_CHANNEL_RANGE_CONFIG 44 // structs OK
//#define PG_SERVO_PROFILE 42
#define PG_SERVO_PARAMS 42
//#define PG_FAILSAFE_CHANNEL_CONFIG 43 // structs OK
#define PG_RX_FAILSAFE_CHANNEL_CONFIG 43
//#define PG_CHANNEL_RANGE_CONFIG 44 // structs OK
#define PG_RX_CHANNEL_RANGE_CONFIG 44
#define PG_MODE_COLOR_CONFIG 45 // part of led strip, structs OK
#define PG_SPECIAL_COLOR_CONFIG 46 // part of led strip, structs OK
#define PG_PILOT_CONFIG 47 // does not exist in betaflight
#define PG_MSP_SERVER_CONFIG 48 // does not exist in betaflight
#define PG_VOLTAGE_METER_CONFIG 49 // Cleanflight has voltageMeterConfig_t, betaflight has batteryConfig_t
#define PG_AMPERAGE_METER_CONFIG 50 // Cleanflight has amperageMeterConfig_t, betaflight has batteryConfig_t
#define PG_SERVO_CONFIG 52
// Driver configuration
#define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight
#define PG_DRIVER_FLASHCHIP_CONFIG 101 // does not exist in betaflight
// betaflight specific parameter group ids start at 500
#define PG_BETAFLIGHT_START 500
#define PG_MODE_ACTIVATION_OPERATOR_CONFIG 500
#define PG_OSD_CONFIG 501
#define PG_BEEPER_CONFIG 5002
#define PG_BETAFLIGHT_END 1002
// OSD configuration (subject to change)
#define PG_OSD_FONT_CONFIG 2047
#define PG_OSD_VIDEO_CONFIG 2046

4
src/main/drivers/accgyro_fake.c
View file

@ -20,6 +20,8 @@
#include "platform.h"
#ifdef USE_FAKE_GYRO
#include "common/axis.h"
#include "common/utils.h"
@ -27,8 +29,6 @@
#include "accgyro_fake.h"
#ifdef USE_FAKE_GYRO
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
static void fakeGyroInit(gyroDev_t *gyro)

10
src/main/drivers/accgyro_mma845x.c
View file

@ -83,12 +83,10 @@ static uint8_t device_id;
static inline void mma8451ConfigureInterrupt(void)
{
#ifdef NAZE
// PA5 - ACC_INT2 output on NAZE rev3/4 hardware
// NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
// OLIMEXINO - The PA5 pin is wired up to LED1, if you need to use an mma8452 on an Olimexino use a different pin and provide support in code.
IOInit(IOGetByTag(IO_TAG(PA5)), OWNER_MPU_EXTI, 0);
IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
#ifdef MMA8451_INT_PIN
IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0);
// TODO - maybe pullup / pulldown ?
IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING);
#endif
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)

236
src/main/drivers/accgyro_mpu.c
View file

@ -48,16 +48,7 @@
#include "accgyro_mpu.h"
mpuResetFuncPtr mpuReset;
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
static void mpu6050FindRevision(gyroDev_t *gyro);
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro);
#endif
mpuResetFnPtr mpuResetFn;
#ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE
@ -71,110 +62,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro);
#define MPU_INQUIRY_MASK 0x7E
mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
{
bool ack;
uint8_t sig;
uint8_t inquiryResult;
// MPU datasheet specifies 30ms.
delay(35);
#ifndef USE_I2C
ack = false;
sig = 0;
#else
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
#endif
if (ack) {
gyro->mpuConfiguration.read = mpuReadRegisterI2C;
gyro->mpuConfiguration.write = mpuWriteRegisterI2C;
} else {
#ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
UNUSED(detectedSpiSensor);
#endif
return &gyro->mpuDetectionResult;
}
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
// If an MPU3050 is connected sig will contain 0.
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050;
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &gyro->mpuDetectionResult;
}
sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision(gyro);
} else if (sig == MPU6500_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
}
return &gyro->mpuDetectionResult;
}
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{
#ifdef USE_GYRO_SPI_MPU6500
uint8_t mpu6500Sensor = mpu6500SpiDetect();
if (mpu6500Sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu6500ReadRegister;
gyro->mpuConfiguration.write = mpu6500WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiDetect()) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = icm20689ReadRegister;
gyro->mpuConfiguration.write = icm20689WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu6000ReadRegister;
gyro->mpuConfiguration.write = mpu6000WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu9250ReadRegister;
gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister;
gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister;
gyro->mpuConfiguration.write = mpu9250WriteRegister;
gyro->mpuConfiguration.reset = mpu9250ResetGyro;
return true;
}
#endif
UNUSED(gyro);
return false;
}
#endif
static void mpu6050FindRevision(gyroDev_t *gyro)
{
bool ack;
@ -188,7 +75,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
ack = gyro->mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer);
ack = gyro->mpuConfiguration.readFn(MPU_RA_XA_OFFS_H, 6, readBuffer);
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) {
/* Congrats, these parts are better. */
@ -202,7 +89,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
failureMode(FAILURE_ACC_INCOMPATIBLE);
}
} else {
ack = gyro->mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId);
ack = gyro->mpuConfiguration.readFn(MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F;
if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE);
@ -289,7 +176,7 @@ bool mpuAccRead(accDev_t *acc)
{
uint8_t data[6];
bool ack = acc->mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
bool ack = acc->mpuConfiguration.readFn(MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) {
return false;
}
@ -312,7 +199,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{
uint8_t data[6];
const bool ack = gyro->mpuConfiguration.read(gyro->mpuConfiguration.gyroReadXRegister, 6, data);
const bool ack = gyro->mpuConfiguration.readFn(gyro->mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) {
return false;
}
@ -324,11 +211,6 @@ bool mpuGyroRead(gyroDev_t *gyro)
return true;
}
void mpuGyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit(gyro);
}
bool mpuCheckDataReady(gyroDev_t* gyro)
{
bool ret;
@ -340,3 +222,111 @@ bool mpuCheckDataReady(gyroDev_t* gyro)
}
return ret;
}
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6000ReadRegister;
gyro->mpuConfiguration.writeFn = mpu6000WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6500
uint8_t mpu6500Sensor = mpu6500SpiDetect();
if (mpu6500Sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6500ReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu9250ReadRegister;
gyro->mpuConfiguration.slowreadFn = mpu9250SlowReadRegister;
gyro->mpuConfiguration.verifywriteFn = verifympu9250WriteRegister;
gyro->mpuConfiguration.writeFn = mpu9250WriteRegister;
gyro->mpuConfiguration.resetFn = mpu9250ResetGyro;
return true;
}
#endif
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiDetect()) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = icm20689ReadRegister;
gyro->mpuConfiguration.writeFn = icm20689WriteRegister;
return true;
}
#endif
UNUSED(gyro);
return false;
}
#endif
mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
{
// MPU datasheet specifies 30ms.
delay(35);
#ifndef USE_I2C
uint8_t sig = 0;
bool ack = false;
#else
uint8_t sig;
bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
#endif
if (ack) {
gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
} else {
#ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
UNUSED(detectedSpiSensor);
#endif
return &gyro->mpuDetectionResult;
}
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
// If an MPU3050 is connected sig will contain 0.
uint8_t inquiryResult;
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050;
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &gyro->mpuDetectionResult;
}
sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision(gyro);
} else if (sig == MPU6500_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
}
return &gyro->mpuDetectionResult;
}
void mpuGyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit(gyro);
}

22
src/main/drivers/accgyro_mpu.h
View file

@ -124,18 +124,18 @@
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFuncPtr)(void);
typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFnPtr)(void);
extern mpuResetFuncPtr mpuReset;
extern mpuResetFnPtr mpuResetFn;
typedef struct mpuConfiguration_s {
mpuReadRegisterFunc read;
mpuWriteRegisterFunc write;
mpuReadRegisterFunc slowread;
mpuWriteRegisterFunc verifywrite;
mpuResetFuncPtr reset;
mpuReadRegisterFnPtr readFn;
mpuWriteRegisterFnPtr writeFn;
mpuReadRegisterFnPtr slowreadFn;
mpuWriteRegisterFnPtr verifywriteFn;
mpuResetFnPtr resetFn;
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
} mpuConfiguration_t;
@ -178,7 +178,7 @@ typedef enum {
ICM_20689_SPI,
ICM_20608_SPI,
ICM_20602_SPI
} detectedMPUSensor_e;
} mpuSensor_e;
typedef enum {
MPU_HALF_RESOLUTION,
@ -186,7 +186,7 @@ typedef enum {
} mpu6050Resolution_e;
typedef struct mpuDetectionResult_s {
detectedMPUSensor_e sensor;
mpuSensor_e sensor;
mpu6050Resolution_e resolution;
} mpuDetectionResult_t;

12
src/main/drivers/accgyro_mpu3050.c
View file

@ -53,21 +53,21 @@ static void mpu3050Init(gyroDev_t *gyro)
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = gyro->mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0);
if (!ack)
failureMode(FAILURE_ACC_INIT);
gyro->mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.write(MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.writeFn(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{
UNUSED(gyro);
uint8_t buf[2];
if (!gyro->mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
if (!gyro->mpuConfiguration.readFn(MPU3050_TEMP_OUT, 2, buf)) {
return false;
}

16
src/main/drivers/accgyro_mpu6050.c
View file

@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff.
// Accel scale 8g (4096 LSB/g)
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG,
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG,
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif
}

22
src/main/drivers/accgyro_mpu6500.c
View file

@ -55,34 +55,34 @@ void mpu6500GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
#ifdef USE_MPU9250_MAG
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
#else
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
#endif
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
#endif
delay(15);
}

22
src/main/drivers/accgyro_spi_icm20689.c
View file

@ -130,32 +130,32 @@ void icm20689GyroInit(gyroDev_t *gyro)
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100);
gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03);
gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100);
// gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
// gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
// delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData);
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
// gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
// gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);

2
src/main/drivers/adc.h
View file

@ -47,5 +47,5 @@ typedef struct adcConfig_s {
adcChannelConfig_t external1;
} adcConfig_t;
void adcInit(adcConfig_t *config);
void adcInit(const adcConfig_t *config);
uint16_t adcGetChannel(uint8_t channel);

2
src/main/drivers/adc_stm32f10x.c
View file

@ -77,7 +77,7 @@ const adcTagMap_t adcTagMap[] = {
// NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
//
void adcInit(adcConfig_t *config)
void adcInit(const adcConfig_t *config)
{
uint8_t configuredAdcChannels = 0;

2
src/main/drivers/adc_stm32f30x.c
View file

@ -104,7 +104,7 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
return ADCINVALID;
}
void adcInit(adcConfig_t *config)
void adcInit(const adcConfig_t *config)
{
ADC_InitTypeDef ADC_InitStructure;
DMA_InitTypeDef DMA_InitStructure;

2
src/main/drivers/adc_stm32f4xx.c
View file

@ -87,7 +87,7 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
return ADCINVALID;
}
void adcInit(adcConfig_t *config)
void adcInit(const adcConfig_t *config)
{
ADC_InitTypeDef ADC_InitStructure;
DMA_InitTypeDef DMA_InitStructure;

2
src/main/drivers/adc_stm32f7xx.c
View file

@ -83,7 +83,7 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
return ADCINVALID;
}
void adcInit(adcConfig_t *config)
void adcInit(const adcConfig_t *config)
{
uint8_t i;
uint8_t configuredAdcChannels = 0;

2
src/main/drivers/bus_spi_hal.c
View file

@ -72,7 +72,7 @@
static spiDevice_t spiHardwareMap[] = {
{ .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER },
{ .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER },
{ .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF5_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER },
{ .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER },
{ .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER }
};

21
src/main/drivers/compass_hmc5883l.c
View file

@ -119,11 +119,9 @@
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
static const hmc5883Config_t *hmc5883Config = NULL;
#ifdef USE_MAG_DATA_READY_SIGNAL
static IO_t intIO;
static IO_t hmc5883InterruptIO;
static extiCallbackRec_t hmc5883_extiCallbackRec;
static void hmc5883_extiHandler(extiCallbackRec_t* cb)
@ -150,20 +148,19 @@ static void hmc5883lConfigureDataReadyInterruptHandling(void)
{
#ifdef USE_MAG_DATA_READY_SIGNAL
if (!(hmc5883Config->intTag)) {
if (!(hmc5883InterruptIO)) {
return;
}
intIO = IOGetByTag(hmc5883Config->intTag);
#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
uint8_t status = IORead(intIO);
uint8_t status = IORead(hmc5883InterruptIO);
if (!status) {
return;
}
#endif
EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler);
EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(intIO, true);
EXTIConfig(hmc5883InterruptIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(hmc5883InterruptIO, true);
#endif
}
@ -257,9 +254,13 @@ static bool hmc5883lInit(void)
return true;
}
bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag)
{
hmc5883Config = hmc5883ConfigToUse;
#ifdef USE_MAG_DATA_READY_SIGNAL
hmc5883InterruptIO = IOGetByTag(interruptTag);
#else
UNUSED(interruptTag);
#endif
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);

6
src/main/drivers/compass_hmc5883l.h
View file

@ -19,8 +19,4 @@
#include "io_types.h"
typedef struct hmc5883Config_s {
ioTag_t intTag;
} hmc5883Config_t;
bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag);

2
src/main/drivers/flash_m25p16.c
View file

@ -208,7 +208,7 @@ static bool m25p16_readIdentification()
* Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with
* m25p16_getGeometry().
*/
bool m25p16_init(flashConfig_t *flashConfig)
bool m25p16_init(const flashConfig_t *flashConfig)
{
/*
if we have already detected a flash device we can simply exit

2
src/main/drivers/flash_m25p16.h
View file

@ -22,7 +22,7 @@
#define M25P16_PAGESIZE 256
bool m25p16_init(flashConfig_t *flashConfig);
bool m25p16_init(const flashConfig_t *flashConfig);
void m25p16_eraseSector(uint32_t address);
void m25p16_eraseCompletely();

2
src/main/drivers/light_led.c
View file

@ -25,7 +25,7 @@
static IO_t leds[LED_NUMBER];
static uint8_t ledPolarity = 0;
void ledInit(statusLedConfig_t *statusLedConfig)
void ledInit(const statusLedConfig_t *statusLedConfig)
{
LED0_OFF;
LED1_OFF;

5
src/main/drivers/light_led.h
View file

@ -17,6 +17,7 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/io_types.h"
#define LED_NUMBER 3
@ -26,6 +27,8 @@ typedef struct statusLedConfig_s {
uint8_t polarity;
} statusLedConfig_t;
PG_DECLARE(statusLedConfig_t, statusLedConfig);
// Helpful macros
#ifdef LED0
# define LED0_TOGGLE ledToggle(0)
@ -57,6 +60,6 @@ typedef struct statusLedConfig_s {
# define LED2_ON do {} while(0)
#endif
void ledInit(statusLedConfig_t *statusLedConfig);
void ledInit(const statusLedConfig_t *statusLedConfig);
void ledToggle(int led);
void ledSet(int led, bool state);

20
src/main/drivers/pwm_output.c
View file

@ -23,8 +23,8 @@
#include "platform.h"
#include "io.h"
#include "timer.h"
#include "pwm_output.h"
#include "timer.h"
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
@ -83,7 +83,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
#endif
}
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion)
{
#if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
@ -91,7 +91,8 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
#endif
configTimeBase(timerHardware->tim, period, mhz);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value,
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
#if defined(USE_HAL_DRIVER)
HAL_TIM_PWM_Start(Handle, timerHardware->channel);
@ -193,7 +194,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount)
pwmCompleteWritePtr(motorCount);
}
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{
memset(motors, 0, sizeof(motors));
@ -259,7 +260,8 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
#ifdef USE_DSHOT
if (isDigital) {
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol,
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
motors[motorIndex].enabled = true;
continue;
}
@ -274,9 +276,9 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
if (useUnsyncedPwm) {
const uint32_t hz = timerMhzCounter * 1000000;
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse);
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion);
} else {
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0);
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion);
}
bool timerAlreadyUsed = false;
@ -323,7 +325,7 @@ void pwmWriteServo(uint8_t index, uint16_t value)
}
}
void servoInit(const servoConfig_t *servoConfig)
void servoDevInit(const servoDevConfig_t *servoConfig)
{
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
const ioTag_t tag = servoConfig->ioTags[servoIndex];
@ -348,7 +350,7 @@ void servoInit(const servoConfig_t *servoConfig)
break;
}
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
servos[servoIndex].enabled = true;
}
}

37
src/main/drivers/pwm_output.h
View file

@ -17,10 +17,17 @@
#pragma once
#include "io/motors.h"
#include "io/servos.h"
#include "drivers/timer.h"
#include "drivers/dma.h"
#include "dma.h"
#include "io_types.h"
#include "timer.h"
#define MAX_SUPPORTED_MOTORS 12
#if defined(USE_QUAD_MIXER_ONLY)
#define MAX_SUPPORTED_SERVOS 1
#else
#define MAX_SUPPORTED_SERVOS 8
#endif
typedef enum {
PWM_TYPE_STANDARD = 0,
@ -111,15 +118,31 @@ typedef struct {
IO_t io;
} pwmOutputPort_t;
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount);
void servoInit(const servoConfig_t *servoConfig);
typedef struct motorDevConfig_s {
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
uint8_t motorPwmProtocol; // Pwm Protocol
uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation
uint8_t useUnsyncedPwm;
ioTag_t ioTags[MAX_SUPPORTED_MOTORS];
} motorDevConfig_t;
void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount);
typedef struct servoDevConfig_s {
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500.
uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz)
ioTag_t ioTags[MAX_SUPPORTED_SERVOS];
} servoDevConfig_t;
void servoDevInit(const servoDevConfig_t *servoDevConfig);
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
#ifdef USE_DSHOT
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDigital(uint8_t index, uint16_t value);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
#endif

12
src/main/drivers/pwm_output_stm32f3xx.c
View file

@ -20,6 +20,8 @@
#include "platform.h"
#ifdef USE_DSHOT
#include "build/debug.h"
#include "io.h"
@ -30,8 +32,6 @@
#include "system.h"
#include "rcc.h"
#ifdef USE_DSHOT
static uint8_t dmaMotorTimerCount = 0;
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@ -105,7 +105,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
}
}
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
DMA_InitTypeDef DMA_InitStructure;
@ -139,14 +139,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
if (output & TIMER_OUTPUT_N_CHANNEL) {
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
} else {
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
}
TIM_OCInitStructure.TIM_Pulse = 0;

12
src/main/drivers/pwm_output_stm32f4xx.c
View file

@ -20,6 +20,8 @@
#include "platform.h"
#ifdef USE_DSHOT
#include "io.h"
#include "timer.h"
#include "timer_stm32f4xx.h"
@ -29,8 +31,6 @@
#include "system.h"
#include "rcc.h"
#ifdef USE_DSHOT
static uint8_t dmaMotorTimerCount = 0;
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@ -102,7 +102,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
}
}
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
DMA_InitTypeDef DMA_InitStructure;
@ -136,14 +136,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
if (output & TIMER_OUTPUT_N_CHANNEL) {
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
} else {
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
}
TIM_OCInitStructure.TIM_Pulse = 0;

16
src/main/drivers/pwm_output_stm32f7xx.c
View file

@ -20,6 +20,8 @@
#include "platform.h"
#ifdef USE_DSHOT
#include "io.h"
#include "timer.h"
#include "pwm_output.h"
@ -28,8 +30,6 @@
#include "system.h"
#include "rcc.h"
#ifdef USE_DSHOT
static uint8_t dmaMotorTimerCount = 0;
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@ -96,7 +96,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
UNUSED(motorCount);
}
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
motor->timerHardware = timerHardware;
@ -174,9 +174,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
/* PWM1 Mode configuration: Channel1 */
TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
if (output & TIMER_OUTPUT_N_CHANNEL) {
TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
} else {
TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
}
TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
TIM_OCInitStructure.Pulse = 0;

2
src/main/drivers/rx_pwm.c
View file

@ -36,6 +36,8 @@
#include "pwm_output.h"
#include "rx_pwm.h"
#include "flight/mixer.h" //!!TODO remove dependency on this
#define DEBUG_PPM_ISR
#define PPM_CAPTURE_COUNT 12

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

@ -49,7 +49,7 @@ void systemBeepToggle(void)
#endif
}
void beeperInit(const beeperConfig_t *config)
void beeperInit(const beeperDevConfig_t *config)
{
#ifndef BEEPER
UNUSED(config);

6
src/main/drivers/sound_beeper.h
View file

@ -29,13 +29,13 @@
#define BEEP_ON do {} while(0)
#endif
typedef struct beeperConfig_s {
typedef struct beeperDevConfig_s {
ioTag_t ioTag;
uint8_t isInverted;
uint8_t isOpenDrain;
} beeperConfig_t;
} beeperDevConfig_t;
void systemBeep(bool on);
void systemBeepToggle(void);
void beeperInit(const beeperConfig_t *beeperConfig);
void beeperInit(const beeperDevConfig_t *beeperDevConfig);

3
src/main/drivers/system.h
View file

@ -17,6 +17,9 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
void systemInit(void);
void delayMicroseconds(uint32_t us);
void delay(uint32_t ms);

8
src/main/drivers/system_stm32f4xx.c
View file

@ -31,8 +31,8 @@ void SetSysClock(void);
void systemReset(void)
{
if (mpuReset) {
mpuReset();
if (mpuResetFn) {
mpuResetFn();
}
__disable_irq();
@ -41,8 +41,8 @@ void systemReset(void)
void systemResetToBootloader(void)
{
if (mpuReset) {
mpuReset();
if (mpuResetFn) {
mpuResetFn();
}
*((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX

8
src/main/drivers/system_stm32f7xx.c
View file

@ -33,8 +33,8 @@ void SystemClock_Config(void);
void systemReset(void)
{
if (mpuReset) {
mpuReset();
if (mpuResetFn) {
mpuResetFn();
}
__disable_irq();
@ -43,8 +43,8 @@ void systemReset(void)
void systemResetToBootloader(void)
{
if (mpuReset) {
mpuReset();
if (mpuResetFn) {
mpuResetFn();
}
(*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot

2
src/main/drivers/vtx_rtc6705.c
View file

@ -87,7 +87,7 @@
#define DISABLE_RTC6705 GPIO_SetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
#define ENABLE_RTC6705 GPIO_ResetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
#if defined(SPRACINGF3NEO)
#ifdef RTC6705_POWER_PIN
static IO_t vtxPowerPin = IO_NONE;
#endif

1088
src/main/fc/cli.c
View file

File diff suppressed because it is too large Load diff

2
src/main/fc/cli.h
View file

@ -20,7 +20,7 @@
extern uint8_t cliMode;
struct serialConfig_s;
void cliInit(struct serialConfig_s *serialConfig);
void cliInit(const struct serialConfig_s *serialConfig);
void cliProcess(void);
struct serialPort_s;
void cliEnter(struct serialPort_s *serialPort);

305
src/main/fc/config.c
View file

@ -29,12 +29,18 @@
#include "cms/cms.h"
#include "common/color.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/color.h"
#include "common/filter.h"
#include "common/maths.h"
#include "config/config_eeprom.h"
#include "config/config_master.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/io.h"
@ -45,6 +51,7 @@
#include "drivers/rx_pwm.h"
#include "drivers/rx_spi.h"
#include "drivers/sdcard.h"
#include "drivers/sensor.h"
#include "drivers/serial.h"
#include "drivers/sound_beeper.h"
#include "drivers/system.h"
@ -56,42 +63,37 @@
#include "fc/fc_rc.h"
#include "fc/runtime_config.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/ledstrip.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/motors.h"
#include "io/osd.h"
#include "io/serial.h"
#include "io/servos.h"
#include "io/vtx.h"
#include "rx/rx.h"
#include "rx/rx_spi.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/altitudehold.h"
#include "flight/navigation.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifndef DEFAULT_RX_FEATURE
#define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
#endif
@ -115,6 +117,16 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
accelerometerTrims->values.yaw = 0;
}
static void resetCompassConfig(compassConfig_t* compassConfig)
{
compassConfig->mag_align = ALIGN_DEFAULT;
#ifdef MAG_INT_EXTI
compassConfig->interruptTag = IO_TAG(MAG_INT_EXTI);
#else
compassConfig->interruptTag = IO_TAG_NONE;
#endif
}
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig)
{
controlRateConfig->rcRate8 = 100;
@ -239,13 +251,15 @@ void resetLedStripConfig(ledStripConfig_t *ledStripConfig)
#ifdef USE_SERVOS
void resetServoConfig(servoConfig_t *servoConfig)
{
servoConfig->servoCenterPulse = 1500;
servoConfig->servoPwmRate = 50;
servoConfig->dev.servoCenterPulse = 1500;
servoConfig->dev.servoPwmRate = 50;
servoConfig->tri_unarmed_servo = 1;
servoConfig->servo_lowpass_freq = 0;
int servoIndex = 0;
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) {
if (timerHardware[i].usageFlags & TIM_USE_SERVO) {
servoConfig->ioTags[servoIndex] = timerHardware[i].tag;
servoConfig->dev.ioTags[servoIndex] = timerHardware[i].tag;
servoIndex++;
}
}
@ -256,22 +270,22 @@ void resetMotorConfig(motorConfig_t *motorConfig)
{
#ifdef BRUSHED_MOTORS
motorConfig->minthrottle = 1000;
motorConfig->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
motorConfig->motorPwmProtocol = PWM_TYPE_BRUSHED;
motorConfig->useUnsyncedPwm = true;
motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
motorConfig->dev.useUnsyncedPwm = true;
#else
#ifdef BRUSHED_ESC_AUTODETECT
if (hardwareMotorType == MOTOR_BRUSHED) {
motorConfig->minthrottle = 1000;
motorConfig->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
motorConfig->motorPwmProtocol = PWM_TYPE_BRUSHED;
motorConfig->useUnsyncedPwm = true;
motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
motorConfig->dev.useUnsyncedPwm = true;
} else
#endif
{
motorConfig->minthrottle = 1070;
motorConfig->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
motorConfig->motorPwmProtocol = PWM_TYPE_ONESHOT125;
motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
motorConfig->dev.motorPwmProtocol = PWM_TYPE_ONESHOT125;
}
#endif
motorConfig->maxthrottle = 2000;
@ -281,7 +295,7 @@ void resetMotorConfig(motorConfig_t *motorConfig)
int motorIndex = 0;
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) {
if (timerHardware[i].usageFlags & TIM_USE_MOTOR) {
motorConfig->ioTags[motorIndex] = timerHardware[i].tag;
motorConfig->dev.ioTags[motorIndex] = timerHardware[i].tag;
motorIndex++;
}
}
@ -338,16 +352,16 @@ void resetAdcConfig(adcConfig_t *adcConfig)
#ifdef BEEPER
void resetBeeperConfig(beeperConfig_t *beeperConfig)
void resetBeeperConfig(beeperDevConfig_t *beeperDevConfig)
{
#ifdef BEEPER_INVERTED
beeperConfig->isOpenDrain = false;
beeperConfig->isInverted = true;
beeperDevConfig->isOpenDrain = false;
beeperDevConfig->isInverted = true;
#else
beeperConfig->isOpenDrain = true;
beeperConfig->isInverted = false;
beeperDevConfig->isOpenDrain = true;
beeperDevConfig->isInverted = false;
#endif
beeperConfig->ioTag = IO_TAG(BEEPER);
beeperDevConfig->ioTag = IO_TAG(BEEPER);
}
#endif
@ -644,15 +658,6 @@ void resetMixerConfig(mixerConfig_t *mixerConfig)
mixerConfig->yaw_motor_direction = 1;
}
#ifdef USE_SERVOS
void resetServoMixerConfig(servoMixerConfig_t *servoMixerConfig)
{
servoMixerConfig->tri_unarmed_servo = 1;
servoMixerConfig->servo_lowpass_freq = 400;
servoMixerConfig->servo_lowpass_enable = 0;
}
#endif
#ifdef USE_MAX7456
void resetMax7456Config(vcdProfile_t *pVcdProfile)
{
@ -713,7 +718,7 @@ uint8_t getCurrentProfile(void)
return masterConfig.current_profile_index;
}
void setProfile(uint8_t profileIndex)
static void setProfile(uint8_t profileIndex)
{
currentProfile = &masterConfig.profile[profileIndex];
currentControlRateProfileIndex = currentProfile->activeRateProfile;
@ -748,10 +753,11 @@ void createDefaultConfig(master_t *config)
// Clear all configuration
memset(config, 0, sizeof(master_t));
uint32_t *featuresPtr = &config->enabledFeatures;
uint32_t *featuresPtr = &config->featureConfig.enabledFeatures;
intFeatureClearAll(featuresPtr);
intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE , featuresPtr);
#ifdef DEFAULT_FEATURES
intFeatureSet(DEFAULT_FEATURES, featuresPtr);
#endif
@ -768,7 +774,6 @@ void createDefaultConfig(master_t *config)
#endif
#ifdef OSD
intFeatureSet(FEATURE_OSD, featuresPtr);
osdResetConfig(&config->osdProfile);
#endif
@ -802,14 +807,15 @@ void createDefaultConfig(master_t *config)
config->gyroConfig.gyro_soft_notch_hz_2 = 200;
config->gyroConfig.gyro_soft_notch_cutoff_2 = 100;
config->debug_mode = DEBUG_MODE;
config->systemConfig.debug_mode = DEBUG_MODE;
config->task_statistics = true;
resetAccelerometerTrims(&config->accelerometerConfig.accZero);
config->gyroConfig.gyro_align = ALIGN_DEFAULT;
config->accelerometerConfig.acc_align = ALIGN_DEFAULT;
config->compassConfig.mag_align = ALIGN_DEFAULT;
resetCompassConfig(&config->compassConfig);
config->boardAlignment.rollDegrees = 0;
config->boardAlignment.pitchDegrees = 0;
@ -839,7 +845,7 @@ void createDefaultConfig(master_t *config)
#endif
#ifdef BEEPER
resetBeeperConfig(&config->beeperConfig);
resetBeeperConfig(&config->beeperDevConfig);
#endif
#ifdef SONAR
@ -868,9 +874,9 @@ void createDefaultConfig(master_t *config)
config->rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &config->rxConfig.failsafe_channel_configurations[i];
channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD;
channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.midrc);
rxFailsafeChannelConfig_t *channelFailsafeConfig = &config->rxConfig.failsafe_channel_configurations[i];
channelFailsafeConfig->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD;
channelFailsafeConfig->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.midrc);
}
config->rxConfig.rssi_channel = 0;
@ -900,7 +906,6 @@ void createDefaultConfig(master_t *config)
resetMixerConfig(&config->mixerConfig);
resetMotorConfig(&config->motorConfig);
#ifdef USE_SERVOS
resetServoMixerConfig(&config->servoMixerConfig);
resetServoConfig(&config->servoConfig);
#endif
resetFlight3DConfig(&config->flight3DConfig);
@ -1041,11 +1046,14 @@ void createDefaultConfig(master_t *config)
}
}
static void resetConf(void)
void resetConfigs(void)
{
createDefaultConfig(&masterConfig);
pgResetAll(MAX_PROFILE_COUNT);
pgActivateProfile(0);
setProfile(0);
setControlRateProfile(0);
#ifdef LED_STRIP
reevaluateLedConfig();
@ -1058,63 +1066,34 @@ void activateConfig(void)
resetAdjustmentStates();
useRcControlsConfig(
modeActivationProfile()->modeActivationConditions,
&masterConfig.motorConfig,
&currentProfile->pidProfile
);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
#endif
useRcControlsConfig(modeActivationConditions(0), &currentProfile->pidProfile);
useAdjustmentConfig(&currentProfile->pidProfile);
#ifdef GPS
gpsUseProfile(&masterConfig.gpsProfile);
gpsUsePIDs(&currentProfile->pidProfile);
#endif
useFailsafeConfig(&masterConfig.failsafeConfig);
setAccelerationTrims(&accelerometerConfig()->accZero);
failsafeReset();
setAccelerationTrims(&accelerometerConfigMutable()->accZero);
setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
mixerUseConfigs(
&masterConfig.flight3DConfig,
&masterConfig.motorConfig,
&masterConfig.mixerConfig,
&masterConfig.airplaneConfig,
&masterConfig.rxConfig
);
#ifdef USE_SERVOS
servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoProfile.servoConf, &masterConfig.gimbalConfig, &masterConfig.channelForwardingConfig);
servoUseConfigs(masterConfig.servoProfile.servoConf, &masterConfig.channelForwardingConfig);
#endif
imuConfigure(
&masterConfig.imuConfig,
&currentProfile->pidProfile,
throttleCorrectionConfig()->throttle_correction_angle
);
imuConfigure(throttleCorrectionConfig()->throttle_correction_angle);
configureAltitudeHold(
&currentProfile->pidProfile,
&masterConfig.barometerConfig,
&masterConfig.rcControlsConfig,
&masterConfig.motorConfig
);
#ifdef BARO
useBarometerConfig(&masterConfig.barometerConfig);
#endif
configureAltitudeHold(&currentProfile->pidProfile);
}
void validateAndFixConfig(void)
{
if ((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) {
motorConfig()->mincommand = 1000;
if((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){
motorConfigMutable()->mincommand = 1000;
}
if ((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
motorConfig()->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
motorConfigMutable()->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
}
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) {
@ -1147,12 +1126,6 @@ void validateAndFixConfig(void)
featureClear(FEATURE_CURRENT_METER);
}
#endif
#if defined(STM32F10X) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
// led strip needs the same ports
featureClear(FEATURE_LED_STRIP);
#endif
// software serial needs free PWM ports
featureClear(FEATURE_SOFTSERIAL);
}
@ -1172,42 +1145,6 @@ void validateAndFixConfig(void)
}
#endif
#if defined(NAZE) && defined(SONAR)
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
#if defined(OLIMEXINO) && defined(SONAR)
if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
#if defined(CC3D) && defined(DISPLAY) && defined(USE_UART3)
if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DASHBOARD)) {
featureClear(FEATURE_DASHBOARD);
}
#endif
#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
// shared pin
if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
featureClear(FEATURE_SONAR);
featureClear(FEATURE_SOFTSERIAL);
featureClear(FEATURE_RSSI_ADC);
}
#endif
#if defined(COLIBRI_RACE)
serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_PARALLEL_PWM);
featureClear(FEATURE_RX_MSP);
featureSet(FEATURE_RX_PPM);
}
#endif
useRxConfig(&masterConfig.rxConfig);
serialConfig_t *serialConfig = &masterConfig.serialConfig;
@ -1227,18 +1164,18 @@ void validateAndFixGyroConfig(void)
{
// Prevent invalid notch cutoff
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
gyroConfig()->gyro_soft_notch_hz_1 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
}
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
gyroConfig()->gyro_soft_notch_hz_2 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
}
float samplingTime = 0.000125f;
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfig()->gyro_sync_denom = 1;
gyroConfig()->gyro_use_32khz = false;
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfigMutable()->gyro_sync_denom = 1;
gyroConfigMutable()->gyro_use_32khz = false;
samplingTime = 0.001f;
}
@ -1246,24 +1183,24 @@ void validateAndFixGyroConfig(void)
samplingTime = 0.00003125;
// F1 and F3 can't handle high sample speed.
#if defined(STM32F1)
gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
#elif defined(STM32F3)
gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
#endif
} else {
#if defined(STM32F1)
gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
#endif
}
#if !defined(GYRO_USES_SPI) || !defined(USE_MPU_DATA_READY_SIGNAL)
gyroConfig()->gyro_isr_update = false;
gyroConfigMutable()->gyro_isr_update = false;
#endif
// check for looptime restrictions based on motor protocol. Motor times have safety margin
const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
float motorUpdateRestriction;
switch(motorConfig()->motorPwmProtocol) {
switch(motorConfig()->dev.motorPwmProtocol) {
case (PWM_TYPE_STANDARD):
motorUpdateRestriction = 1.0f/BRUSHLESS_MOTORS_PWM_RATE;
break;
@ -1287,33 +1224,65 @@ void validateAndFixGyroConfig(void)
if (pidLooptime < motorUpdateRestriction) {
const uint8_t maxPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM);
pidConfig()->pid_process_denom = MIN(pidConfig()->pid_process_denom, maxPidProcessDenom);
pidConfigMutable()->pid_process_denom = MIN(pidConfigMutable()->pid_process_denom, maxPidProcessDenom);
}
// Prevent overriding the max rate of motors
if (motorConfig()->useUnsyncedPwm && (motorConfig()->motorPwmProtocol <= PWM_TYPE_BRUSHED) && motorConfig()->motorPwmProtocol != PWM_TYPE_STANDARD) {
if (motorConfig()->dev.useUnsyncedPwm && (motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD) {
uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
if(motorConfig()->motorPwmRate > maxEscRate)
motorConfig()->motorPwmRate = maxEscRate;
if(motorConfig()->dev.motorPwmRate > maxEscRate)
motorConfigMutable()->dev.motorPwmRate = maxEscRate;
}
}
void readEEPROM(void)
{
suspendRxSignal();
// Sanity check, read flash
if (!loadEEPROM()) {
failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
}
// pgActivateProfile(getCurrentProfile());
// setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex);
if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check
masterConfig.current_profile_index = 0;
}
setProfile(masterConfig.current_profile_index);
validateAndFixConfig();
activateConfig();
resumeRxSignal();
}
void writeEEPROM(void)
{
suspendRxSignal();
writeConfigToEEPROM();
resumeRxSignal();
}
void resetEEPROM(void)
{
resetConfigs();
writeEEPROM();
}
void ensureEEPROMContainsValidData(void)
{
if (isEEPROMContentValid()) {
return;
}
resetEEPROM();
}
void resetEEPROM(void)
{
resetConf();
writeEEPROM();
}
void saveConfigAndNotify(void)
{
writeEEPROM();

46
src/main/fc/config.h
View file

@ -17,8 +17,18 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "config/parameter_group.h"
#include "drivers/adc.h"
#include "drivers/flash.h"
#include "drivers/rx_pwm.h"
#include "drivers/sdcard.h"
#include "drivers/sound_beeper.h"
#include "drivers/vcd.h"
#if FLASH_SIZE <= 128
#define MAX_PROFILE_COUNT 2
#else
@ -58,6 +68,33 @@ typedef enum {
FEATURE_ESC_SENSOR = 1 << 27,
} features_e;
typedef struct systemConfig_s {
uint8_t debug_mode;
char name[MAX_NAME_LENGTH + 1];
} systemConfig_t;
PG_DECLARE(systemConfig_t, systemConfig);
PG_DECLARE(adcConfig_t, adcConfig);
PG_DECLARE(beeperDevConfig_t, beeperDevConfig);
PG_DECLARE(flashConfig_t, flashConfig);
PG_DECLARE(ppmConfig_t, ppmConfig);
PG_DECLARE(pwmConfig_t, pwmConfig);
PG_DECLARE(vcdProfile_t, vcdProfile);
PG_DECLARE(sdcardConfig_t, sdcardConfig);
/*typedef struct beeperConfig_s {
uint32_t beeper_off_flags;
uint32_t preferred_beeper_off_flags;
} beeperConfig_t;
PG_DECLARE(beeperConfig_t, beeperConfig);
*/
struct profile_s;
extern struct profile_s *currentProfile;
struct controlRateConfig_s;
extern struct controlRateConfig_s *currentControlRateProfile;
void beeperOffSet(uint32_t mask);
void beeperOffSetAll(uint8_t beeperCount);
void beeperOffClear(uint32_t mask);
@ -69,7 +106,10 @@ void setPreferredBeeperOffMask(uint32_t mask);
void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex);
void initEEPROM(void);
void resetEEPROM(void);
void readEEPROM(void);
void writeEEPROM();
void ensureEEPROMContainsValidData(void);
void saveConfigAndNotify(void);
@ -79,14 +119,16 @@ void activateConfig(void);
uint8_t getCurrentProfile(void);
void changeProfile(uint8_t profileIndex);
void setProfile(uint8_t profileIndex);
struct profile_s;
void resetProfile(struct profile_s *profile);
uint8_t getCurrentControlRateProfile(void);
void changeControlRateProfile(uint8_t profileIndex);
bool canSoftwareSerialBeUsed(void);
uint16_t getCurrentMinthrottle(void);
struct master_s;
void resetConfigs(void);
struct master_s;
void targetConfiguration(struct master_s *config);
void targetValidateConfiguration(struct master_s *config);

76
src/main/fc/fc_core.c
View file

@ -24,30 +24,39 @@
#include "blackbox/blackbox.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/utils.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/light_led.h"
#include "drivers/system.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "fc/cli.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/fc_rc.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "fc/cli.h"
#include "fc/fc_rc.h"
#include "msp/msp_serial.h"
#include "io/beeper.h"
#include "io/gps.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/serial.h"
@ -59,15 +68,16 @@
#include "scheduler/scheduler.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/failsafe.h"
#include "flight/altitudehold.h"
#include "telemetry/telemetry.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
// June 2013 V2.2-dev
@ -87,7 +97,6 @@ int16_t headFreeModeHold;
uint8_t motorControlEnable = false;
int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
bool isRXDataNew;
@ -95,8 +104,8 @@ static bool armingCalibrationWasInitialised;
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
accelerometerConfig()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
saveConfigAndNotify();
}
@ -183,15 +192,6 @@ void mwArm(void)
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
startBlackbox();
}
#endif
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
@ -291,7 +291,7 @@ void processRx(timeUs_t currentTimeUs)
failsafeUpdateState();
}
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
const throttleStatus_e throttleStatus = calculateThrottleStatus();
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
@ -357,17 +357,17 @@ void processRx(timeUs_t currentTimeUs)
}
}
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, armingConfig()->disarm_kill_switch);
processRcStickPositions(throttleStatus);
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
}
updateActivatedModes(modeActivationProfile()->modeActivationConditions);
updateActivatedModes();
if (!cliMode) {
updateAdjustmentStates(adjustmentProfile()->adjustmentRanges);
processRcAdjustments(currentControlRateProfile, rxConfig());
updateAdjustmentStates();
processRcAdjustments(currentControlRateProfile);
}
bool canUseHorizonMode = true;
@ -473,8 +473,8 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// Read out gyro temperature if used for telemmetry
if (feature(FEATURE_TELEMETRY) && gyro.dev.temperature) {
gyro.dev.temperature(&gyro.dev, &telemTemperature1);
if (feature(FEATURE_TELEMETRY)) {
gyroReadTemperature();
}
#ifdef MAG
@ -488,7 +488,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
applyAltHold(&masterConfig.airplaneConfig);
applyAltHold();
}
}
#endif
@ -500,7 +500,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
#ifndef USE_QUAD_MIXER_ONLY
#ifdef USE_SERVOS
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoMixerConfig()->tri_unarmed_servo)
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
#endif
&& mixerConfig()->mixerMode != MIXER_AIRPLANE
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
@ -531,6 +531,8 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
if (!cliMode && feature(FEATURE_BLACKBOX)) {
handleBlackbox(currentTimeUs);
}
#else
UNUSED(currentTimeUs);
#endif
#ifdef TRANSPONDER
@ -558,8 +560,6 @@ static void subTaskMotorUpdate(void)
#ifdef USE_SERVOS
// motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
if (isMixerUsingServos()) {
servoTable();
filterServos();
writeServos();
}
#endif

8
src/main/fc/fc_core.h
View file

@ -18,11 +18,19 @@
#pragma once
#include "common/time.h"
#include "config/parameter_group.h"
extern int16_t magHold;
extern bool isRXDataNew;
extern int16_t headFreeModeHold;
typedef struct throttleCorrectionConfig_s {
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
} throttleCorrectionConfig_t;
PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
union rollAndPitchTrims_u;
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
void handleInflightCalibrationStickPosition();

102
src/main/fc/fc_init.c
View file

@ -28,6 +28,12 @@
#include "common/maths.h"
#include "common/printf.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
@ -96,29 +102,26 @@
#include "scheduler/scheduler.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/esc_sensor.h"
#include "sensors/gyro.h"
#include "sensors/initialisation.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "telemetry/telemetry.h"
#include "sensors/esc_sensor.h"
#include "flight/pid.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/failsafe.h"
#include "flight/navigation.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"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
@ -186,7 +189,7 @@ void init(void)
//i2cSetOverclock(masterConfig.i2c_overclock);
debugMode = masterConfig.debug_mode;
debugMode = systemConfig()->debug_mode;
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
@ -241,13 +244,13 @@ void init(void)
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (rxConfig()->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(rxConfig());
break;
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(rxConfigMutable());
break;
}
}
#endif
@ -257,21 +260,21 @@ void init(void)
timerInit(); // timer must be initialized before any channel is allocated
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
serialInit(feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
serialInit(feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
serialInit(feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
mixerInit(mixerConfig()->mixerMode, masterConfig.customMotorMixer);
mixerInit(mixerConfig()->mixerMode);
#ifdef USE_SERVOS
servoMixerInit(masterConfig.customServoMixer);
servosInit();
#endif
uint16_t idlePulse = motorConfig()->mincommand;
@ -279,25 +282,25 @@ void init(void)
idlePulse = flight3DConfig()->neutral3d;
}
if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
idlePulse = 0; // brushed motors
}
mixerConfigureOutput();
motorInit(motorConfig(), idlePulse, getMotorCount());
motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
#ifdef USE_SERVOS
servoConfigureOutput();
if (isMixerUsingServos()) {
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
servoInit(servoConfig());
servoDevInit(&servoConfig()->dev);
}
#endif
#if defined(USE_PWM) || defined(USE_PPM)
if (feature(FEATURE_RX_PPM)) {
ppmRxInit(ppmConfig(), motorConfig()->motorPwmProtocol);
ppmRxInit(ppmConfig(), motorConfig()->dev.motorPwmProtocol);
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
pwmRxInit(pwmConfig());
}
@ -306,7 +309,7 @@ void init(void)
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperInit(beeperConfig());
beeperInit(beeperDevConfig());
#endif
/* temp until PGs are implemented. */
#ifdef USE_INVERTER
@ -362,9 +365,9 @@ void init(void)
#ifdef USE_ADC
/* these can be removed from features! */
adcConfig()->vbat.enabled = feature(FEATURE_VBAT);
adcConfig()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
adcConfig()->rssi.enabled = feature(FEATURE_RSSI_ADC);
adcConfigMutable()->vbat.enabled = feature(FEATURE_VBAT);
adcConfigMutable()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC);
adcInit(adcConfig());
#endif
@ -376,7 +379,7 @@ void init(void)
#ifdef USE_DASHBOARD
if (feature(FEATURE_DASHBOARD)) {
dashboardInit(rxConfig());
dashboardInit();
}
#endif
@ -393,7 +396,7 @@ void init(void)
if (feature(FEATURE_OSD)) {
#ifdef USE_MAX7456
// if there is a max7456 chip for the OSD then use it, otherwise use MSP
displayPort_t *osdDisplayPort = max7456DisplayPortInit(vcdProfile(), displayPortProfileMax7456());
displayPort_t *osdDisplayPort = max7456DisplayPortInit(vcdProfile());
#else
displayPort_t *osdDisplayPort = displayPortMspInit(displayPortProfileMax7456());
#endif
@ -401,12 +404,7 @@ void init(void)
}
#endif
#ifdef SONAR
const sonarConfig_t *sonarConfig = sonarConfig();
#else
const void *sonarConfig = NULL;
#endif
if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) {
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
@ -439,32 +437,26 @@ void init(void)
mspSerialInit();
#if defined(USE_MSP_DISPLAYPORT) && defined(CMS)
cmsDisplayPortRegister(displayPortMspInit(displayPortProfileMsp()));
cmsDisplayPortRegister(displayPortMspInit());
#endif
#ifdef USE_CLI
cliInit(serialConfig());
#endif
failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle);
failsafeInit();
rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions);
rxInit(rxConfig(), modeActivationConditions(0));
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
serialConfig(),
gpsConfig()
);
navigationInit(
gpsProfile(),
&currentProfile->pidProfile
);
gpsInit();
navigationInit(&currentProfile->pidProfile);
}
#endif
#ifdef LED_STRIP
ledStripInit(ledStripConfig());
ledStripInit();
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
@ -554,7 +546,7 @@ void init(void)
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(batteryConfig());
batteryInit();
#ifdef USE_DASHBOARD
if (feature(FEATURE_DASHBOARD)) {

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

@ -17,7 +17,6 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
@ -34,79 +33,82 @@
#include "common/maths.h"
#include "common/streambuf.h"
#include "drivers/system.h"
#include "config/config_master.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/serial.h"
#include "drivers/bus_i2c.h"
#include "drivers/io.h"
#include "drivers/compass.h"
#include "drivers/flash.h"
#include "drivers/sdcard.h"
#include "drivers/vcd.h"
#include "drivers/io.h"
#include "drivers/max7456.h"
#include "drivers/vtx_soft_spi_rtc6705.h"
#include "drivers/pwm_output.h"
#include "drivers/sdcard.h"
#include "drivers/serial.h"
#include "drivers/serial_escserial.h"
#include "drivers/system.h"
#include "drivers/vcd.h"
#include "drivers/vtx_common.h"
#include "drivers/vtx_soft_spi_rtc6705.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/fc_msp.h"
#include "fc/fc_rc.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "io/beeper.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/gps.h"
#include "io/gimbal.h"
#include "io/serial.h"
#include "io/ledstrip.h"
#include "io/flashfs.h"
#include "io/transponder_ir.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
#include "io/flashfs.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/motors.h"
#include "io/osd.h"
#include "io/serial.h"
#include "io/serial_4way.h"
#include "io/servos.h"
#include "io/transponder_ir.h"
#include "msp/msp.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
#include "rx/rx.h"
#include "rx/msp.h"
#include "rx/rx.h"
#include "scheduler/scheduler.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
#include "sensors/sonar.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "flight/altitudehold.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif
extern uint16_t cycleTime; // FIXME dependency on mw.c
extern void resetProfile(profile_t *profile);
static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
@ -150,6 +152,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXAIRMODE, "AIR MODE;", 28 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX;", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s);", 31 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -254,14 +257,6 @@ static void mspRebootFn(serialPort_t *serialPort)
while (true) ;
}
static void serializeNames(sbuf_t *dst, const char *s)
{
const char *c;
for (c = s; *c; c++) {
sbufWriteU8(dst, *c);
}
}
static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
{
for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
@ -286,20 +281,19 @@ static const box_t *findBoxByPermenantId(uint8_t permenantId)
static void serializeBoxNamesReply(sbuf_t *dst)
{
int activeBoxId, flag = 1, count = 0, len;
int flag = 1, count = 0;
reset:
// in first run of the loop, we grab total size of junk to be sent
// then come back and actually send it
for (int i = 0; i < activeBoxIdCount; i++) {
activeBoxId = activeBoxIds[i];
const int activeBoxId = activeBoxIds[i];
const box_t *box = findBoxByActiveBoxId(activeBoxId);
if (!box) {
continue;
}
len = strlen(box->boxName);
const int len = strlen(box->boxName);
if (flag) {
count += len;
} else {
@ -378,6 +372,9 @@ void initActiveBoxIds(void)
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
#ifdef USE_FLASHFS
activeBoxIds[activeBoxIdCount++] = BOXBLACKBOXERASE;
#endif
}
#endif
@ -442,6 +439,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) << BOXBLACKBOXERASE |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX;
@ -573,9 +571,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_FC_VARIANT:
for (int i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
sbufWriteU8(dst, flightControllerIdentifier[i]);
}
sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
break;
case MSP_FC_VERSION:
@ -585,9 +581,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_BOARD_INFO:
for (int i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
sbufWriteU8(dst, boardIdentifier[i]);
}
sbufWriteData(dst, boardIdentifier, BOARD_IDENTIFIER_LENGTH);
#ifdef USE_HARDWARE_REVISION_DETECTION
sbufWriteU16(dst, hardwareRevision);
#else
@ -596,15 +590,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_BUILD_INFO:
for (int i = 0; i < BUILD_DATE_LENGTH; i++) {
sbufWriteU8(dst, buildDate[i]);
}
for (int i = 0; i < BUILD_TIME_LENGTH; i++) {
sbufWriteU8(dst, buildTime[i]);
}
for (int i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
sbufWriteU8(dst, shortGitRevision[i]);
}
sbufWriteData(dst, buildDate, BUILD_DATE_LENGTH);
sbufWriteData(dst, buildTime, BUILD_TIME_LENGTH);
sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
break;
// DEPRECATED - Use MSP_API_VERSION
@ -632,9 +620,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_NAME:
{
const int nameLen = strlen(masterConfig.name);
const int nameLen = strlen(systemConfig()->name);
for (int i = 0; i < nameLen; i++) {
sbufWriteU8(dst, masterConfig.name[i]);
sbufWriteU8(dst, systemConfig()->name[i]);
}
}
break;
@ -661,7 +649,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, acc.accSmooth[i] / scale);
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale));
sbufWriteU16(dst, gyroRateDps(i));
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, mag.magADC[i]);
@ -675,25 +663,25 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_SERVO_CONFIGURATIONS:
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
sbufWriteU16(dst, servoProfile()->servoConf[i].min);
sbufWriteU16(dst, servoProfile()->servoConf[i].max);
sbufWriteU16(dst, servoProfile()->servoConf[i].middle);
sbufWriteU8(dst, servoProfile()->servoConf[i].rate);
sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMin);
sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMax);
sbufWriteU8(dst, servoProfile()->servoConf[i].forwardFromChannel);
sbufWriteU32(dst, servoProfile()->servoConf[i].reversedSources);
sbufWriteU16(dst, servoParams(i)->min);
sbufWriteU16(dst, servoParams(i)->max);
sbufWriteU16(dst, servoParams(i)->middle);
sbufWriteU8(dst, servoParams(i)->rate);
sbufWriteU8(dst, servoParams(i)->angleAtMin);
sbufWriteU8(dst, servoParams(i)->angleAtMax);
sbufWriteU8(dst, servoParams(i)->forwardFromChannel);
sbufWriteU32(dst, servoParams(i)->reversedSources);
}
break;
case MSP_SERVO_MIX_RULES:
for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, customServoMixer(i)->targetChannel);
sbufWriteU8(dst, customServoMixer(i)->inputSource);
sbufWriteU8(dst, customServoMixer(i)->rate);
sbufWriteU8(dst, customServoMixer(i)->speed);
sbufWriteU8(dst, customServoMixer(i)->min);
sbufWriteU8(dst, customServoMixer(i)->max);
sbufWriteU8(dst, customServoMixer(i)->box);
sbufWriteU8(dst, customServoMixers(i)->targetChannel);
sbufWriteU8(dst, customServoMixers(i)->inputSource);
sbufWriteU8(dst, customServoMixers(i)->rate);
sbufWriteU8(dst, customServoMixers(i)->speed);
sbufWriteU8(dst, customServoMixers(i)->min);
sbufWriteU8(dst, customServoMixers(i)->max);
sbufWriteU8(dst, customServoMixers(i)->box);
}
break;
#endif
@ -780,7 +768,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_PIDNAMES:
serializeNames(dst, pidnames);
for (const char *c = pidnames; *c; c++) {
sbufWriteU8(dst, *c);
}
break;
case MSP_PID_CONTROLLER:
@ -789,7 +779,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_MODE_RANGES:
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
const modeActivationCondition_t *mac = modeActivationConditions(i);
const box_t *box = &boxes[mac->modeId];
sbufWriteU8(dst, box->permanentId);
sbufWriteU8(dst, mac->auxChannelIndex);
@ -800,7 +790,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_ADJUSTMENT_RANGES:
for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
const adjustmentRange_t *adjRange = adjustmentRanges(i);
sbufWriteU8(dst, adjRange->adjustmentIndex);
sbufWriteU8(dst, adjRange->auxChannelIndex);
sbufWriteU8(dst, adjRange->range.startStep);
@ -967,8 +957,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_RXFAIL_CONFIG:
for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
sbufWriteU8(dst, rxConfig()->failsafe_channel_configurations[i].mode);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig()->failsafe_channel_configurations[i].step));
sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
}
break;
@ -977,9 +967,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_RX_MAP:
for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
sbufWriteU8(dst, rxConfig()->rcmap[i]);
}
sbufWriteData(dst, rxConfig()->rcmap, MAX_MAPPABLE_RX_INPUTS);
break;
case MSP_BF_CONFIG:
@ -1014,7 +1002,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#ifdef LED_STRIP
case MSP_LED_COLORS:
for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &ledStripConfig()->colors[i];
const hsvColor_t *color = &ledStripConfig()->colors[i];
sbufWriteU16(dst, color->h);
sbufWriteU8(dst, color->s);
sbufWriteU8(dst, color->v);
@ -1023,7 +1011,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_LED_STRIP_CONFIG:
for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
sbufWriteU32(dst, *ledConfig);
}
break;
@ -1091,13 +1079,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, osdProfile()->units);
sbufWriteU8(dst, osdProfile()->rssi_alarm);
sbufWriteU16(dst, osdProfile()->cap_alarm);
sbufWriteU16(dst, osdProfile()->time_alarm);
sbufWriteU16(dst, osdProfile()->alt_alarm);
sbufWriteU8(dst, osdConfig()->units);
sbufWriteU8(dst, osdConfig()->rssi_alarm);
sbufWriteU16(dst, osdConfig()->cap_alarm);
sbufWriteU16(dst, osdConfig()->time_alarm);
sbufWriteU16(dst, osdConfig()->alt_alarm);
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
sbufWriteU16(dst, osdProfile()->item_pos[i]);
sbufWriteU16(dst, osdConfig()->item_pos[i]);
}
#else
sbufWriteU8(dst, 0); // OSD not supported
@ -1105,9 +1093,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_BF_BUILD_INFO:
for (int i = 0; i < 11; i++) {
sbufWriteU8(dst, buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
}
sbufWriteData(dst, buildDate, 11); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
sbufWriteU32(dst, 0); // future exp
sbufWriteU32(dst, 0); // future exp
break;
@ -1139,13 +1125,14 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
sbufWriteU8(dst, pidConfig()->pid_process_denom);
}
sbufWriteU8(dst, motorConfig()->useUnsyncedPwm);
sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
sbufWriteU16(dst, motorConfig()->motorPwmRate);
sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
sbufWriteU16(dst, (uint16_t)lrintf(motorConfig()->digitalIdleOffsetPercent * 100));
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
//!!TODO gyro_isr_update to be added pending decision
//sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
break;
case MSP_FILTER_CONFIG :
@ -1316,12 +1303,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#endif
break;
case MSP_SET_ACC_TRIM:
accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src);
accelerometerConfig()->accelerometerTrims.values.roll = sbufReadU16(src);
accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(src);
break;
case MSP_SET_ARMING_CONFIG:
armingConfig()->auto_disarm_delay = sbufReadU8(src);
armingConfig()->disarm_kill_switch = sbufReadU8(src);
armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
armingConfigMutable()->disarm_kill_switch = sbufReadU8(src);
break;
case MSP_SET_LOOP_TIME:
@ -1343,7 +1330,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_MODE_RANGE:
i = sbufReadU8(src);
if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
i = sbufReadU8(src);
const box_t *box = findBoxByPermenantId(i);
if (box) {
@ -1352,7 +1339,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
mac->range.startStep = sbufReadU8(src);
mac->range.endStep = sbufReadU8(src);
useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile);
useRcControlsConfig(modeActivationConditions(0), &currentProfile->pidProfile);
} else {
return MSP_RESULT_ERROR;
}
@ -1364,7 +1351,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_ADJUSTMENT_RANGE:
i = sbufReadU8(src);
if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
adjustmentRange_t *adjRange = adjustmentRangesMutable(i);
i = sbufReadU8(src);
if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
adjRange->adjustmentIndex = i;
@ -1407,32 +1394,32 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_MISC:
rxConfig()->midrc = sbufReadU16(src);
motorConfig()->minthrottle = sbufReadU16(src);
motorConfig()->maxthrottle = sbufReadU16(src);
motorConfig()->mincommand = sbufReadU16(src);
rxConfigMutable()->midrc = sbufReadU16(src);
motorConfigMutable()->minthrottle = sbufReadU16(src);
motorConfigMutable()->maxthrottle = sbufReadU16(src);
motorConfigMutable()->mincommand = sbufReadU16(src);
failsafeConfig()->failsafe_throttle = sbufReadU16(src);
failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
#ifdef GPS
gpsConfig()->provider = sbufReadU8(src); // gps_type
gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
sbufReadU8(src); // gps_baudrate
gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
#else
sbufReadU8(src); // gps_type
sbufReadU8(src); // gps_baudrate
sbufReadU8(src); // gps_ubx_sbas
#endif
batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src);
rxConfig()->rssi_channel = sbufReadU8(src);
batteryConfigMutable()->multiwiiCurrentMeterOutput = sbufReadU8(src);
rxConfigMutable()->rssi_channel = sbufReadU8(src);
sbufReadU8(src);
compassConfig()->mag_declination = sbufReadU16(src) * 10;
compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
break;
case MSP_SET_MOTOR:
@ -1450,14 +1437,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= MAX_SUPPORTED_SERVOS) {
return MSP_RESULT_ERROR;
} else {
servoProfile()->servoConf[i].min = sbufReadU16(src);
servoProfile()->servoConf[i].max = sbufReadU16(src);
servoProfile()->servoConf[i].middle = sbufReadU16(src);
servoProfile()->servoConf[i].rate = sbufReadU8(src);
servoProfile()->servoConf[i].angleAtMin = sbufReadU8(src);
servoProfile()->servoConf[i].angleAtMax = sbufReadU8(src);
servoProfile()->servoConf[i].forwardFromChannel = sbufReadU8(src);
servoProfile()->servoConf[i].reversedSources = sbufReadU32(src);
servoParamsMutable(i)->min = sbufReadU16(src);
servoParamsMutable(i)->max = sbufReadU16(src);
servoParamsMutable(i)->middle = sbufReadU16(src);
servoParamsMutable(i)->rate = sbufReadU8(src);
servoParamsMutable(i)->angleAtMin = sbufReadU8(src);
servoParamsMutable(i)->angleAtMax = sbufReadU8(src);
servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
servoParamsMutable(i)->reversedSources = sbufReadU32(src);
}
#endif
break;
@ -1468,76 +1455,80 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= MAX_SERVO_RULES) {
return MSP_RESULT_ERROR;
} else {
customServoMixer(i)->targetChannel = sbufReadU8(src);
customServoMixer(i)->inputSource = sbufReadU8(src);
customServoMixer(i)->rate = sbufReadU8(src);
customServoMixer(i)->speed = sbufReadU8(src);
customServoMixer(i)->min = sbufReadU8(src);
customServoMixer(i)->max = sbufReadU8(src);
customServoMixer(i)->box = sbufReadU8(src);
customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
customServoMixersMutable(i)->inputSource = sbufReadU8(src);
customServoMixersMutable(i)->rate = sbufReadU8(src);
customServoMixersMutable(i)->speed = sbufReadU8(src);
customServoMixersMutable(i)->min = sbufReadU8(src);
customServoMixersMutable(i)->max = sbufReadU8(src);
customServoMixersMutable(i)->box = sbufReadU8(src);
loadCustomServoMixer();
}
#endif
break;
case MSP_SET_3D:
flight3DConfig()->deadband3d_low = sbufReadU16(src);
flight3DConfig()->deadband3d_high = sbufReadU16(src);
flight3DConfig()->neutral3d = sbufReadU16(src);
flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
flight3DConfigMutable()->neutral3d = sbufReadU16(src);
break;
case MSP_SET_RC_DEADBAND:
rcControlsConfig()->deadband = sbufReadU8(src);
rcControlsConfig()->yaw_deadband = sbufReadU8(src);
rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
flight3DConfig()->deadband3d_throttle = sbufReadU16(src);
rcControlsConfigMutable()->deadband = sbufReadU8(src);
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
break;
case MSP_SET_RESET_CURR_PID:
resetProfile(currentProfile);
break;
case MSP_SET_SENSOR_ALIGNMENT:
gyroConfig()->gyro_align = sbufReadU8(src);
accelerometerConfig()->acc_align = sbufReadU8(src);
compassConfig()->mag_align = sbufReadU8(src);
gyroConfigMutable()->gyro_align = sbufReadU8(src);
accelerometerConfigMutable()->acc_align = sbufReadU8(src);
compassConfigMutable()->mag_align = sbufReadU8(src);
break;
case MSP_SET_ADVANCED_CONFIG:
gyroConfig()->gyro_sync_denom = sbufReadU8(src);
pidConfig()->pid_process_denom = sbufReadU8(src);
motorConfig()->useUnsyncedPwm = sbufReadU8(src);
gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);
pidConfigMutable()->pid_process_denom = sbufReadU8(src);
motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
#ifdef USE_DSHOT
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
#else
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
motorConfigMutable()->dev.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
#endif
motorConfig()->motorPwmRate = sbufReadU16(src);
if (dataSize > 7) {
motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src);
if (sbufBytesRemaining(src) >= 2) {
motorConfigMutable()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
}
if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_use_32khz = sbufReadU8(src);
gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
}
//!!TODO gyro_isr_update to be added pending decision
/*if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_isr_update = sbufReadU8(src);
gyroConfigMutable()->gyro_isr_update = sbufReadU8(src);
}*/
validateAndFixGyroConfig();
if (sbufBytesRemaining(src)) {
motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
}
break;
case MSP_SET_FILTER_CONFIG:
gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src);
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src);
currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src);
if (dataSize > 5) {
gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src);
}
if (dataSize > 13) {
gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
}
// reinitialize the gyro filters with the new values
validateAndFixGyroConfig();
@ -1567,9 +1558,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_SENSOR_CONFIG:
accelerometerConfig()->acc_hardware = sbufReadU8(src);
barometerConfig()->baro_hardware = sbufReadU8(src);
compassConfig()->mag_hardware = sbufReadU8(src);
accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
barometerConfigMutable()->baro_hardware = sbufReadU8(src);
compassConfigMutable()->mag_hardware = sbufReadU8(src);
break;
case MSP_RESET_CONF:
@ -1601,9 +1592,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_BLACKBOX_CONFIG:
// Don't allow config to be updated while Blackbox is logging
if (blackboxMayEditConfig()) {
blackboxConfig()->device = sbufReadU8(src);
blackboxConfig()->rate_num = sbufReadU8(src);
blackboxConfig()->rate_denom = sbufReadU8(src);
blackboxConfigMutable()->device = sbufReadU8(src);
blackboxConfigMutable()->rate_num = sbufReadU8(src);
blackboxConfigMutable()->rate_denom = sbufReadU8(src);
}
break;
#endif
@ -1627,20 +1618,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
// set all the other settings
if ((int8_t)addr == -1) {
#ifdef USE_MAX7456
vcdProfile()->video_system = sbufReadU8(src);
vcdProfileMutable()->video_system = sbufReadU8(src);
#else
sbufReadU8(src); // Skip video system
#endif
osdProfile()->units = sbufReadU8(src);
osdProfile()->rssi_alarm = sbufReadU8(src);
osdProfile()->cap_alarm = sbufReadU16(src);
osdProfile()->time_alarm = sbufReadU16(src);
osdProfile()->alt_alarm = sbufReadU16(src);
osdConfigMutable()->units = sbufReadU8(src);
osdConfigMutable()->rssi_alarm = sbufReadU8(src);
osdConfigMutable()->cap_alarm = sbufReadU16(src);
osdConfigMutable()->time_alarm = sbufReadU16(src);
osdConfigMutable()->alt_alarm = sbufReadU16(src);
} else {
// set a position setting
const uint16_t pos = sbufReadU16(src);
if (addr < OSD_ITEM_COUNT) {
osdProfile()->item_pos[addr] = pos;
osdConfigMutable()->item_pos[addr] = pos;
}
}
}
@ -1759,85 +1750,85 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_BOARD_ALIGNMENT:
boardAlignment()->rollDegrees = sbufReadU16(src);
boardAlignment()->pitchDegrees = sbufReadU16(src);
boardAlignment()->yawDegrees = sbufReadU16(src);
boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
break;
case MSP_SET_VOLTAGE_METER_CONFIG:
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
if (dataSize > 4) {
batteryConfig()->batteryMeterType = sbufReadU8(src);
batteryConfigMutable()->batteryMeterType = sbufReadU8(src);
}
break;
case MSP_SET_CURRENT_METER_CONFIG:
batteryConfig()->currentMeterScale = sbufReadU16(src);
batteryConfig()->currentMeterOffset = sbufReadU16(src);
batteryConfig()->currentMeterType = sbufReadU8(src);
batteryConfig()->batteryCapacity = sbufReadU16(src);
batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
batteryConfigMutable()->currentMeterType = sbufReadU8(src);
batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
break;
#ifndef USE_QUAD_MIXER_ONLY
case MSP_SET_MIXER:
mixerConfig()->mixerMode = sbufReadU8(src);
mixerConfigMutable()->mixerMode = sbufReadU8(src);
break;
#endif
case MSP_SET_RX_CONFIG:
rxConfig()->serialrx_provider = sbufReadU8(src);
rxConfig()->maxcheck = sbufReadU16(src);
rxConfig()->midrc = sbufReadU16(src);
rxConfig()->mincheck = sbufReadU16(src);
rxConfig()->spektrum_sat_bind = sbufReadU8(src);
rxConfigMutable()->serialrx_provider = sbufReadU8(src);
rxConfigMutable()->maxcheck = sbufReadU16(src);
rxConfigMutable()->midrc = sbufReadU16(src);
rxConfigMutable()->mincheck = sbufReadU16(src);
rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
if (dataSize > 8) {
rxConfig()->rx_min_usec = sbufReadU16(src);
rxConfig()->rx_max_usec = sbufReadU16(src);
rxConfigMutable()->rx_min_usec = sbufReadU16(src);
rxConfigMutable()->rx_max_usec = sbufReadU16(src);
}
if (dataSize > 12) {
rxConfig()->rcInterpolation = sbufReadU8(src);
rxConfig()->rcInterpolationInterval = sbufReadU8(src);
rxConfig()->airModeActivateThreshold = sbufReadU16(src);
rxConfigMutable()->rcInterpolation = sbufReadU8(src);
rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src);
rxConfigMutable()->airModeActivateThreshold = sbufReadU16(src);
}
if (dataSize > 16) {
rxConfig()->rx_spi_protocol = sbufReadU8(src);
rxConfig()->rx_spi_id = sbufReadU32(src);
rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src);
rxConfigMutable()->rx_spi_protocol = sbufReadU8(src);
rxConfigMutable()->rx_spi_id = sbufReadU32(src);
rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
}
if (dataSize > 22) {
rxConfig()->fpvCamAngleDegrees = sbufReadU8(src);
rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src);
}
break;
case MSP_SET_FAILSAFE_CONFIG:
failsafeConfig()->failsafe_delay = sbufReadU8(src);
failsafeConfig()->failsafe_off_delay = sbufReadU8(src);
failsafeConfig()->failsafe_throttle = sbufReadU16(src);
failsafeConfig()->failsafe_kill_switch = sbufReadU8(src);
failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src);
failsafeConfig()->failsafe_procedure = sbufReadU8(src);
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
failsafeConfigMutable()->failsafe_kill_switch = sbufReadU8(src);
failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
break;
case MSP_SET_RXFAIL_CONFIG:
i = sbufReadU8(src);
if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
rxConfig()->failsafe_channel_configurations[i].mode = sbufReadU8(src);
rxConfig()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
rxConfigMutable()->failsafe_channel_configurations[i].mode = sbufReadU8(src);
rxConfigMutable()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
} else {
return MSP_RESULT_ERROR;
}
break;
case MSP_SET_RSSI_CONFIG:
rxConfig()->rssi_channel = sbufReadU8(src);
rxConfigMutable()->rssi_channel = sbufReadU8(src);
break;
case MSP_SET_RX_MAP:
for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
rxConfig()->rcmap[i] = sbufReadU8(src);
rxConfigMutable()->rcmap[i] = sbufReadU8(src);
}
break;
@ -1845,20 +1836,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef USE_QUAD_MIXER_ONLY
sbufReadU8(src); // mixerMode ignored
#else
mixerConfig()->mixerMode = sbufReadU8(src); // mixerMode
mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode
#endif
featureClearAll();
featureSet(sbufReadU32(src)); // features bitmap
rxConfig()->serialrx_provider = sbufReadU8(src); // serialrx_type
rxConfigMutable()->serialrx_provider = sbufReadU8(src); // serialrx_type
boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll
boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch
boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw
boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll
boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch
boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_yaw
batteryConfig()->currentMeterScale = sbufReadU16(src);
batteryConfig()->currentMeterOffset = sbufReadU16(src);
batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
break;
case MSP_SET_CF_SERIAL_CONFIG:
@ -1892,7 +1883,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef LED_STRIP
case MSP_SET_LED_COLORS:
for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &ledStripConfig()->colors[i];
hsvColor_t *color = &ledStripConfigMutable()->colors[i];
color->h = sbufReadU16(src);
color->s = sbufReadU8(src);
color->v = sbufReadU8(src);
@ -1905,7 +1896,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
return MSP_RESULT_ERROR;
}
ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i];
*ledConfig = sbufReadU32(src);
reevaluateLedConfig();
}
@ -1924,9 +1915,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#endif
case MSP_SET_NAME:
memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name));
memset(systemConfigMutable()->name, 0, ARRAYLEN(systemConfig()->name));
for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
masterConfig.name[i] = sbufReadU8(src);
systemConfigMutable()->name[i] = sbufReadU8(src);
}
break;

19
src/main/fc/fc_tasks.c
View file

@ -27,6 +27,11 @@
#include "common/color.h"
#include "common/utils.h"
#include "config/feature.h"
#include "config/config_profile.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
@ -43,8 +48,10 @@
#include "fc/cli.h"
#include "fc/fc_dispatch.h"
#include "flight/pid.h"
#include "flight/altitudehold.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/dashboard.h"
@ -72,10 +79,6 @@
#include "telemetry/telemetry.h"
#include "config/feature.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
#endif
@ -94,7 +97,7 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
accUpdate(&accelerometerConfig()->accelerometerTrims);
accUpdate(&accelerometerConfigMutable()->accelerometerTrims);
}
static void taskHandleSerial(timeUs_t currentTimeUs)
@ -128,7 +131,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs)
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTimeUs;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
updateCurrentMeter(ibatTimeSinceLastServiced);
}
}
}
@ -201,7 +204,7 @@ static void taskTelemetry(timeUs_t currentTimeUs)
telemetryCheckState();
if (!cliMode && feature(FEATURE_TELEMETRY)) {
telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
telemetryProcess(currentTimeUs);
}
}
#endif

457
src/main/fc/rc_adjustments.c Normal file
View file

@ -0,0 +1,457 @@
/*
* 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 <math.h>
#include "platform.h"
#include "blackbox/blackbox.h"
#include "build/build_config.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "config/feature.h"
#include "config/config_master.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/motors.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/fc_rc.h"
#include "fc/config.h"
#include "rx/rx.h"
static pidProfile_t *pidProfile;
static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
{
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newValue = newValue;
eventData.floatFlag = false;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
#if 0
static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue)
{
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newFloatValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newFloatValue = newFloatValue;
eventData.floatFlag = true;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
#endif
static uint8_t adjustmentStateMask = 0;
#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
// sync with adjustmentFunction_e
static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
{
.adjustmentFunction = ADJUSTMENT_RC_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .selectConfig = { .switchPositions = 3 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}
};
#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig)
{
adjustmentState_t *adjustmentState = &adjustmentStates[index];
if (adjustmentState->config == adjustmentConfig) {
// already configured
return;
}
adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
adjustmentState->config = adjustmentConfig;
adjustmentState->timeoutAt = 0;
MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
}
static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
{
int newValue;
if (delta > 0) {
beeperConfirmationBeeps(2);
} else {
beeperConfirmationBeeps(1);
}
switch(adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue);
break;
case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:
newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
controlRateConfig->thrExpo8 = newValue;
generateThrottleCurve();
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P:
newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I:
newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D:
newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
break;
case ADJUSTMENT_YAW_P:
newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
break;
case ADJUSTMENT_YAW_I:
newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
break;
case ADJUSTMENT_YAW_D:
newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
break;
case ADJUSTMENT_RC_RATE_YAW:
newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcYawRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
case ADJUSTMENT_D_SETPOINT:
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
break;
case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
break;
default:
break;
};
}
static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
bool applied = false;
switch(adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) {
changeControlRateProfile(position);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
applied = true;
}
break;
}
if (applied) {
beeperConfirmationBeeps(position + 1);
}
}
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(controlRateConfig_t *controlRateConfig)
{
const uint32_t now = millis();
const bool canUseRxData = rxIsReceivingSignal();
for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
if (!adjustmentState->config) {
continue;
}
const uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
if (adjustmentFunction == ADJUSTMENT_NONE) {
continue;
}
const int32_t signedDiff = now - adjustmentState->timeoutAt;
const bool canResetReadyStates = signedDiff >= 0L;
if (canResetReadyStates) {
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
}
if (!canUseRxData) {
continue;
}
const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
int delta;
if (rcData[channelIndex] > rxConfig()->midrc + 200) {
delta = adjustmentState->config->data.stepConfig.step;
} else if (rcData[channelIndex] < rxConfig()->midrc - 200) {
delta = 0 - adjustmentState->config->data.stepConfig.step;
} else {
// returning the switch to the middle immediately resets the ready state
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
continue;
}
if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
continue;
}
applyStepAdjustment(controlRateConfig,adjustmentFunction,delta);
pidInitConfig(pidProfile);
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
applySelectAdjustment(adjustmentFunction, position);
}
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
}
}
void resetAdjustmentStates(void)
{
memset(adjustmentStates, 0, sizeof(adjustmentStates));
}
void updateAdjustmentStates(void)
{
for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index);
if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
}
}
}
void useAdjustmentConfig(pidProfile_t *pidProfileToUse)
{
pidProfile = pidProfileToUse;
}

116
src/main/fc/rc_adjustments.h Normal file
View file

@ -0,0 +1,116 @@
/*
* 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 <stdbool.h>
#include "config/parameter_group.h"
#include "fc/rc_controls.h"
typedef enum {
ADJUSTMENT_NONE = 0,
ADJUSTMENT_RC_RATE,
ADJUSTMENT_RC_EXPO,
ADJUSTMENT_THROTTLE_EXPO,
ADJUSTMENT_PITCH_ROLL_RATE,
ADJUSTMENT_YAW_RATE,
ADJUSTMENT_PITCH_ROLL_P,
ADJUSTMENT_PITCH_ROLL_I,
ADJUSTMENT_PITCH_ROLL_D,
ADJUSTMENT_YAW_P,
ADJUSTMENT_YAW_I,
ADJUSTMENT_YAW_D,
ADJUSTMENT_RATE_PROFILE,
ADJUSTMENT_PITCH_RATE,
ADJUSTMENT_ROLL_RATE,
ADJUSTMENT_PITCH_P,
ADJUSTMENT_PITCH_I,
ADJUSTMENT_PITCH_D,
ADJUSTMENT_ROLL_P,
ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D,
ADJUSTMENT_RC_RATE_YAW,
ADJUSTMENT_D_SETPOINT,
ADJUSTMENT_D_SETPOINT_TRANSITION,
ADJUSTMENT_FUNCTION_COUNT
} adjustmentFunction_e;
typedef enum {
ADJUSTMENT_MODE_STEP,
ADJUSTMENT_MODE_SELECT
} adjustmentMode_e;
typedef struct adjustmentStepConfig_s {
uint8_t step;
} adjustmentStepConfig_t;
typedef struct adjustmentSelectConfig_s {
uint8_t switchPositions;
} adjustmentSelectConfig_t;
typedef union adjustmentConfig_u {
adjustmentStepConfig_t stepConfig;
adjustmentSelectConfig_t selectConfig;
} adjustmentData_t;
typedef struct adjustmentConfig_s {
uint8_t adjustmentFunction;
uint8_t mode;
adjustmentData_t data;
} adjustmentConfig_t;
typedef struct adjustmentRange_s {
// when aux channel is in range...
uint8_t auxChannelIndex;
channelRange_t range;
// ..then apply the adjustment function to the auxSwitchChannel ...
uint8_t adjustmentFunction;
uint8_t auxSwitchChannelIndex;
// ... via slot
uint8_t adjustmentIndex;
} adjustmentRange_t;
#define ADJUSTMENT_INDEX_OFFSET 1
typedef struct adjustmentState_s {
uint8_t auxChannelIndex;
const adjustmentConfig_t *config;
uint32_t timeoutAt;
} adjustmentState_t;
#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
#endif
#define MAX_ADJUSTMENT_RANGE_COUNT 15
PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges);
typedef struct adjustmentProfile_s {
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
} adjustmentProfile_t;
void resetAdjustmentStates(void);
void updateAdjustmentStates(void);
struct controlRateConfig_s;
void processRcAdjustments(struct controlRateConfig_s *controlRateConfig);
struct pidProfile_s;
void useAdjustmentConfig(struct pidProfile_s *pidProfileToUse);

439
src/main/fc/rc_controls.c
View file

@ -31,6 +31,8 @@
#include "common/maths.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
@ -59,7 +61,6 @@
#include "flight/failsafe.h"
static motorConfig_t *motorConfig;
static pidProfile_t *pidProfile;
// true if arming is done via the sticks (as opposed to a switch)
@ -73,37 +74,6 @@ bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
#ifndef BLACKBOX
#define UNUSED(x) (void)(x)
UNUSED(adjustmentFunction);
UNUSED(newValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newValue = newValue;
eventData.floatFlag = false;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) {
#ifndef BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newFloatValue);
#else
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newFloatValue = newFloatValue;
eventData.floatFlag = true;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
bool isUsingSticksForArming(void)
{
return isUsingSticksToArm;
@ -114,20 +84,20 @@ bool areSticksInApModePosition(uint16_t ap_mode)
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
}
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
throttleStatus_e calculateThrottleStatus(void)
{
if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
if ((rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle)))
if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)))
return THROTTLE_LOW;
} else {
if (rcData[THROTTLE] < rxConfig->mincheck)
if (rcData[THROTTLE] < rxConfig()->mincheck)
return THROTTLE_LOW;
}
return THROTTLE_HIGH;
}
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch)
void processRcStickPositions(throttleStatus_e throttleStatus)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
@ -139,9 +109,9 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
// checking sticks positions
for (i = 0; i < 4; i++) {
stTmp >>= 2;
if (rcData[i] > rxConfig->mincheck)
if (rcData[i] > rxConfig()->mincheck)
stTmp |= 0x80; // check for MIN
if (rcData[i] < rxConfig->maxcheck)
if (rcData[i] < rxConfig()->maxcheck)
stTmp |= 0x40; // check for MAX
}
if (stTmp == rcSticks) {
@ -168,7 +138,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
if (disarm_kill_switch) {
if (armingConfig()->disarm_kill_switch) {
mwDisarm();
} else if (throttleStatus == THROTTLE_LOW) {
mwDisarm();
@ -317,12 +287,12 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
}
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
{
uint8_t index;
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
return true;
@ -332,24 +302,22 @@ bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationC
return false;
}
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) {
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
if (!IS_RANGE_USABLE(range)) {
return false;
}
uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
return (channelValue >= 900 + (range->startStep * 25) &&
channelValue < 900 + (range->endStep * 25));
}
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
void updateActivatedModes(void)
{
rcModeActivationMask = 0;
uint8_t index;
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index);
if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) {
ACTIVATE_RC_MODE(modeActivationCondition->modeId);
@ -357,386 +325,13 @@ void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
}
}
uint8_t adjustmentStateMask = 0;
#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
// sync with adjustmentFunction_e
static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
{
.adjustmentFunction = ADJUSTMENT_RC_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_YAW_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .selectConfig = { .switchPositions = 3 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_PITCH_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}
};
#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) {
adjustmentState_t *adjustmentState = &adjustmentStates[index];
if (adjustmentState->config == adjustmentConfig) {
// already configured
return;
}
adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
adjustmentState->config = adjustmentConfig;
adjustmentState->timeoutAt = 0;
MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
}
static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
int newValue;
if (delta > 0) {
beeperConfirmationBeeps(2);
} else {
beeperConfirmationBeeps(1);
}
switch(adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue);
break;
case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcExpo8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:
newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
controlRateConfig->thrExpo8 = newValue;
generateThrottleCurve();
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_PITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
case ADJUSTMENT_ROLL_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
controlRateConfig->rates[FD_ROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
break;
case ADJUSTMENT_YAW_RATE:
newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
controlRateConfig->rates[FD_YAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P:
newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I:
newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D:
newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
break;
case ADJUSTMENT_YAW_P:
newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->P8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
break;
case ADJUSTMENT_YAW_I:
newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->I8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
break;
case ADJUSTMENT_YAW_D:
newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
pidProfile->D8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
break;
case ADJUSTMENT_RC_RATE_YAW:
newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c
controlRateConfig->rcYawRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
case ADJUSTMENT_D_SETPOINT:
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
break;
case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
break;
default:
break;
};
}
static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
bool applied = false;
switch(adjustmentFunction) {
case ADJUSTMENT_RATE_PROFILE:
if (getCurrentControlRateProfile() != position) {
changeControlRateProfile(position);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
applied = true;
}
break;
}
if (applied) {
beeperConfirmationBeeps(position + 1);
}
}
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
{
uint8_t adjustmentIndex;
uint32_t now = millis();
bool canUseRxData = rxIsReceivingSignal();
for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
if (!adjustmentState->config) {
continue;
}
uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
if (adjustmentFunction == ADJUSTMENT_NONE) {
continue;
}
int32_t signedDiff = now - adjustmentState->timeoutAt;
bool canResetReadyStates = signedDiff >= 0L;
if (canResetReadyStates) {
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
}
if (!canUseRxData) {
continue;
}
uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
int delta;
if (rcData[channelIndex] > rxConfig->midrc + 200) {
delta = adjustmentState->config->data.stepConfig.step;
} else if (rcData[channelIndex] < rxConfig->midrc - 200) {
delta = 0 - adjustmentState->config->data.stepConfig.step;
} else {
// returning the switch to the middle immediately resets the ready state
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
continue;
}
if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
continue;
}
applyStepAdjustment(controlRateConfig,adjustmentFunction,delta);
pidInitConfig(pidProfile);
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
applySelectAdjustment(adjustmentFunction, position);
}
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
}
}
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges)
{
uint8_t index;
for (index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
adjustmentRange_t *adjustmentRange = &adjustmentRanges[index];
if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
}
}
}
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
return MIN(ABS(rcData[axis] - midrc), 500);
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse)
void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse)
{
motorConfig = motorConfigToUse;
pidProfile = pidProfileToUse;
isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);
}
void resetAdjustmentStates(void)
{
memset(adjustmentStates, 0, sizeof(adjustmentStates));
}

124
src/main/fc/rc_controls.h
View file

@ -19,6 +19,8 @@
#include <stdbool.h>
#include "config/parameter_group.h"
typedef enum {
BOXARM = 0,
BOXANGLE,
@ -51,6 +53,7 @@ typedef enum {
BOXAIRMODE,
BOX3DDISABLESWITCH,
BOXFPVANGLEMIX,
BOXBLACKBOXERASE,
CHECKBOX_ITEM_COUNT
} boxId_e;
@ -140,6 +143,8 @@ typedef struct modeActivationCondition_s {
channelRange_t range;
} modeActivationCondition_t;
PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
typedef struct modeActivationProfile_s {
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
} modeActivationProfile_t;
@ -158,6 +163,9 @@ typedef struct controlRateConfig_s {
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
} controlRateConfig_t;
#define MAX_CONTROL_RATE_PROFILE_COUNT 3
PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);
extern int16_t rcCommand[4];
typedef struct rcControlsConfig_s {
@ -168,117 +176,39 @@ typedef struct rcControlsConfig_s {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
} rcControlsConfig_t;
PG_DECLARE(rcControlsConfig_t, rcControlsConfig);
typedef struct flight3DConfig_s {
uint16_t deadband3d_low; // min 3d value
uint16_t deadband3d_high; // max 3d value
uint16_t neutral3d; // center 3d value
uint16_t deadband3d_throttle; // default throttle deadband from MIDRC
} flight3DConfig_t;
PG_DECLARE(flight3DConfig_t, flight3DConfig);
typedef struct armingConfig_s {
uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
} armingConfig_t;
PG_DECLARE(armingConfig_t, armingConfig);
bool areUsingSticksToArm(void);
bool areSticksInApModePosition(uint16_t ap_mode);
struct rxConfig_s;
throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
throttleStatus_e calculateThrottleStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus);
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range);
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);
typedef enum {
ADJUSTMENT_NONE = 0,
ADJUSTMENT_RC_RATE,
ADJUSTMENT_RC_EXPO,
ADJUSTMENT_THROTTLE_EXPO,
ADJUSTMENT_PITCH_ROLL_RATE,
ADJUSTMENT_YAW_RATE,
ADJUSTMENT_PITCH_ROLL_P,
ADJUSTMENT_PITCH_ROLL_I,
ADJUSTMENT_PITCH_ROLL_D,
ADJUSTMENT_YAW_P,
ADJUSTMENT_YAW_I,
ADJUSTMENT_YAW_D,
ADJUSTMENT_RATE_PROFILE,
ADJUSTMENT_PITCH_RATE,
ADJUSTMENT_ROLL_RATE,
ADJUSTMENT_PITCH_P,
ADJUSTMENT_PITCH_I,
ADJUSTMENT_PITCH_D,
ADJUSTMENT_ROLL_P,
ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D,
ADJUSTMENT_RC_RATE_YAW,
ADJUSTMENT_D_SETPOINT,
ADJUSTMENT_D_SETPOINT_TRANSITION,
ADJUSTMENT_FUNCTION_COUNT
} adjustmentFunction_e;
typedef enum {
ADJUSTMENT_MODE_STEP,
ADJUSTMENT_MODE_SELECT
} adjustmentMode_e;
typedef struct adjustmentStepConfig_s {
uint8_t step;
} adjustmentStepConfig_t;
typedef struct adjustmentSelectConfig_s {
uint8_t switchPositions;
} adjustmentSelectConfig_t;
typedef union adjustmentConfig_u {
adjustmentStepConfig_t stepConfig;
adjustmentSelectConfig_t selectConfig;
} adjustmentData_t;
typedef struct adjustmentConfig_s {
uint8_t adjustmentFunction;
uint8_t mode;
adjustmentData_t data;
} adjustmentConfig_t;
typedef struct adjustmentRange_s {
// when aux channel is in range...
uint8_t auxChannelIndex;
channelRange_t range;
// ..then apply the adjustment function to the auxSwitchChannel ...
uint8_t adjustmentFunction;
uint8_t auxSwitchChannelIndex;
// ... via slot
uint8_t adjustmentIndex;
} adjustmentRange_t;
#define ADJUSTMENT_INDEX_OFFSET 1
typedef struct adjustmentState_s {
uint8_t auxChannelIndex;
const adjustmentConfig_t *config;
uint32_t timeoutAt;
} adjustmentState_t;
#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
#endif
#define MAX_ADJUSTMENT_RANGE_COUNT 15
typedef struct adjustmentProfile_s {
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
} adjustmentProfile_t;
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
void updateActivatedModes(void);
bool isAirmodeActive(void);
void resetAdjustmentStates(void);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig);
bool isUsingSticksForArming(void);
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
struct pidProfile_s;
struct motorConfig_s;
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse);
void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse);

57
src/main/flight/altitudehold.c
View file

@ -21,25 +21,27 @@
#include <stdlib.h>
#include <math.h>
#include "platform.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/maths.h"
#include "sensors/barometer.h"
#include "sensors/sonar.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/altitudehold.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "rx/rx.h"
#include "fc/rc_controls.h"
#include "io/motors.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "fc/runtime_config.h"
#include "sensors/barometer.h"
#include "sensors/sonar.h"
int32_t setVelocity = 0;
@ -50,22 +52,11 @@ int32_t AltHold;
int32_t vario = 0; // variometer in cm/s
static barometerConfig_t *barometerConfig;
static pidProfile_t *pidProfile;
static rcControlsConfig_t *rcControlsConfig;
static motorConfig_t *motorConfig;
void configureAltitudeHold(
pidProfile_t *initialPidProfile,
barometerConfig_t *intialBarometerConfig,
rcControlsConfig_t *initialRcControlsConfig,
motorConfig_t *initialMotorConfig
)
void configureAltitudeHold(pidProfile_t *initialPidProfile)
{
pidProfile = initialPidProfile;
barometerConfig = intialBarometerConfig;
rcControlsConfig = initialRcControlsConfig;
motorConfig = initialMotorConfig;
}
#if defined(BARO) || defined(SONAR)
@ -82,12 +73,12 @@ static void applyMultirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
// multirotor alt hold
if (rcControlsConfig->alt_hold_fast_change) {
if (rcControlsConfig()->alt_hold_fast_change) {
// rapid alt changes
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband;
rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
@ -97,7 +88,7 @@ static void applyMultirotorAltHold(void)
}
} else {
// slow alt changes, mostly used for aerial photography
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;
@ -111,19 +102,19 @@ static void applyMultirotorAltHold(void)
}
}
static void applyFixedWingAltHold(airplaneConfig_t *airplaneConfig)
static void applyFixedWingAltHold(void)
{
// handle fixedwing-related althold. UNTESTED! and probably wrong
// most likely need to check changes on pitch channel and 'reset' althold similar to
// how throttle does it on multirotor
rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig->fixedwing_althold_dir;
rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig()->fixedwing_althold_dir;
}
void applyAltHold(airplaneConfig_t *airplaneConfig)
void applyAltHold(void)
{
if (STATE(FIXED_WING)) {
applyFixedWingAltHold(airplaneConfig);
applyFixedWingAltHold();
} else {
applyMultirotorAltHold();
}
@ -272,7 +263,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
#ifdef DEBUG_ALT_HOLD
@ -308,7 +299,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel);
vel = vel * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario

16
src/main/flight/altitudehold.h
View file

@ -17,19 +17,23 @@
#pragma once
#include "common/time.h"
extern int32_t AltHold;
extern int32_t vario;
typedef struct airplaneConfig_s {
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
} airplaneConfig_t;
PG_DECLARE(airplaneConfig_t, airplaneConfig);
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
struct pidProfile_s;
struct barometerConfig_s;
struct rcControlsConfig_s;
struct motorConfig_s;
void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct motorConfig_s *initialMotorConfig);
void configureAltitudeHold(struct pidProfile_s *initialPidProfile);
struct airplaneConfig_s;
void applyAltHold(struct airplaneConfig_s *airplaneConfig);
void applyAltHold(void);
void updateAltHoldState(void);
void updateSonarAltHoldState(void);

50
src/main/flight/failsafe.c
View file

@ -24,6 +24,9 @@
#include "common/axis.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "fc/config.h"
@ -40,9 +43,9 @@
/*
* Usage:
*
* failsafeInit() and useFailsafeConfig() must be called before the other methods are used.
* failsafeInit() and resetFailsafe() must be called before the other methods are used.
*
* failsafeInit() and useFailsafeConfig() can be called in any order.
* failsafeInit() and resetFailsafe() can be called in any order.
* failsafeInit() should only be called once.
*
* enable() should be called after system initialisation.
@ -50,15 +53,12 @@
static failsafeState_t failsafeState;
static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC
static void failsafeReset(void)
/*
* Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
*/
void failsafeReset(void)
{
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig->failsafe_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.validRxDataReceivedAt = 0;
failsafeState.validRxDataFailedAt = 0;
failsafeState.throttleLowPeriod = 0;
@ -69,20 +69,8 @@ static void failsafeReset(void)
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
}
/*
* Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
*/
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
void failsafeInit(void)
{
failsafeConfig = failsafeConfigToUse;
failsafeReset();
}
void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle)
{
rxConfig = intialRxConfig;
deadband3dThrottle = deadband3d_throttle;
failsafeState.events = 0;
failsafeState.monitoring = false;
@ -119,7 +107,7 @@ static void failsafeActivate(void)
failsafeState.active = true;
failsafeState.phase = FAILSAFE_LANDING;
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.events++;
}
@ -127,9 +115,9 @@ static void failsafeActivate(void)
static void failsafeApplyControlInput(void)
{
for (int i = 0; i < 3; i++) {
rcData[i] = rxConfig->midrc;
rcData[i] = rxConfig()->midrc;
}
rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
rcData[THROTTLE] = failsafeConfig()->failsafe_throttle;
}
bool failsafeIsReceivingRxData(void)
@ -189,11 +177,11 @@ void failsafeUpdateState(void)
case FAILSAFE_IDLE:
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
if (THROTTLE_HIGH == calculateThrottleStatus()) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
// Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) {
if (failsafeSwitchIsOn && failsafeConfig()->failsafe_kill_switch) {
// KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
@ -226,7 +214,7 @@ void failsafeUpdateState(void)
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
} else {
switch (failsafeConfig->failsafe_procedure) {
switch (failsafeConfig()->failsafe_procedure) {
default:
case FAILSAFE_PROCEDURE_AUTO_LANDING:
// Stabilize, and set Throttle to specified level
@ -288,7 +276,7 @@ void failsafeUpdateState(void)
// Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period.
// This is to prevent that JustDisarm is activated on the next iteration.
// Because that would have the effect of shutting down failsafe handling on intermittent connections.
failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false;
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);

9
src/main/flight/failsafe.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "config/parameter_group.h"
#define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5)
#define MILLIS_PER_TENTH_SECOND 100
#define MILLIS_PER_SECOND 1000
@ -36,6 +38,8 @@ typedef struct failsafeConfig_s {
uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it
} failsafeConfig_t;
PG_DECLARE(failsafeConfig_t, failsafeConfig);
typedef enum {
FAILSAFE_IDLE = 0,
FAILSAFE_RX_LOSS_DETECTED,
@ -70,9 +74,8 @@ typedef struct failsafeState_s {
failsafeRxLinkState_e rxLinkState;
} failsafeState_t;
struct rxConfig_s;
void failsafeInit(struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle);
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
void failsafeInit(void);
void failsafeReset(void);
void failsafeStartMonitoring(void);
void failsafeUpdateState(void);

125
src/main/flight/imu.c
View file

@ -21,37 +21,40 @@
#include <stdint.h>
#include <math.h>
#include "common/maths.h"
#include "platform.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/sonar.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "io/gps.h"
#include "fc/runtime_config.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
// the limit (in degrees/second) beyond which we stop integrating
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
// which results in false gyro drift. See
// http://gentlenav.googlecode.com/files/fastRotations.pdf
#define SPIN_RATE_LIMIT 20
int32_t accSum[XYZ_AXIS_COUNT];
@ -60,14 +63,13 @@ uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
float accVelScale;
float throttleAngleScale;
float fc_acc;
float smallAngleCosZ = 0;
static float throttleAngleScale;
static float fc_acc;
static float smallAngleCosZ = 0;
float magneticDeclination = 0.0f; // calculated at startup from config
static float magneticDeclination = 0.0f; // calculated at startup from config
static imuRuntimeConfig_t imuRuntimeConfig;
static pidProfile_t *pidProfile;
STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to earth frame
static float rMat[3][3];
@ -100,18 +102,26 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
void imuConfigure(
imuConfig_t *imuConfig,
pidProfile_t *initialPidProfile,
uint16_t throttle_correction_angle
)
/*
* Calculate RC time constant used in the accZ lpf.
*/
static float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
{
imuRuntimeConfig.dcm_kp = imuConfig->dcm_kp / 10000.0f;
imuRuntimeConfig.dcm_ki = imuConfig->dcm_ki / 10000.0f;
imuRuntimeConfig.acc_unarmedcal = imuConfig->acc_unarmedcal;
imuRuntimeConfig.small_angle = imuConfig->small_angle;
return 0.5f / (M_PIf * accz_lpf_cutoff);
}
static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
}
void imuConfigure(uint16_t throttle_correction_angle)
{
imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f;
imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f;
imuRuntimeConfig.acc_unarmedcal = imuConfig()->acc_unarmedcal;
imuRuntimeConfig.small_angle = imuConfig()->small_angle;
pidProfile = initialPidProfile;
fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
}
@ -124,19 +134,6 @@ void imuInit(void)
imuComputeRotationMatrix();
}
float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
}
/*
* Calculate RC time constant used in the accZ lpf.
*/
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
{
return 0.5f / (M_PIf * accz_lpf_cutoff);
}
void imuResetAccelerationSum(void)
{
accSum[0] = 0;
@ -146,14 +143,12 @@ void imuResetAccelerationSum(void)
accTimeSum = 0;
}
void imuTransformVectorBodyToEarth(t_fp_vector * v)
static void imuTransformVectorBodyToEarth(t_fp_vector * v)
{
float x,y,z;
/* From body frame to earth frame */
x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
v->V.X = x;
v->V.Y = -y;
@ -161,16 +156,15 @@ void imuTransformVectorBodyToEarth(t_fp_vector * v)
}
// rotate acc into Earth frame and calculate acceleration in it
void imuCalculateAcceleration(uint32_t deltaT)
static void imuCalculateAcceleration(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
float dT;
t_fp_vector accel_ned;
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
const float dT = (float)deltaT * 1e-6f;
t_fp_vector accel_ned;
accel_ned.V.X = acc.accSmooth[X];
accel_ned.V.Y = acc.accSmooth[Y];
accel_ned.V.Z = acc.accSmooth[Z];
@ -183,8 +177,9 @@ void imuCalculateAcceleration(uint32_t deltaT)
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
} else {
accel_ned.V.Z -= acc.dev.acc_1G;
}
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
@ -224,15 +219,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useYaw, float yawError)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
float recipNorm;
float hx, hy, bx;
float ex = 0, ey = 0, ez = 0;
float qa, qb, qc;
// Calculate general spin rate (rad/s)
float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else)
float ez = 0;
if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
@ -241,7 +233,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Use measured magnetic field vector
recipNorm = sq(mx) + sq(my) + sq(mz);
float ex = 0, ey = 0;
float recipNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
@ -254,12 +247,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
bx = sqrtf(hx * hx + hy * hy);
const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
const float bx = sqrtf(hx * hx + hy * hy);
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
float ez_ef = -(hy * bx);
const float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex += rMat[2][0] * ez_ef;
@ -286,7 +279,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
if(imuRuntimeConfig.dcm_ki > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
float dcmKiGain = imuRuntimeConfig.dcm_ki;
const float dcmKiGain = imuRuntimeConfig.dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
@ -299,7 +292,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
const float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;
@ -311,9 +304,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
gy *= (0.5f * dt);
gz *= (0.5f * dt);
qa = q0;
qb = q1;
qc = q2;
const float qa = q0;
const float qb = q1;
const float qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
@ -350,10 +343,8 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
static bool imuIsAccelerometerHealthy(void)
{
int32_t axis;
int32_t accMagnitude = 0;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
accMagnitude += (int32_t)acc.accSmooth[axis] * acc.accSmooth[axis];
}

43
src/main/flight/imu.h
View file

@ -18,10 +18,9 @@
#pragma once
#include "common/axis.h"
#include "common/maths.h"
#include "common/time.h"
#include "sensors/acceleration.h"
#include "config/parameter_group.h"
// Exported symbols
extern uint32_t accTimeSum;
@ -30,11 +29,6 @@ extern float accVelScale;
extern int32_t accSum[XYZ_AXIS_COUNT];
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
typedef union {
int16_t raw[XYZ_AXIS_COUNT];
struct {
@ -52,11 +46,6 @@ typedef struct accDeadband_s {
uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations
} accDeadband_t;
typedef struct throttleCorrectionConfig_s {
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
} throttleCorrectionConfig_t;
typedef struct imuConfig_s {
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
@ -65,6 +54,8 @@ typedef struct imuConfig_s {
accDeadband_t accDeadband;
} imuConfig_t;
PG_DECLARE(imuConfig_t, imuConfig);
typedef struct imuRuntimeConfig_s {
float dcm_ki;
float dcm_kp;
@ -73,38 +64,12 @@ typedef struct imuRuntimeConfig_s {
accDeadband_t accDeadband;
} imuRuntimeConfig_t;
typedef enum {
ACCPROC_READ = 0,
ACCPROC_CHUNK_1,
ACCPROC_CHUNK_2,
ACCPROC_CHUNK_3,
ACCPROC_CHUNK_4,
ACCPROC_CHUNK_5,
ACCPROC_CHUNK_6,
ACCPROC_CHUNK_7,
ACCPROC_COPY
} accProcessorState_e;
typedef struct accProcessor_s {
accProcessorState_e state;
} accProcessor_t;
struct pidProfile_s;
void imuConfigure(
imuConfig_t *imuConfig,
struct pidProfile_s *initialPidProfile,
uint16_t throttle_correction_angle
);
void imuConfigure(uint16_t throttle_correction_angle);
float getCosTiltAngle(void);
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
void imuUpdateAttitude(timeUs_t currentTimeUs);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
union u_fp_vector;
int16_t imuCalculateHeading(union u_fp_vector *vec);
void imuResetAccelerationSum(void);
void imuInit(void);

83
src/main/flight/mixer.c
View file

@ -28,6 +28,10 @@
#include "common/maths.h"
#include "common/filter.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/pwm_output.h"
@ -46,9 +50,6 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "config/feature.h"
#include "config/config_master.h"
#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2
// (minimum output value(1001) - (minimum input value(48) / conversion factor(2))
#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977
@ -62,12 +63,6 @@ static float motorMixRange;
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static mixerConfig_t *mixerConfig;
static flight3DConfig_t *flight3DConfig;
static motorConfig_t *motorConfig;
static airplaneConfig_t *airplaneConfig;
rxConfig_t *rxConfig;
mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@ -236,8 +231,6 @@ const mixer_t mixers[] = {
};
#endif
static motorMixer_t *customMixers;
static uint16_t disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
uint16_t motorOutputHigh, motorOutputLow;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
@ -254,7 +247,7 @@ float getMotorMixRange()
bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT
switch(motorConfig->motorPwmProtocol) {
switch(motorConfig()->dev.motorPwmProtocol) {
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
@ -274,47 +267,31 @@ void initEscEndpoints(void) {
if (isMotorProtocolDshot()) {
disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_3D))
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent);
else
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent);
motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
} else
#endif
{
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand;
motorOutputLow = motorConfig->minthrottle;
motorOutputHigh = motorConfig->maxthrottle;
deadbandMotor3dHigh = flight3DConfig->deadband3d_high;
deadbandMotor3dLow = flight3DConfig->deadband3d_low;
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
motorOutputLow = motorConfig()->minthrottle;
motorOutputHigh = motorConfig()->maxthrottle;
deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
}
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck);
rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck);
rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
}
void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse,
motorConfig_t *motorConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
rxConfig_t *rxConfigToUse)
{
flight3DConfig = flight3DConfigToUse;
motorConfig = motorConfigToUse;
mixerConfig = mixerConfigToUse;
airplaneConfig = airplaneConfigToUse;
rxConfig = rxConfigToUse;
}
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
void mixerInit(mixerMode_e mixerMode)
{
currentMixerMode = mixerMode;
customMixers = initialCustomMixers;
initEscEndpoints();
}
@ -328,9 +305,9 @@ void mixerConfigureOutput(void)
// load custom mixer into currentMixer
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done
if (customMixers[i].throttle == 0.0f)
if (customMotorMixer(i)->throttle == 0.0f)
break;
currentMixer[i] = customMixers[i];
currentMixer[i] = *customMotorMixer(i);
motorCount++;
}
} else {
@ -441,25 +418,25 @@ void mixTable(pidProfile_t *pidProfile)
// Find min and max throttle based on condition.
if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Out of band handling
motorOutputMax = deadbandMotor3dLow;
motorOutputMin = motorOutputLow;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true;
} else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
} else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling
motorOutputMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
} else if ((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
motorOutputMax = deadbandMotor3dLow;
motorOutputMin = motorOutputLow;
throttle = rxConfig->midrc - flight3DConfig->deadband3d_throttle;
throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true;
} else { // Deadband handling from positive to negative
@ -469,7 +446,7 @@ void mixTable(pidProfile_t *pidProfile)
currentThrottleInputRange = rcCommandThrottleRange3dHigh;
}
} else {
throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
currentThrottleInputRange = rcCommandThrottleRange;
motorOutputMin = motorOutputLow;
motorOutputMax = motorOutputHigh;
@ -485,7 +462,7 @@ void mixTable(pidProfile_t *pidProfile)
}
// Calculate voltage compensation
const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f;
const float vbatCompensationFactor = pidProfile->vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f;
// Find roll/pitch/yaw desired output
float motorMix[MAX_SUPPORTED_MOTORS];
@ -494,7 +471,7 @@ void mixTable(pidProfile_t *pidProfile)
motorMix[i] =
scaledAxisPIDf[PITCH] * currentMixer[i].pitch +
scaledAxisPIDf[ROLL] * currentMixer[i].roll +
scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig->yaw_motor_direction);
scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig()->yaw_motor_direction);
if (vbatCompensationFactor > 1.0f) {
motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
@ -542,7 +519,7 @@ void mixTable(pidProfile_t *pidProfile)
// Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
if (((rcData[THROTTLE]) < rxConfig()->mincheck)) {
motor[i] = disarmMotorOutput;
}
}

36
src/main/flight/mixer.h
View file

@ -17,7 +17,9 @@
#pragma once
#define MAX_SUPPORTED_MOTORS 12
#include "config/parameter_group.h"
#include "drivers/io_types.h"
#include "drivers/pwm_output.h"
#define QUAD_MOTOR_COUNT 4
@ -85,6 +87,8 @@ typedef struct motorMixer_s {
float yaw;
} motorMixer_t;
PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer);
// Custom mixer configuration
typedef struct mixer_s {
uint8_t motorCount;
@ -97,38 +101,30 @@ typedef struct mixerConfig_s {
int8_t yaw_motor_direction;
} mixerConfig_t;
typedef struct flight3DConfig_s {
uint16_t deadband3d_low; // min 3d value
uint16_t deadband3d_high; // max 3d value
uint16_t neutral3d; // center 3d value
uint16_t deadband3d_throttle; // default throttle deadband from MIDRC
} flight3DConfig_t;
PG_DECLARE(mixerConfig_t, mixerConfig);
typedef struct airplaneConfig_s {
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
} airplaneConfig_t;
typedef struct motorConfig_s {
motorDevConfig_t dev;
float digitalIdleOffsetPercent;
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
} motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig);
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
extern const mixer_t mixers[];
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
struct motorConfig_s;
struct rxConfig_s;
uint8_t getMotorCount();
float getMotorMixRange();
void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse,
struct motorConfig_s *motorConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
struct rxConfig_s *rxConfigToUse);
void mixerLoadMix(int index, motorMixer_t *customMixers);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
void mixerInit(mixerMode_e mixerMode);
void mixerConfigureOutput(void);

49
src/main/flight/navigation.c
View file

@ -27,30 +27,33 @@
#include "build/debug.h"
#include "common/axis.h"
#include "common/gps_conversion.h"
#include "common/maths.h"
#include "common/time.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/acceleration.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "io/gps.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "flight/gps_conversion.h"
#include "flight/imu.h"
#include "rx/rx.h"
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
extern int16_t magHold;
@ -72,17 +75,9 @@ static int16_t nav[2];
static int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode
static gpsProfile_t *gpsProfile;
void gpsUseProfile(gpsProfile_t *gpsProfileToUse)
{
gpsProfile = gpsProfileToUse;
}
// When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile)
void navigationInit(pidProfile_t *pidProfile)
{
gpsUseProfile(initialGpsProfile);
gpsUsePIDs(pidProfile);
}
@ -163,7 +158,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
// Low pass filter cut frequency for derivative calculation
// Set to "1 / ( 2 * PI * gps_lpf )
float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile->gps_lpf));
float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile()->gps_lpf));
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);
@ -327,13 +322,13 @@ void onGpsNewData(void)
break;
case NAV_MODE_WP:
speed = GPS_calc_desired_speed(gpsProfile->nav_speed_max, NAV_SLOW_NAV); // slow navigation
speed = GPS_calc_desired_speed(gpsProfile()->nav_speed_max, NAV_SLOW_NAV); // slow navigation
// use error as the desired rate towards the target
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_nav_rate(speed);
// Tail control
if (gpsProfile->nav_controls_heading) {
if (gpsProfile()->nav_controls_heading) {
if (NAV_TAIL_FIRST) {
magHold = wrap_18000(nav_bearing - 18000) / 100;
} else {
@ -341,7 +336,7 @@ void onGpsNewData(void)
}
}
// Are we there yet ?(within x meters of the destination)
if ((wp_distance <= gpsProfile->gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode
if ((wp_distance <= gpsProfile()->gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode
nav_mode = NAV_MODE_POSHOLD;
if (NAV_SET_TAKEOFF_HEADING) {
magHold = nav_takeoff_bearing;
@ -431,7 +426,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
nav_bearing = target_bearing;
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
original_target_bearing = target_bearing;
waypoint_speed_gov = gpsProfile->nav_speed_min;
waypoint_speed_gov = gpsProfile()->nav_speed_min;
}
////////////////////////////////////////////////////////////////////////////////////
@ -610,7 +605,7 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
max_speed = MIN(max_speed, wp_distance / 2);
} else {
max_speed = MIN(max_speed, wp_distance);
max_speed = MAX(max_speed, gpsProfile->nav_speed_min); // go at least 100cm/s
max_speed = MAX(max_speed, gpsProfile()->nav_speed_min); // go at least 100cm/s
}
// limit the ramp up of the speed
@ -647,9 +642,9 @@ void updateGpsStateForHomeAndHoldMode(void)
{
float sin_yaw_y = sin_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
float cos_yaw_x = cos_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
if (gpsProfile->nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate);
if (gpsProfile()->nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile()->nav_slew_rate, gpsProfile()->nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile()->nav_slew_rate, gpsProfile()->nav_slew_rate);
GPS_angle[AI_ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[AI_PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else {
@ -693,7 +688,7 @@ void updateGpsWaypointsAndMode(void)
// process HOLD mode
//
if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile->ap_mode)) {
if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile()->ap_mode)) {
if (!FLIGHT_MODE(GPS_HOLD_MODE)) {
// Transition to HOLD mode

5
src/main/flight/navigation.h
View file

@ -36,6 +36,8 @@ typedef struct gpsProfile_s {
uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS
} gpsProfile_t;
PG_DECLARE(gpsProfile_t, gpsProfile);
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
extern int32_t GPS_home[2];
@ -47,11 +49,10 @@ extern int16_t GPS_directionToHome; // direction to home or hol point in
extern navigationMode_e nav_mode; // Navigation mode
struct pidProfile_s;
void navigationInit(gpsProfile_t *initialGpsProfile, struct pidProfile_s *pidProfile);
void navigationInit(struct pidProfile_s *pidProfile);
void GPS_reset_home_position(void);
void GPS_reset_nav(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon);
void gpsUseProfile(gpsProfile_t *gpsProfileToUse);
void gpsUsePIDs(struct pidProfile_s *pidProfile);
void updateGpsStateForHomeAndHoldMode(void);
void updateGpsWaypointsAndMode(void);

3
src/main/flight/pid.c
View file

@ -28,6 +28,9 @@
#include "common/maths.h"
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/fc_core.h"
#include "fc/fc_rc.h"

4
src/main/flight/pid.h
View file

@ -85,10 +85,14 @@ typedef struct pidProfile_s {
float rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
} pidProfile_t;
//PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
typedef struct pidConfig_s {
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
} pidConfig_t;
PG_DECLARE(pidConfig_t, pidConfig);
union rollAndPitchTrims_u;
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);

72
src/main/flight/servos.c
View file

@ -27,6 +27,10 @@
#include "build/build_config.h"
#include "common/filter.h"
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/pwm_output.h"
#include "drivers/system.h"
@ -34,11 +38,11 @@
#include "rx/rx.h"
#include "io/gimbal.h"
#include "io/servos.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/servos.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
@ -47,13 +51,9 @@
#include "config/feature.h"
extern mixerMode_e currentMixerMode;
extern rxConfig_t *rxConfig;
static servoMixerConfig_t *servoMixerConfig;
static uint8_t servoRuleCount = 0;
static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
static gimbalConfig_t *gimbalConfig;
int16_t servo[MAX_SUPPORTED_SERVOS];
static int useServo;
static servoParam_t *servoConf;
@ -111,6 +111,12 @@ static const servoMixer_t servoMixerGimbal[] = {
};
// Custom mixer configuration
typedef struct mixerRules_s {
uint8_t servoRuleCount;
const servoMixer_t *rule;
} mixerRules_t;
const mixerRules_t servoMixers[] = {
{ 0, NULL }, // entry 0
{ COUNT_SERVO_RULES(servoMixerTri), servoMixerTri }, // MULTITYPE_TRI
@ -141,19 +147,15 @@ const mixerRules_t servoMixers[] = {
{ 0, NULL },
};
static servoMixer_t *customServoMixers;
void servoUseConfigs(servoMixerConfig_t *servoMixerConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse)
void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse)
{
servoMixerConfig = servoMixerConfigToUse;
servoConf = servoParamsToUse;
gimbalConfig = gimbalConfigToUse;
channelForwardingConfig = channelForwardingConfigToUse;
}
int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
{
uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel;
const uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel;
if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) {
return rcData[channelToForwardFrom];
@ -162,7 +164,6 @@ int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
return servoConf[servoIndex].middle;
}
int servoDirection(int servoIndex, int inputSource)
{
// determine the direction (reversed or not) from the direction bitfield of the servo
@ -172,10 +173,8 @@ int servoDirection(int servoIndex, int inputSource)
return 1;
}
void servoMixerInit(servoMixer_t *initialCustomServoMixers)
void servosInit(void)
{
customServoMixers = initialCustomServoMixers;
// enable servos for mixes that require them. note, this shifts motor counts.
useServo = mixers[currentMixerMode].useServo;
// if we want camstab/trig, that also enables servos, even if mixer doesn't
@ -197,10 +196,10 @@ void loadCustomServoMixer(void)
// load custom mixer into currentServoMixer
for (uint8_t i = 0; i < MAX_SERVO_RULES; i++) {
// check if done
if (customServoMixers[i].rate == 0)
if (customServoMixers(i)->rate == 0)
break;
currentServoMixer[i] = customServoMixers[i];
currentServoMixer[i] = *customServoMixers(i);
servoRuleCount++;
}
}
@ -266,10 +265,15 @@ static void updateGimbalServos(uint8_t firstServoIndex)
pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
}
static void servoTable(void);
static void filterServos(void);
void writeServos(void)
{
uint8_t servoIndex = 0;
servoTable();
filterServos();
uint8_t servoIndex = 0;
switch (currentMixerMode) {
case MIXER_BICOPTER:
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
@ -278,7 +282,7 @@ void writeServos(void)
case MIXER_TRI:
case MIXER_CUSTOM_TRI:
if (servoMixerConfig->tri_unarmed_servo) {
if (servoConfig()->tri_unarmed_servo) {
// if unarmed flag set, we always move servo
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
} else {
@ -348,7 +352,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING;
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) {
input[INPUT_STABILIZED_YAW] *= -1;
}
}
@ -364,14 +368,14 @@ STATIC_UNIT_TESTED void servoMixer(void)
// 2000 - 1500 = +500
// 1500 - 1500 = 0
// 1000 - 1500 = -500
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc;
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig()->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig()->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig()->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig()->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig()->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig()->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig()->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig()->midrc;
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
servo[i] = 0;
@ -408,7 +412,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
}
void servoTable(void)
static void servoTable(void)
{
// airplane / servo mixes
switch (currentMixerMode) {
@ -442,7 +446,7 @@ void servoTable(void)
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) {
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
} else {
@ -463,7 +467,7 @@ bool isMixerUsingServos(void)
return useServo;
}
void filterServos(void)
static void filterServos(void)
{
static int16_t servoIdx;
static bool servoFilterIsSet;
@ -473,10 +477,10 @@ void filterServos(void)
uint32_t startTime = micros();
#endif
if (servoMixerConfig->servo_lowpass_enable) {
if (servoConfig()->servo_lowpass_freq) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime);
biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}

37
src/main/flight/servos.h
View file

@ -17,7 +17,9 @@
#pragma once
#define MAX_SUPPORTED_SERVOS 8
#include "config/parameter_group.h"
#include "drivers/io_types.h"
#include "drivers/pwm_output.h"
// These must be consecutive, see 'reversedSources'
enum {
@ -87,13 +89,10 @@ typedef struct servoMixer_s {
#define MAX_SERVO_SPEED UINT8_MAX
#define MAX_SERVO_BOXES 3
// Custom mixer configuration
typedef struct mixerRules_s {
uint8_t servoRuleCount;
const servoMixer_t *rule;
} mixerRules_t;
PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers);
typedef struct servoParam_s {
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
int16_t min; // servo min
int16_t max; // servo max
int16_t middle; // servo middle
@ -101,14 +100,17 @@ typedef struct servoParam_s {
uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value.
uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value.
int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
} __attribute__ ((__packed__)) servoParam_t;
} servoParam_t;
typedef struct servoMixerConfig_s{
PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams);
typedef struct servoConfig_s {
servoDevConfig_t dev;
uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
int8_t servo_lowpass_enable; // enable/disable lowpass filter
} servoMixerConfig_t;
} servoConfig_t;
PG_DECLARE(servoConfig_t, servoConfig);
typedef struct servoProfile_s {
servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
@ -120,16 +122,11 @@ typedef struct channelForwardingConfig_s {
extern int16_t servo[MAX_SUPPORTED_SERVOS];
void servoTable(void);
bool isMixerUsingServos(void);
void writeServos(void);
void filterServos(void);
void servoMixerInit(servoMixer_t *customServoMixers);
struct gimbalConfig_s;
void servoUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse);
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
void loadCustomServoMixer(void);
void servoConfigureOutput(void);
void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse);
int servoDirection(int servoIndex, int fromChannel);
void servoConfigureOutput(void);
void servosInit(void);

6
src/main/io/beeper.c
View file

@ -171,9 +171,9 @@ typedef struct beeperTableEntry_s {
{ BEEPER_ENTRY(BEEPER_ARMED, 15, beep_armedBeep, "ARMED") },
{ BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 16, NULL, "SYSTEM_INIT") },
{ BEEPER_ENTRY(BEEPER_USB, 17, NULL, "ON_USB") },
{ BEEPER_ENTRY(BEEPER_ALL, 18, NULL, "ALL") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 19, NULL, "PREFERRED") },
{ BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 18, beep_2shortBeeps, "BLACKBOX_ERASE") },
{ BEEPER_ENTRY(BEEPER_ALL, 19, NULL, "ALL") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 20, NULL, "PREFERRED") },
};
static const beeperTableEntry_t *currentBeeperEntry = NULL;

2
src/main/io/beeper.h
View file

@ -41,7 +41,7 @@ typedef enum {
BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased)
BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on
BEEPER_USB, // Some boards have beeper powered USB connected
BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes
BEEPER_ALL, // Turn ON or OFF all beeper conditions
BEEPER_PREFERENCE // Save preferred beeper configuration
// BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum

5
src/main/io/dashboard.c
View file

@ -84,7 +84,6 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex);
static uint32_t nextDisplayUpdateAt = 0;
static bool dashboardPresent = false;
static rxConfig_t *rxConfig;
static displayPort_t *displayPort;
#define PAGE_TITLE_LINE_COUNT 1
@ -701,7 +700,7 @@ void dashboardSetPage(pageId_e pageId)
pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
}
void dashboardInit(rxConfig_t *rxConfigToUse)
void dashboardInit(void)
{
delay(200);
resetDisplay();
@ -714,8 +713,6 @@ void dashboardInit(rxConfig_t *rxConfigToUse)
}
#endif
rxConfig = rxConfigToUse;
memset(&pageState, 0, sizeof(pageState));
dashboardSetPage(PAGE_WELCOME);

3
src/main/io/dashboard.h
View file

@ -37,8 +37,7 @@ typedef enum {
#endif
} pageId_e;
struct rxConfig_s;
void dashboardInit(struct rxConfig_s *intialRxConfig);
void dashboardInit(void);
void dashboardUpdate(timeUs_t currentTimeUs);
void dashboardShowFixedPage(pageId_e pageId);

15
src/main/io/displayport_max7456.c
View file

@ -24,13 +24,17 @@
#include "common/utils.h"
#include "config/config_master.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/display.h"
#include "drivers/max7456.h"
#include "drivers/vcd.h"
#include "io/displayport_max7456.h"
#include "io/osd.h"
displayPort_t max7456DisplayPort; // Referenced from osd.c
displayPortProfile_t *max7456DisplayPortProfile;
extern uint16_t refreshTimeout;
@ -102,8 +106,8 @@ static void resync(displayPort_t *displayPort)
{
UNUSED(displayPort);
max7456RefreshAll();
displayPort->rows = max7456GetRowsCount() + max7456DisplayPortProfile->rowAdjust;
displayPort->cols = 30 + max7456DisplayPortProfile->colAdjust;
displayPort->rows = max7456GetRowsCount() + displayPortProfileMax7456()->rowAdjust;
displayPort->cols = 30 + displayPortProfileMax7456()->colAdjust;
}
static int heartbeat(displayPort_t *displayPort)
@ -132,9 +136,8 @@ static const displayPortVTable_t max7456VTable = {
.txBytesFree = txBytesFree,
};
displayPort_t *max7456DisplayPortInit(const vcdProfile_t *vcdProfile, displayPortProfile_t *displayPortProfileToUse)
displayPort_t *max7456DisplayPortInit(const vcdProfile_t *vcdProfile)
{
max7456DisplayPortProfile = displayPortProfileToUse;
displayInit(&max7456DisplayPort, &max7456VTable);
max7456Init(vcdProfile);
resync(&max7456DisplayPort);

7
src/main/io/displayport_max7456.h
View file

@ -17,5 +17,10 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/display.h"
PG_DECLARE(displayPortProfile_t, displayPortProfileMax7456);
struct vcdProfile_s;
displayPort_t *max7456DisplayPortInit(const struct vcdProfile_s *vcdProfile, displayPortProfile_t *displayPortProfileToUse);
displayPort_t *max7456DisplayPortInit(const struct vcdProfile_s *vcdProfile);

13
src/main/io/displayport_msp.c
View file

@ -26,6 +26,8 @@
#include "common/utils.h"
#include "config/config_master.h"
#include "drivers/display.h"
#include "drivers/system.h"
@ -34,9 +36,9 @@
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
static displayPort_t mspDisplayPort;
#include "io/displayport_msp.h"
static displayPortProfile_t *mspDisplayPortProfile;
static displayPort_t mspDisplayPort;
static int output(displayPort_t *displayPort, uint8_t cmd, const uint8_t *buf, int len)
{
@ -118,8 +120,8 @@ static bool isTransferInProgress(const displayPort_t *displayPort)
static void resync(displayPort_t *displayPort)
{
displayPort->rows = 13 + mspDisplayPortProfile->rowAdjust; // XXX Will reflect NTSC/PAL in the future
displayPort->cols = 30 + mspDisplayPortProfile->colAdjust;
displayPort->rows = 13 + displayPortProfileMsp()->rowAdjust; // XXX Will reflect NTSC/PAL in the future
displayPort->cols = 30 + displayPortProfileMsp()->colAdjust;
}
static uint32_t txBytesFree(const displayPort_t *displayPort)
@ -142,9 +144,8 @@ static const displayPortVTable_t mspDisplayPortVTable = {
.txBytesFree = txBytesFree
};
displayPort_t *displayPortMspInit(displayPortProfile_t *displayPortProfileToUse)
displayPort_t *displayPortMspInit(void)
{
mspDisplayPortProfile = displayPortProfileToUse;
displayInit(&mspDisplayPort, &mspDisplayPortVTable);
resync(&mspDisplayPort);
return &mspDisplayPort;

7
src/main/io/displayport_msp.h
View file

@ -17,5 +17,10 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/display.h"
PG_DECLARE(displayPortProfile_t, displayPortProfileMsp);
struct displayPort_s;
struct displayPort_s *displayPortMspInit(displayPortProfile_t *displayPortProfileToUse);
struct displayPort_s *displayPortMspInit(void);

4
src/main/io/gimbal.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "config/parameter_group.h"
typedef enum {
GIMBAL_MODE_NORMAL = 0,
GIMBAL_MODE_MIXTILT = 1
@ -27,3 +29,5 @@ typedef enum {
typedef struct gimbalConfig_s {
uint8_t mode;
} gimbalConfig_t;
PG_DECLARE(gimbalConfig_t, gimbalConfig);

43
src/main/io/gps.c
View file

@ -29,27 +29,32 @@
#include "build/build_config.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/gps_conversion.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/light_led.h"
#include "sensors/sensors.h"
#include "drivers/light_led.h"
#include "drivers/system.h"
#include "io/serial.h"
#include "io/dashboard.h"
#include "io/gps.h"
#include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "io/serial.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "config/feature.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "sensors/sensors.h"
#define LOG_ERROR '?'
#define LOG_IGNORED '!'
@ -87,15 +92,12 @@ uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS]; // Carrier to Noise Ratio (Signal Strength)
static gpsConfig_t *gpsConfig;
// GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
#define GPS_TIMEOUT (2500)
// How many entries in gpsInitData array below
#define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
#define GPS_BAUDRATE_CHANGE_DELAY (200)
static serialConfig_t *serialConfig;
static serialPort_t *gpsPort;
typedef struct gpsInitData_s {
@ -202,19 +204,14 @@ static void gpsSetState(gpsState_e state)
gpsData.messageState = GPS_MESSAGE_STATE_IDLE;
}
void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
void gpsInit(void)
{
serialConfig = initialSerialConfig;
gpsData.baudrateIndex = 0;
gpsData.errors = 0;
gpsData.timeouts = 0;
memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
gpsConfig = initialGpsConfig;
// init gpsData structure. if we're not actually enabled, don't bother doing anything else
gpsSetState(GPS_UNKNOWN);
@ -237,7 +234,7 @@ void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig)
portMode_t mode = MODE_RXTX;
// only RX is needed for NMEA-style GPS
#if !defined(COLIBRI_RACE) || !defined(LUX_RACE)
if (gpsConfig->provider == GPS_NMEA)
if (gpsConfig()->provider == GPS_NMEA)
mode &= ~MODE_TX;
#endif
@ -345,7 +342,7 @@ void gpsInitUblox(void)
case GPS_CONFIGURE:
// Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->autoConfig == GPS_AUTOCONFIG_OFF ) {
if( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) {
gpsSetState(GPS_RECEIVING_DATA);
break;
}
@ -367,7 +364,7 @@ void gpsInitUblox(void)
if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]);
serialWrite(gpsPort, ubloxSbas[gpsConfig()->sbasMode].message[gpsData.state_position]);
gpsData.state_position++;
} else {
gpsData.messageState++;
@ -384,7 +381,7 @@ void gpsInitUblox(void)
void gpsInitHardware(void)
{
switch (gpsConfig->provider) {
switch (gpsConfig()->provider) {
case GPS_NMEA:
gpsInitNmea();
break;
@ -424,7 +421,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
case GPS_LOST_COMMUNICATION:
gpsData.timeouts++;
if (gpsConfig->autoBaud) {
if (gpsConfig()->autoBaud) {
// try another rate
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
@ -475,7 +472,7 @@ static void gpsNewData(uint16_t c)
bool gpsNewFrame(uint8_t c)
{
switch (gpsConfig->provider) {
switch (gpsConfig()->provider) {
case GPS_NMEA: // NMEA
return gpsNewFrameNMEA(c);
case GPS_UBLOX: // UBX binary

Some files were not shown because too many files have changed in this diff Show more