diff --git a/Makefile b/Makefile index aabdf94544..b6090bbf92 100644 --- a/Makefile +++ b/Makefile @@ -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)) diff --git a/reference/spektrum/Bi-Directional SRXL.pdf b/reference/spektrum/Bi-Directional SRXL.pdf deleted file mode 100644 index f073908efd..0000000000 Binary files a/reference/spektrum/Bi-Directional SRXL.pdf and /dev/null differ diff --git a/reference/spektrum/spektrumTelemetrySensors.h b/reference/spektrum/spektrumTelemetrySensors.h deleted file mode 100644 index 13bec79b99..0000000000 --- a/reference/spektrum/spektrumTelemetrySensors.h +++ /dev/null @@ -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 - diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 49ceee0848..82fbb81c34 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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; @@ -1460,18 +1469,24 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) 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) { diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 388ca2f75d..d085f531c4 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -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); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 307ada96c1..57852f2d15 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -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. */ diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 8a8087f180..9dc7b7566b 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -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); diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index 959c6e9d8d..0508f77e4f 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -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 diff --git a/src/main/build/version.h b/src/main/build/version.h index 400719fae5..4770ad98e9 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -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) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 33b01b35d1..ae791cd1fc 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -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 diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 7e5e891eeb..2301a1937c 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -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 }, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 26cce45584..f90c96f076 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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(¤tProfile->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(¤tProfile->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; } diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c index 45f9863f8c..e92f42c99a 100644 --- a/src/main/cms/cms_menu_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -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" diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 38d1e07160..c927aa66b1 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -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 }; diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 2d8b88f0ac..dc56cb7850 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -17,34 +17,60 @@ #include #include +#include #include #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 }; diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index 066fe8eeca..a07a776b1a 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -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; diff --git a/src/main/flight/gps_conversion.c b/src/main/common/gps_conversion.c similarity index 100% rename from src/main/flight/gps_conversion.c rename to src/main/common/gps_conversion.c diff --git a/src/main/flight/gps_conversion.h b/src/main/common/gps_conversion.h similarity index 100% rename from src/main/flight/gps_conversion.h rename to src/main/common/gps_conversion.h diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 78fd819d77..5283d6eba4 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -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)) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index 5c32f18183..295cd77b97 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -18,294 +18,285 @@ #include #include #include +#include #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); } diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index c87f07b4b7..640bc0299e 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -17,9 +17,12 @@ #pragma once +#include +#include + #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); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index fffa42e358..4537bfda3a 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -20,6 +20,7 @@ #include #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); diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index a0ffa53905..d1f2c513fd 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -17,7 +17,6 @@ #pragma once -#include "common/axis.h" #include "fc/config.h" #include "fc/rc_controls.h" #include "flight/pid.h" diff --git a/src/main/config/config_reset.h b/src/main/config/config_reset.h new file mode 100644 index 0000000000..5d13e12475 --- /dev/null +++ b/src/main/config/config_reset.h @@ -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 . + */ + +#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__ \ + }; \ + /**/ diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c new file mode 100644 index 0000000000..c4b6f6af03 --- /dev/null +++ b/src/main/config/config_streamer.c @@ -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 . + */ + +#include + +#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; +} diff --git a/src/main/config/config_streamer.h b/src/main/config/config_streamer.h new file mode 100644 index 0000000000..a62356fbea --- /dev/null +++ b/src/main/config/config_streamer.h @@ -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 . + */ + +#pragma once + +#include +#include + +// 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); diff --git a/src/main/config/feature.c b/src/main/config/feature.c index 44b3c12d8b..09410192e7 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -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; } diff --git a/src/main/config/feature.h b/src/main/config/feature.h index 11f6ff355f..2eb75f8a42 100644 --- a/src/main/config/feature.h +++ b/src/main/config/feature.h @@ -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); @@ -27,4 +35,4 @@ uint32_t featureMask(void); void intFeatureClearAll(uint32_t *features); void intFeatureSet(uint32_t mask, uint32_t *features); -void intFeatureClear(uint32_t mask, uint32_t *features); \ No newline at end of file +void intFeatureClear(uint32_t mask, uint32_t *features); diff --git a/src/main/config/parameter_group.c b/src/main/config/parameter_group.c index 2711639cc1..58dbe51e1a 100644 --- a/src/main/config/parameter_group.c +++ b/src/main/config/parameter_group.c @@ -19,6 +19,8 @@ #include #include +#include "platform.h" + #include "parameter_group.h" #include "common/maths.h" @@ -122,4 +124,3 @@ void pgActivateProfile(int profileIndex) } } } - diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 734933ca9c..33d49c6f4f 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -20,6 +20,8 @@ #include #include +#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); diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index c42ef32f0e..0d6d189f63 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -15,8 +15,12 @@ * along with Cleanflight. If not, see . */ +#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 diff --git a/src/main/drivers/accgyro_fake.c b/src/main/drivers/accgyro_fake.c index 9e661f55a6..105f6a33e4 100644 --- a/src/main/drivers/accgyro_fake.c +++ b/src/main/drivers/accgyro_fake.c @@ -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) diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 96fed71c3b..d61e4b666c 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -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) diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index b452afc774..9e7c3ea98f 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -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); +} diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 8ba01540a3..8a44af0464 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -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; diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index 15728d9e12..374604b7c1 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -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; } diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 631f7ec805..0a1fe6f336 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -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 } diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index f9a46fbfc1..30d7caa0fb 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -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); } diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index 29e22d5743..8d0af213df 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -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); diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 50c6f925a8..f4270cdb2d 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -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); diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index c3e06f0e56..84ecf1048a 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -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; diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index e51ee98055..9c7216529c 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -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; diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index e8de729409..29e3348347 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -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; diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c index efe512b1b6..c5fb482867 100644 --- a/src/main/drivers/adc_stm32f7xx.c +++ b/src/main/drivers/adc_stm32f7xx.c @@ -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; diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 9e5fa16889..a20dcdb762 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -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 } }; diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index ff0cd12d22..cfb160f031 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -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); diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 9028143afc..0be30a9dc0 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -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); diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 79fc8509e7..139ead18cc 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -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 diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index f3cec0d9f7..3569ff9a89 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -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(); diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 8011f04cf4..8f3fcd9d15 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -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; diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 25961579e4..341bbb81d9 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -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); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 8f99c16adc..2b36a8d18a 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -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; } } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 7102e7e351..f43ba861e1 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -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 diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 5ae299dcf0..fc5a8ff5f4 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -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; diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 25f753c0e2..0ce3ec006e 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -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; diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index b898493250..72a02d6660 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -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; diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index cf37e764ce..954d424465 100644 --- a/src/main/drivers/rx_pwm.c +++ b/src/main/drivers/rx_pwm.c @@ -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 diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index bbeec07ebf..b0f4b0d128 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -49,7 +49,7 @@ void systemBeepToggle(void) #endif } -void beeperInit(const beeperConfig_t *config) +void beeperInit(const beeperDevConfig_t *config) { #ifndef BEEPER UNUSED(config); diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index 5c6b8329bc..baeb54827f 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -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); diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 87af44df54..5ac340c31e 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -17,6 +17,9 @@ #pragma once +#include +#include + void systemInit(void); void delayMicroseconds(uint32_t us); void delay(uint32_t ms); diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 55d570eecb..0216d45427 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -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 diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index 3254c475d6..0099833674 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -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 diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 36e6be4d54..59aa7c2f7d 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -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 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e666b8e77d..d47339371f 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -31,6 +31,8 @@ uint8_t cliMode = 0; #ifdef USE_CLI +#include "blackbox/blackbox.h" + #include "build/build_config.h" #include "build/debug.h" #include "build/version.h" @@ -43,15 +45,18 @@ uint8_t cliMode = 0; #include "common/printf.h" #include "common/typeconversion.h" +#include "config/config_master.h" #include "config/config_eeprom.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/accgyro.h" #include "drivers/buf_writer.h" #include "drivers/bus_i2c.h" #include "drivers/compass.h" +#include "drivers/display.h" #include "drivers/dma.h" #include "drivers/flash.h" #include "drivers/io.h" @@ -65,18 +70,19 @@ uint8_t cliMode = 0; #include "drivers/system.h" #include "drivers/timer.h" #include "drivers/vcd.h" -#include "drivers/display.h" +#include "fc/cli.h" #include "fc/config.h" +#include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "fc/cli.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" @@ -84,10 +90,8 @@ uint8_t cliMode = 0; #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/servos.h" #include "io/vtx.h" #include "rx/rx.h" @@ -113,16 +117,6 @@ static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128]; static char cliBuffer[48]; static uint32_t bufferIndex = 0; -typedef enum { - DUMP_MASTER = (1 << 0), - DUMP_PROFILE = (1 << 1), - DUMP_RATES = (1 << 2), - DUMP_ALL = (1 << 3), - DO_DIFF = (1 << 4), - SHOW_DEFAULTS = (1 << 5), - HIDE_UNUSED = (1 << 6) -} dumpFlags_e; - static const char* const emptyName = "-"; #ifndef USE_QUAD_MIXER_ONLY @@ -399,6 +393,7 @@ typedef enum { #ifdef OSD TABLE_OSD, #endif + LOOKUP_TABLE_COUNT } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -448,21 +443,21 @@ static const lookupTableEntry_t lookupTables[] = { #define VALUE_MODE_OFFSET 6 typedef enum { - // value type + // value type, bits 0-3 VAR_UINT8 = (0 << VALUE_TYPE_OFFSET), VAR_INT8 = (1 << VALUE_TYPE_OFFSET), VAR_UINT16 = (2 << VALUE_TYPE_OFFSET), VAR_INT16 = (3 << VALUE_TYPE_OFFSET), //VAR_UINT32 = (4 << VALUE_TYPE_OFFSET), - VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), + VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), // 0x05 - // value section + // value section, bits 4-5 MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET), - PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), + PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), // 0x20 // value mode - MODE_DIRECT = (0 << VALUE_MODE_OFFSET), - MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) + MODE_DIRECT = (0 << VALUE_MODE_OFFSET), // 0x40 + MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) // 0x80 } cliValueFlag_e; #define VALUE_TYPE_MASK (0x0F) @@ -483,6 +478,22 @@ typedef union { cliMinMaxConfig_t minmax; } cliValueConfig_t; +#ifdef USE_PARAMETER_GROUPS +typedef struct { + const char *name; + const uint8_t type; // see cliValueFlag_e + const cliValueConfig_t config; + + pgn_t pgn; + uint16_t offset; +} __attribute__((packed)) clivalue_t; + +static const clivalue_t valueTable[] = { + { "dummy", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, 0, 0 } +}; + +#else + typedef struct { const char *name; const uint8_t type; // see cliValueFlag_e @@ -490,7 +501,7 @@ typedef struct { const cliValueConfig_t config; } clivalue_t; -const clivalue_t valueTable[] = { +static const clivalue_t valueTable[] = { #ifndef SKIP_TASK_STATISTICS { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.task_statistics, .config.lookup = { TABLE_OFF_ON } }, #endif @@ -508,7 +519,7 @@ const clivalue_t valueTable[] = { #endif { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, &rxConfig()->fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &rxConfig()->max_aux_channel, .config.minmax = { 0, MAX_AUX_CHANNEL_COUNT } }, - { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } }, + { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &systemConfig()->debug_mode, .config.lookup = { TABLE_DEBUG } }, { "min_throttle", VAR_UINT16 | MASTER_VALUE, &motorConfig()->minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &motorConfig()->maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, @@ -521,9 +532,10 @@ const clivalue_t valueTable[] = { { "3d_neutral", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, - { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, - { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &motorConfig()->motorPwmRate, .config.minmax = { 200, 32000 } }, + { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->dev.useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, + { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->dev.motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, + { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &motorConfig()->dev.motorPwmRate, .config.minmax = { 200, 32000 } }, + { "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE, &motorConfig()->dev.motorPwmInversion, .config.lookup = { TABLE_OFF_ON } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } }, @@ -558,8 +570,8 @@ const clivalue_t valueTable[] = { #endif #ifdef BEEPER - { "beeper_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &beeperConfig()->isInverted, .config.lookup = { TABLE_OFF_ON } }, - { "beeper_od", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &beeperConfig()->isOpenDrain, .config.lookup = { TABLE_OFF_ON } }, + { "beeper_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &beeperDevConfig()->isInverted, .config.lookup = { TABLE_OFF_ON } }, + { "beeper_od", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &beeperDevConfig()->isOpenDrain, .config.lookup = { TABLE_OFF_ON } }, #endif #ifdef SERIAL_RX @@ -646,11 +658,10 @@ const clivalue_t valueTable[] = { { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } }, #ifdef USE_SERVOS - { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, - { "servo_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &servoMixerConfig()->servo_lowpass_freq, .config.minmax = { 10, 400} }, - { "servo_lowpass", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, - { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoPwmRate, .config.minmax = { 50, 498 } }, + { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->dev.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, + { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, + { "servo_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servo_lowpass_freq, .config.minmax = { 0, 400} }, + { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->dev.servoPwmRate, .config.minmax = { 50, 498 } }, { "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } }, { "channel_forwarding_start", VAR_UINT8 | MASTER_VALUE, &channelForwardingConfig()->startChannel, .config.minmax = { AUX1, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, #endif @@ -770,34 +781,34 @@ const clivalue_t valueTable[] = { { "sdcard_dma", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sdcardConfig()->useDma, .config.lookup = { TABLE_OFF_ON } }, #endif #ifdef OSD - { "osd_units", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &osdProfile()->units, .config.lookup = { TABLE_UNIT } }, + { "osd_units", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &osdConfig()->units, .config.lookup = { TABLE_UNIT } }, - { "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, &osdProfile()->rssi_alarm, .config.minmax = { 0, 100 } }, - { "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->cap_alarm, .config.minmax = { 0, 20000 } }, - { "osd_time_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->time_alarm, .config.minmax = { 0, 60 } }, - { "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->alt_alarm, .config.minmax = { 0, 10000 } }, + { "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, &osdConfig()->rssi_alarm, .config.minmax = { 0, 100 } }, + { "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, &osdConfig()->cap_alarm, .config.minmax = { 0, 20000 } }, + { "osd_time_alarm", VAR_UINT16 | MASTER_VALUE, &osdConfig()->time_alarm, .config.minmax = { 0, 60 } }, + { "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, &osdConfig()->alt_alarm, .config.minmax = { 0, 10000 } }, - { "osd_vbat_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYTIME], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_ontimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ONTIME], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYMODE], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_crosshairs", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CROSSHAIRS], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_horizon_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_current_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SPEED], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SATS], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ALTITUDE], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ROLL_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PITCH_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_YAW_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_power_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_POWER], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_pidrate_profile_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PIDRATE_PROFILE], .config.minmax = { 0, OSD_POSCFG_MAX } }, - { "osd_battery_warning_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_WARNING], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_vbat_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_FLYTIME], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_ontimer_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_ONTIME], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_FLYMODE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_crosshairs", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_CROSSHAIRS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_horizon_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_current_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_GPS_SPEED], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_GPS_SATS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_ALTITUDE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_ROLL_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_PITCH_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_YAW_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_power_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_POWER], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_pidrate_profile_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_PIDRATE_PROFILE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_battery_warning_pos", VAR_UINT16 | MASTER_VALUE, &osdConfig()->item_pos[OSD_MAIN_BATT_WARNING], .config.minmax = { 0, OSD_POSCFG_MAX } }, #endif #ifdef USE_MAX7456 { "vcd_video_system", VAR_UINT8 | MASTER_VALUE, &vcdProfile()->video_system, .config.minmax = { 0, 2 } }, @@ -813,8 +824,70 @@ const clivalue_t valueTable[] = { { "displayport_max7456_row_adjust", VAR_INT8 | MASTER_VALUE, &displayPortProfileMax7456()->rowAdjust, .config.minmax = { -3, 0 } }, #endif }; +#endif -#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) +#ifdef USE_PARAMETER_GROUPS +static featureConfig_t featureConfigCopy; +static gyroConfig_t gyroConfigCopy; +static accelerometerConfig_t accelerometerConfigCopy; +#ifdef MAG +static compassConfig_t compassConfigCopy; +#endif +#ifdef BARO +static barometerConfig_t barometerConfigCopy; +#endif +#ifdef PITOT +static pitotmeterConfig_t pitotmeterConfigCopy; +#endif +static featureConfig_t featureConfigCopy; +static rxConfig_t rxConfigCopy; +#ifdef BLACKBOX +static blackboxConfig_t blackboxConfigCopy; +#endif +static rxFailsafeChannelConfig_t rxFailsafeChannelConfigsCopy[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +static rxChannelRangeConfig_t rxChannelRangeConfigsCopy[NON_AUX_CHANNEL_COUNT]; +static motorConfig_t motorConfigCopy; +static failsafeConfig_t failsafeConfigCopy; +static boardAlignment_t boardAlignmentCopy; +#ifdef USE_SERVOS +static servoConfig_t servoConfigCopy; +static gimbalConfig_t gimbalConfigCopy; +static servoMixer_t customServoMixersCopy[MAX_SERVO_RULES]; +static servoParam_t servoParamsCopy[MAX_SUPPORTED_SERVOS]; +#endif +static batteryConfig_t batteryConfigCopy; +static motorMixer_t customMotorMixerCopy[MAX_SUPPORTED_MOTORS]; +static mixerConfig_t mixerConfigCopy; +static flight3DConfig_t flight3DConfigCopy; +static serialConfig_t serialConfigCopy; +static imuConfig_t imuConfigCopy; +static armingConfig_t armingConfigCopy; +static rcControlsConfig_t rcControlsConfigCopy; +#ifdef GPS +static gpsConfig_t gpsConfigCopy; +#endif +#ifdef NAV +static positionEstimationConfig_t positionEstimationConfigCopy; +static navConfig_t navConfigCopy; +#endif +#ifdef TELEMETRY +static telemetryConfig_t telemetryConfigCopy; +#endif +static modeActivationCondition_t modeActivationConditionsCopy[MAX_MODE_ACTIVATION_CONDITION_COUNT]; +static adjustmentRange_t adjustmentRangesCopy[MAX_ADJUSTMENT_RANGE_COUNT]; +#ifdef LED_STRIP +static ledStripConfig_t ledStripConfigCopy; +#endif +#ifdef OSD +static osd_profile_t osdConfigCopy; +#endif +static systemConfig_t systemConfigCopy; +#ifdef BEEPER +static beeperDevConfig_t beeperDevConfigCopy; +#endif +static controlRateConfig_t controlRateProfilesCopy[MAX_CONTROL_RATE_PROFILE_COUNT]; +static pidProfile_t pidProfileCopy[MAX_PROFILE_COUNT]; +#endif // USE_PARAMETER_GROUPS static void cliPrint(const char *str) { @@ -840,6 +913,16 @@ static void cliPutp(void *p, char ch) bufWriterAppend(p, ch); } +typedef enum { + DUMP_MASTER = (1 << 0), + DUMP_PROFILE = (1 << 1), + DUMP_RATES = (1 << 2), + DUMP_ALL = (1 << 3), + DO_DIFF = (1 << 4), + SHOW_DEFAULTS = (1 << 5), + HIDE_UNUSED = (1 << 6) +} dumpFlags_e; + static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...) { if (!((dumpMask & DO_DIFF) && equalsDefault)) { @@ -884,55 +967,341 @@ static void cliPrintf(const char *format, ...) bufWriterFlush(cliWriter); } -static void printValuePointer(const clivalue_t *var, void *valuePointer, uint32_t full) +static void printValuePointer(const clivalue_t *var, const void *valuePointer, uint32_t full) { int32_t value = 0; char buf[8]; switch (var->type & VALUE_TYPE_MASK) { - case VAR_UINT8: - value = *(uint8_t *)valuePointer; - break; + case VAR_UINT8: + value = *(uint8_t *)valuePointer; + break; - case VAR_INT8: - value = *(int8_t *)valuePointer; - break; + case VAR_INT8: + value = *(int8_t *)valuePointer; + break; - case VAR_UINT16: - value = *(uint16_t *)valuePointer; - break; + case VAR_UINT16: + value = *(uint16_t *)valuePointer; + break; - case VAR_INT16: - value = *(int16_t *)valuePointer; - break; + case VAR_INT16: + value = *(int16_t *)valuePointer; + break; /* not currently used - case VAR_UINT32: - value = *(uint32_t *)valuePointer; - break; */ + case VAR_UINT32: + value = *(uint32_t *)valuePointer; + break; */ - case VAR_FLOAT: - cliPrintf("%s", ftoa(*(float *)valuePointer, buf)); - if (full && (var->type & VALUE_MODE_MASK) == MODE_DIRECT) { - cliPrintf(" %s", ftoa((float)var->config.minmax.min, buf)); - cliPrintf(" %s", ftoa((float)var->config.minmax.max, buf)); - } - return; // return from case for float only + case VAR_FLOAT: + cliPrintf("%s", ftoa(*(float *)valuePointer, buf)); + if (full && (var->type & VALUE_MODE_MASK) == MODE_DIRECT) { + cliPrintf(" %s", ftoa((float)var->config.minmax.min, buf)); + cliPrintf(" %s", ftoa((float)var->config.minmax.max, buf)); + } + return; // return from case for float only } switch(var->type & VALUE_MODE_MASK) { - case MODE_DIRECT: - cliPrintf("%d", value); - if (full) { - cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); - } - break; - case MODE_LOOKUP: - cliPrintf(lookupTables[var->config.lookup.tableIndex].values[value]); - break; + case MODE_DIRECT: + cliPrintf("%d", value); + if (full) { + cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); + } + break; + case MODE_LOOKUP: + cliPrintf(lookupTables[var->config.lookup.tableIndex].values[value]); + break; } } +#ifdef USE_PARAMETER_GROUPS + +static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptrDefault) +{ + bool result = false; + switch (type & VALUE_TYPE_MASK) { + case VAR_UINT8: + result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; + break; + + case VAR_INT8: + result = *(int8_t *)ptr == *(int8_t *)ptrDefault; + break; + + case VAR_UINT16: + result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault; + break; + + case VAR_INT16: + result = *(int16_t *)ptr == *(int16_t *)ptrDefault; + break; + +/* not currently used + case VAR_UINT32: + result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault; + break;*/ + + case VAR_FLOAT: + result = *(float *)ptr == *(float *)ptrDefault; + break; + } + return result; +} + +typedef struct cliCurrentAndDefaultConfig_s { + const void *currentConfig; // the copy + const void *defaultConfig; // the PG value as set by default +} cliCurrentAndDefaultConfig_t; + +static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn) +{ + static cliCurrentAndDefaultConfig_t ret; + + switch (pgn) { + case PG_GYRO_CONFIG: + ret.currentConfig = &gyroConfigCopy; + ret.defaultConfig = gyroConfig(); + break; + case PG_ACCELEROMETER_CONFIG: + ret.currentConfig = &accelerometerConfigCopy; + ret.defaultConfig = accelerometerConfig(); + break; +#ifdef MAG + case PG_COMPASS_CONFIG: + ret.currentConfig = &compassConfigCopy; + ret.defaultConfig = compassConfig(); + break; +#endif +#ifdef BARO + case PG_BAROMETER_CONFIG: + ret.currentConfig = &barometerConfigCopy; + ret.defaultConfig = barometerConfig(); + break; +#endif +#ifdef PITOT + case PG_PITOTMETER_CONFIG: + ret.currentConfig = &pitotmeterConfigCopy; + ret.defaultConfig = pitotmeterConfig(); + break; +#endif + case PG_FEATURE_CONFIG: + ret.currentConfig = &featureConfigCopy; + ret.defaultConfig = featureConfig(); + break; + case PG_RX_CONFIG: + ret.currentConfig = &rxConfigCopy; + ret.defaultConfig = rxConfig(); + break; +#ifdef BLACKBOX + case PG_BLACKBOX_CONFIG: + ret.currentConfig = &blackboxConfigCopy; + ret.defaultConfig = blackboxConfig(); + break; +#endif + case PG_MOTOR_CONFIG: + ret.currentConfig = &motorConfigCopy; + ret.defaultConfig = motorConfig(); + break; + case PG_FAILSAFE_CONFIG: + ret.currentConfig = &failsafeConfigCopy; + ret.defaultConfig = failsafeConfig(); + break; + case PG_BOARD_ALIGNMENT: + ret.currentConfig = &boardAlignmentCopy; + ret.defaultConfig = boardAlignment(); + break; + case PG_MIXER_CONFIG: + ret.currentConfig = &mixerConfigCopy; + ret.defaultConfig = mixerConfig(); + break; + case PG_MOTOR_3D_CONFIG: + ret.currentConfig = &flight3DConfigCopy; + ret.defaultConfig = flight3DConfig(); + break; +#ifdef USE_SERVOS + case PG_SERVO_CONFIG: + ret.currentConfig = &servoConfigCopy; + ret.defaultConfig = servoConfig(); + break; + case PG_GIMBAL_CONFIG: + ret.currentConfig = &gimbalConfigCopy; + ret.defaultConfig = gimbalConfig(); + break; +#endif + case PG_BATTERY_CONFIG: + ret.currentConfig = &batteryConfigCopy; + ret.defaultConfig = batteryConfig(); + break; + case PG_SERIAL_CONFIG: + ret.currentConfig = &serialConfigCopy; + ret.defaultConfig = serialConfig(); + break; + case PG_IMU_CONFIG: + ret.currentConfig = &imuConfigCopy; + ret.defaultConfig = imuConfig(); + break; + case PG_RC_CONTROLS_CONFIG: + ret.currentConfig = &rcControlsConfigCopy; + ret.defaultConfig = rcControlsConfig(); + break; + case PG_ARMING_CONFIG: + ret.currentConfig = &armingConfigCopy; + ret.defaultConfig = armingConfig(); + break; +#ifdef GPS + case PG_GPS_CONFIG: + ret.currentConfig = &gpsConfigCopy; + ret.defaultConfig = gpsConfig(); + break; +#endif +#ifdef NAV + case PG_POSITION_ESTIMATION_CONFIG: + ret.currentConfig = &positionEstimationConfigCopy; + ret.defaultConfig = positionEstimationConfig(); + break; + case PG_NAV_CONFIG: + ret.currentConfig = &navConfigCopy; + ret.defaultConfig = navConfig(); + break; +#endif +#ifdef TELEMETRY + case PG_TELEMETRY_CONFIG: + ret.currentConfig = &telemetryConfigCopy; + ret.defaultConfig = telemetryConfig(); + break; +#endif +#ifdef LED_STRIP + case PG_LED_STRIP_CONFIG: + ret.currentConfig = &ledStripConfigCopy; + ret.defaultConfig = ledStripConfig(); + break; +#endif +#ifdef OSD + case PG_OSD_CONFIG: + ret.currentConfig = &osdConfigCopy; + ret.defaultConfig = osdConfig(); + break; +#endif + case PG_SYSTEM_CONFIG: + ret.currentConfig = &systemConfigCopy; + ret.defaultConfig = systemConfig(); + break; + case PG_CONTROL_RATE_PROFILES: + ret.currentConfig = &controlRateProfilesCopy[0]; + ret.defaultConfig = controlRateProfiles(0); + break; +/*!!TODO case PG_PID_PROFILE: + ret.currentConfig = &pidProfileCopy[getConfigProfile()]; + ret.defaultConfig = pidProfile(); + break;*/ + case PG_RX_FAILSAFE_CHANNEL_CONFIG: + ret.currentConfig = &rxFailsafeChannelConfigsCopy[0]; + ret.defaultConfig = rxFailsafeChannelConfigs(0); + break; + case PG_RX_CHANNEL_RANGE_CONFIG: + ret.currentConfig = &rxChannelRangeConfigsCopy[0]; + ret.defaultConfig = rxChannelRangeConfigs(0); + break; +#ifdef USE_SERVOS + case PG_SERVO_MIXER: + ret.currentConfig = &customServoMixersCopy[0]; + ret.defaultConfig = customServoMixers(0); + break; + case PG_SERVO_PARAMS: + ret.currentConfig = &servoParamsCopy[0]; + ret.defaultConfig = servoParams(0); + break; +#endif + case PG_MOTOR_MIXER: + ret.currentConfig = &customMotorMixerCopy[0]; + ret.defaultConfig = customMotorMixer(0); + break; + case PG_MODE_ACTIVATION_PROFILE: + ret.currentConfig = &modeActivationConditionsCopy[0]; + ret.defaultConfig = modeActivationConditions(0); + break; + case PG_ADJUSTMENT_RANGE_CONFIG: + ret.currentConfig = &adjustmentRangesCopy[0]; + ret.defaultConfig = adjustmentRanges(0); + break; + case PG_BEEPER_CONFIG: + ret.currentConfig = &beeperDevConfigCopy; + ret.defaultConfig = beeperDevConfig(); + break; + default: + ret.currentConfig = NULL; + ret.defaultConfig = NULL; + break; + } + return &ret; +} + +static uint16_t getValueOffset(const clivalue_t *value) +{ + switch (value->type & VALUE_SECTION_MASK) { + case MASTER_VALUE: + case PROFILE_VALUE: + return value->offset; + case PROFILE_RATE_VALUE: + return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); + } + return 0; +} + +static void *getValuePointer(const clivalue_t *value) +{ + const pgRegistry_t* rec = pgFind(value->pgn); + + switch (value->type & VALUE_SECTION_MASK) { + case MASTER_VALUE: + case PROFILE_VALUE: + return rec->address + value->offset; + case PROFILE_RATE_VALUE: + return rec->address + value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); + } + return NULL; +} + +static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) +{ + const char *format = "set %s = "; + const cliCurrentAndDefaultConfig_t *config = getCurrentAndDefaultConfigs(value->pgn); + if (config->currentConfig == NULL || config->defaultConfig == NULL) { + // has not been set up properly + cliPrintf("VALUE %s ERROR\r\n", value->name); + return; + } + const int valueOffset = getValueOffset(value); + switch (dumpMask & (DO_DIFF | SHOW_DEFAULTS)) { + case DO_DIFF: + if (valuePtrEqualsDefault(value->type, (uint8_t*)config->currentConfig + valueOffset, (uint8_t*)config->defaultConfig + valueOffset)) { + break; + } + // drop through, since not equal to default + case 0: + case SHOW_DEFAULTS: + cliPrintf(format, value->name); + printValuePointer(value, (uint8_t*)config->currentConfig + valueOffset, 0); + cliPrint("\r\n"); + break; + } +} + +static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) +{ + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { + const clivalue_t *value = &valueTable[i]; + bufWriterFlush(cliWriter); + if ((value->type & VALUE_SECTION_MASK) == valueSection) { + dumpPgValue(value, dumpMask); + } + } +} + +#else + void *getValuePointer(const clivalue_t *value) { void *ptr = value->ptr; @@ -947,6 +1316,7 @@ void *getValuePointer(const clivalue_t *value) return ptr; } +#endif static void *getDefaultPointer(void *valuePointer, const master_t *defaultConfig) { @@ -961,37 +1331,37 @@ static bool valueEqualsDefault(const clivalue_t *value, const master_t *defaultC bool result = false; switch (value->type & VALUE_TYPE_MASK) { - case VAR_UINT8: - result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; - break; + case VAR_UINT8: + result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; + break; - case VAR_INT8: - result = *(int8_t *)ptr == *(int8_t *)ptrDefault; - break; + case VAR_INT8: + result = *(int8_t *)ptr == *(int8_t *)ptrDefault; + break; - case VAR_UINT16: - result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault; - break; + case VAR_UINT16: + result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault; + break; - case VAR_INT16: - result = *(int16_t *)ptr == *(int16_t *)ptrDefault; - break; + case VAR_INT16: + result = *(int16_t *)ptr == *(int16_t *)ptrDefault; + break; /* not currently used - case VAR_UINT32: - result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault; - break; */ + case VAR_UINT32: + result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault; + break; */ - case VAR_FLOAT: - result = *(float *)ptr == *(float *)ptrDefault; - break; + case VAR_FLOAT: + result = *(float *)ptr == *(float *)ptrDefault; + break; } return result; } static void cliPrintVar(const clivalue_t *var, uint32_t full) { - void *ptr = getValuePointer(var); + const void *ptr = getValuePointer(var); printValuePointer(var, ptr, full); } @@ -1008,7 +1378,7 @@ static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, const maste static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *defaultConfig) { const clivalue_t *value; - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { value = &valueTable[i]; if ((value->type & VALUE_SECTION_MASK) != valueSection) { @@ -1030,21 +1400,21 @@ static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t * static void cliPrintVarRange(const clivalue_t *var) { switch (var->type & VALUE_MODE_MASK) { - case (MODE_DIRECT): { - cliPrintf("Allowed range: %d - %d\r\n", var->config.minmax.min, var->config.minmax.max); + case (MODE_DIRECT): { + cliPrintf("Allowed range: %d - %d\r\n", var->config.minmax.min, var->config.minmax.max); + } + break; + case (MODE_LOOKUP): { + const lookupTableEntry_t *tableEntry = &lookupTables[var->config.lookup.tableIndex]; + cliPrint("Allowed values:"); + for (uint32_t i = 0; i < tableEntry->valueCount ; i++) { + if (i > 0) + cliPrint(","); + cliPrintf(" %s", tableEntry->values[i]); } - break; - case (MODE_LOOKUP): { - const lookupTableEntry_t *tableEntry = &lookupTables[var->config.lookup.tableIndex]; - cliPrint("Allowed values:"); - for (uint32_t i = 0; i < tableEntry->valueCount ; i++) { - if (i > 0) - cliPrint(","); - cliPrintf(" %s", tableEntry->values[i]); - } - cliPrint("\r\n"); - } - break; + cliPrint("\r\n"); + } + break; } } @@ -1055,33 +1425,27 @@ typedef union { static void cliSetVar(const clivalue_t *var, const int_float_value_t value) { - void *ptr = var->ptr; - if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); - } - if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); - } + void *ptr = getValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { - case VAR_UINT8: - case VAR_INT8: - *(int8_t *)ptr = value.int_value; - break; + case VAR_UINT8: + case VAR_INT8: + *(int8_t *)ptr = value.int_value; + break; - case VAR_UINT16: - case VAR_INT16: - *(int16_t *)ptr = value.int_value; - break; + case VAR_UINT16: + case VAR_INT16: + *(int16_t *)ptr = value.int_value; + break; /* not currently used - case VAR_UINT32: - *(uint32_t *)ptr = value.int_value; - break; */ + case VAR_UINT32: + *(uint32_t *)ptr = value.int_value; + break; */ - case VAR_FLOAT: - *(float *)ptr = (float)value.float_value; - break; + case VAR_FLOAT: + *(float *)ptr = (float)value.float_value; + break; } } @@ -1110,9 +1474,9 @@ static void cliShowArgumentRangeError(char *name, int min, int max) cliPrintf("%s not between %d and %d\r\n", name, min, max); } -static char *nextArg(char *currentArg) +static const char *nextArg(const char *currentArg) { - char *ptr = strchr(currentArg, ' '); + const char *ptr = strchr(currentArg, ' '); while (ptr && *ptr == ' ') { ptr++; } @@ -1120,14 +1484,12 @@ static char *nextArg(char *currentArg) return ptr; } -static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount) +static const char *processChannelRangeArgs(const char *ptr, channelRange_t *range, uint8_t *validArgumentCount) { - int val; - for (uint32_t argIndex = 0; argIndex < 2; argIndex++) { ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); val = CHANNEL_VALUE_TO_STEP(val); if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) { if (argIndex == 0) { @@ -1146,49 +1508,45 @@ static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t * // Check if a string's length is zero static bool isEmpty(const char *string) { - return *string == '\0'; + return (string == NULL || *string == '\0') ? true : false; } -static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) +static void printRxFailsafe(uint8_t dumpMask, const rxFailsafeChannelConfig_t *rxFailsafeChannelConfigs, const rxFailsafeChannelConfig_t *defaultRxFailsafeChannelConfigs) { // print out rxConfig failsafe settings for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { - const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; - const rxFailsafeChannelConfiguration_t *channelFailsafeConfigurationDefault; - bool equalsDefault = false; - if (defaultRxConfig) { - channelFailsafeConfigurationDefault = &defaultRxConfig->failsafe_channel_configurations[channel]; - equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode - && channelFailsafeConfiguration->step == channelFailsafeConfigurationDefault->step; - } - const bool requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; + const rxFailsafeChannelConfig_t *channelFailsafeConfig = &rxFailsafeChannelConfigs[channel]; + const rxFailsafeChannelConfig_t *defaultChannelFailsafeConfig = &defaultRxFailsafeChannelConfigs[channel]; + const bool equalsDefault = channelFailsafeConfig->mode == defaultChannelFailsafeConfig->mode + && channelFailsafeConfig->step == defaultChannelFailsafeConfig->step; + const bool requireValue = channelFailsafeConfig->mode == RX_FAILSAFE_MODE_SET; if (requireValue) { const char *format = "rxfail %u %c %d\r\n"; cliDefaultPrintf(dumpMask, equalsDefault, format, channel, - rxFailsafeModeCharacters[channelFailsafeConfigurationDefault->mode], - RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfigurationDefault->step) + rxFailsafeModeCharacters[defaultChannelFailsafeConfig->mode], + RXFAIL_STEP_TO_CHANNEL_VALUE(defaultChannelFailsafeConfig->step) ); cliDumpPrintf(dumpMask, equalsDefault, format, channel, - rxFailsafeModeCharacters[channelFailsafeConfiguration->mode], - RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step) + rxFailsafeModeCharacters[channelFailsafeConfig->mode], + RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step) ); } else { const char *format = "rxfail %u %c\r\n"; cliDefaultPrintf(dumpMask, equalsDefault, format, channel, - rxFailsafeModeCharacters[channelFailsafeConfigurationDefault->mode] + rxFailsafeModeCharacters[defaultChannelFailsafeConfig->mode] ); cliDumpPrintf(dumpMask, equalsDefault, format, channel, - rxFailsafeModeCharacters[channelFailsafeConfiguration->mode] + rxFailsafeModeCharacters[channelFailsafeConfig->mode] ); } } } -static void cliRxFail(char *cmdline) +static void cliRxFailsafe(char *cmdline) { uint8_t channel; char buf[3]; @@ -1196,25 +1554,24 @@ static void cliRxFail(char *cmdline) if (isEmpty(cmdline)) { // print out rxConfig failsafe settings for (channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { - cliRxFail(itoa(channel, buf, 10)); + cliRxFailsafe(itoa(channel, buf, 10)); } } else { - char *ptr = cmdline; + const char *ptr = cmdline; channel = atoi(ptr++); if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)) { - rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig()->failsafe_channel_configurations[channel]; + rxFailsafeChannelConfig_t *channelFailsafeConfig = &rxConfigMutable()->failsafe_channel_configurations[channel]; - uint16_t value; - rxFailsafeChannelType_e type = (channel < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_TYPE_FLIGHT : RX_FAILSAFE_TYPE_AUX; - rxFailsafeChannelMode_e mode = channelFailsafeConfiguration->mode; - bool requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; + const rxFailsafeChannelType_e type = (channel < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_TYPE_FLIGHT : RX_FAILSAFE_TYPE_AUX; + rxFailsafeChannelMode_e mode = channelFailsafeConfig->mode; + bool requireValue = channelFailsafeConfig->mode == RX_FAILSAFE_MODE_SET; ptr = nextArg(ptr); if (ptr) { - char *p = strchr(rxFailsafeModeCharacters, *(ptr)); + const char *p = strchr(rxFailsafeModeCharacters, *(ptr)); if (p) { - uint8_t requestedMode = p - rxFailsafeModeCharacters; + const uint8_t requestedMode = p - rxFailsafeModeCharacters; mode = rxFailsafeModesTable[type][requestedMode]; } else { mode = RX_FAILSAFE_MODE_INVALID; @@ -1232,23 +1589,22 @@ static void cliRxFail(char *cmdline) cliShowParseError(); return; } - value = atoi(ptr); + uint16_t value = atoi(ptr); value = CHANNEL_VALUE_TO_RXFAIL_STEP(value); if (value > MAX_RXFAIL_RANGE_STEP) { cliPrint("Value out of range\r\n"); return; } - channelFailsafeConfiguration->step = value; + channelFailsafeConfig->step = value; } else if (requireValue) { cliShowParseError(); return; } - channelFailsafeConfiguration->mode = mode; - + channelFailsafeConfig->mode = mode; } - char modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; + char modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfig->mode]; // double use of cliPrintf below // 1. acknowledge interpretation on command, @@ -1258,7 +1614,7 @@ static void cliRxFail(char *cmdline) cliPrintf("rxfail %u %c %d\r\n", channel, modeCharacter, - RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step) + RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step) ); } else { cliPrintf("rxfail %u %c\r\n", @@ -1272,15 +1628,15 @@ static void cliRxFail(char *cmdline) } } -static void printAux(uint8_t dumpMask, const modeActivationProfile_t *modeActivationProfile, const modeActivationProfile_t *defaultModeActivationProfile) +static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActivationConditions, const modeActivationCondition_t *defaultModeActivationConditions) { const char *format = "aux %u %u %u %u %u\r\n"; // print out aux channel settings for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - const modeActivationCondition_t *mac = &modeActivationProfile->modeActivationConditions[i]; + const modeActivationCondition_t *mac = &modeActivationConditions[i]; bool equalsDefault = false; - if (defaultModeActivationProfile) { - const modeActivationCondition_t *macDefault = &defaultModeActivationProfile->modeActivationConditions[i]; + if (defaultModeActivationConditions) { + const modeActivationCondition_t *macDefault = &defaultModeActivationConditions[i]; equalsDefault = mac->modeId == macDefault->modeId && mac->auxChannelIndex == macDefault->auxChannelIndex && mac->range.startStep == macDefault->range.startStep @@ -1306,15 +1662,15 @@ static void printAux(uint8_t dumpMask, const modeActivationProfile_t *modeActiva static void cliAux(char *cmdline) { int i, val = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { - printAux(DUMP_MASTER, modeActivationProfile(), NULL); + printAux(DUMP_MASTER, modeActivationConditions(0), NULL); } else { ptr = cmdline; i = atoi(ptr++); if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { - modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i]; + modeActivationCondition_t *mac = modeActivationConditionsMutable(i); uint8_t validArgumentCount = 0; ptr = nextArg(ptr); if (ptr) { @@ -1380,13 +1736,9 @@ static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, co static void cliSerial(char *cmdline) { - int i, val; - char *ptr; - if (isEmpty(cmdline)) { printSerial(DUMP_MASTER, serialConfig(), NULL); - - return; + return; } serialPortConfig_t portConfig; memset(&portConfig, 0 , sizeof(portConfig)); @@ -1395,9 +1747,9 @@ static void cliSerial(char *cmdline) uint8_t validArgumentCount = 0; - ptr = cmdline; + const char *ptr = cmdline; - val = atoi(ptr++); + int val = atoi(ptr++); currentConfig = serialFindPortConfiguration(val); if (currentConfig) { portConfig.identifier = val; @@ -1411,7 +1763,7 @@ static void cliSerial(char *cmdline) validArgumentCount++; } - for (i = 0; i < 4; i ++) { + for (int i = 0; i < 4; i ++) { ptr = nextArg(ptr); if (!ptr) { break; @@ -1460,7 +1812,6 @@ static void cliSerial(char *cmdline) } memcpy(currentConfig, &portConfig, sizeof(portConfig)); - } #ifndef SKIP_SERIAL_PASSTHROUGH @@ -1474,7 +1825,8 @@ static void cliSerialPassthrough(char *cmdline) int id = -1; uint32_t baud = 0; unsigned mode = 0; - char* tok = strtok(cmdline, " "); + char *saveptr; + char* tok = strtok_r(cmdline, " ", &saveptr); int index = 0; while (tok != NULL) { @@ -1493,7 +1845,7 @@ static void cliSerialPassthrough(char *cmdline) break; } index++; - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ", &saveptr); } serialPort_t *passThroughPort; @@ -1538,15 +1890,15 @@ static void cliSerialPassthrough(char *cmdline) } #endif -static void printAdjustmentRange(uint8_t dumpMask, const adjustmentProfile_t *adjustmentProfile, const adjustmentProfile_t *defaultAdjustmentProfile) +static void printAdjustmentRange(uint8_t dumpMask, const adjustmentRange_t *adjustmentRanges, const adjustmentRange_t *defaultAdjustmentRanges) { const char *format = "adjrange %u %u %u %u %u %u %u\r\n"; // print out adjustment ranges channel settings for (uint32_t i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - const adjustmentRange_t *ar = &adjustmentProfile->adjustmentRanges[i]; + const adjustmentRange_t *ar = &adjustmentRanges[i]; bool equalsDefault = false; - if (defaultAdjustmentProfile) { - const adjustmentRange_t *arDefault = &defaultAdjustmentProfile->adjustmentRanges[i]; + if (defaultAdjustmentRanges) { + const adjustmentRange_t *arDefault = &defaultAdjustmentRanges[i]; equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex && ar->range.startStep == arDefault->range.startStep && ar->range.endStep == arDefault->range.endStep @@ -1578,15 +1930,15 @@ static void printAdjustmentRange(uint8_t dumpMask, const adjustmentProfile_t *ad static void cliAdjustmentRange(char *cmdline) { int i, val = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { - printAdjustmentRange(DUMP_MASTER, adjustmentProfile(), NULL); + printAdjustmentRange(DUMP_MASTER, adjustmentRanges(0), NULL); } else { ptr = cmdline; i = atoi(ptr++); if (i < MAX_ADJUSTMENT_RANGE_COUNT) { - adjustmentRange_t *ar = &adjustmentProfile()->adjustmentRanges[i]; + adjustmentRange_t *ar = adjustmentRangesMutable(i); uint8_t validArgumentCount = 0; ptr = nextArg(ptr); @@ -1644,7 +1996,7 @@ static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer char buf2[8]; char buf3[8]; for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - if (customMotorMixer(i)->throttle == 0.0f) + if (customMotorMixer[i].throttle == 0.0f) break; const float thr = customMotorMixer[i].throttle; const float roll = customMotorMixer[i].roll; @@ -1682,14 +2034,15 @@ static void cliMotorMix(char *cmdline) #else int check = 0; uint8_t len; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer - for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) - customMotorMixer(i)->throttle = 0.0f; + for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + customMotorMixerMutable(i)->throttle = 0.0f; + } } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = nextArg(cmdline); if (ptr) { @@ -1700,7 +2053,7 @@ static void cliMotorMix(char *cmdline) break; } if (strncasecmp(ptr, mixerNames[i], len) == 0) { - mixerLoadMix(i, masterConfig.customMotorMixer); + mixerLoadMix(i, customMotorMixerMutable(0)); cliPrintf("Loaded %s\r\n", mixerNames[i]); cliMotorMix(""); break; @@ -1713,22 +2066,22 @@ static void cliMotorMix(char *cmdline) if (i < MAX_SUPPORTED_MOTORS) { ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->throttle = fastA2F(ptr); + customMotorMixerMutable(i)->throttle = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->roll = fastA2F(ptr); + customMotorMixerMutable(i)->roll = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->pitch = fastA2F(ptr); + customMotorMixerMutable(i)->pitch = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->yaw = fastA2F(ptr); + customMotorMixerMutable(i)->yaw = fastA2F(ptr); check++; } if (check != 4) { @@ -1743,26 +2096,24 @@ static void cliMotorMix(char *cmdline) #endif } -static void printRxRange(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) +static void printRxRange(uint8_t dumpMask, const rxChannelRangeConfig_t *channelRangeConfigs, const rxChannelRangeConfig_t *defaultChannelRangeConfigs) { const char *format = "rxrange %u %u %u\r\n"; for (uint32_t i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { - const rxChannelRangeConfiguration_t *channelRangeConfiguration = &rxConfig->channelRanges[i]; bool equalsDefault = false; - if (defaultRxConfig) { - const rxChannelRangeConfiguration_t *channelRangeConfigurationDefault = &defaultRxConfig->channelRanges[i]; - equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min - && channelRangeConfiguration->max == channelRangeConfigurationDefault->max; + if (defaultChannelRangeConfigs) { + equalsDefault = channelRangeConfigs[i].min == defaultChannelRangeConfigs[i].min + && channelRangeConfigs[i].max == defaultChannelRangeConfigs[i].max; cliDefaultPrintf(dumpMask, equalsDefault, format, i, - channelRangeConfigurationDefault->min, - channelRangeConfigurationDefault->max + defaultChannelRangeConfigs[i].min, + defaultChannelRangeConfigs[i].max ); } cliDumpPrintf(dumpMask, equalsDefault, format, i, - channelRangeConfiguration->min, - channelRangeConfiguration->max + channelRangeConfigs[i].min, + channelRangeConfigs[i].max ); } } @@ -1770,12 +2121,12 @@ static void printRxRange(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxC static void cliRxRange(char *cmdline) { int i, validArgumentCount = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { - printRxRange(DUMP_MASTER, rxConfig(), NULL); + printRxRange(DUMP_MASTER, rxConfig()->channelRanges, NULL); } else if (strcasecmp(cmdline, "reset") == 0) { - resetAllRxChannelRangeConfigurations(rxConfig()->channelRanges); + resetAllRxChannelRangeConfigurations(rxConfigMutable()->channelRanges); } else { ptr = cmdline; i = atoi(ptr); @@ -1799,9 +2150,9 @@ static void cliRxRange(char *cmdline) } else if (rangeMin < PWM_PULSE_MIN || rangeMin > PWM_PULSE_MAX || rangeMax < PWM_PULSE_MIN || rangeMax > PWM_PULSE_MAX) { cliShowParseError(); } else { - rxChannelRangeConfiguration_t *channelRangeConfiguration = &rxConfig()->channelRanges[i]; - channelRangeConfiguration->min = rangeMin; - channelRangeConfiguration->max = rangeMax; + rxChannelRangeConfig_t *channelRangeConfig = &rxConfigMutable()->channelRanges[i]; + channelRangeConfig->min = rangeMin; + channelRangeConfig->max = rangeMax; } } else { cliShowArgumentRangeError("channel", 0, NON_AUX_CHANNEL_COUNT - 1); @@ -1832,7 +2183,7 @@ static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledC static void cliLed(char *cmdline) { int i; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printLed(DUMP_MASTER, ledStripConfig()->ledConfigs, NULL); @@ -1869,14 +2220,11 @@ static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColo static void cliColor(char *cmdline) { - int i; - char *ptr; - if (isEmpty(cmdline)) { printColor(DUMP_MASTER, ledStripConfig()->colors, NULL); } else { - ptr = cmdline; - i = atoi(ptr); + const char *ptr = cmdline; + const int i = atoi(ptr); if (i < LED_CONFIGURABLE_COLOR_COUNT) { ptr = nextArg(cmdline); if (!parseColor(i, ptr)) { @@ -1933,10 +2281,11 @@ static void cliModeColor(char *cmdline) enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; int args[ARGS_COUNT]; int argNo = 0; - char* ptr = strtok(cmdline, " "); + char *saveptr; + const char* ptr = strtok_r(cmdline, " ", &saveptr); while (ptr && argNo < ARGS_COUNT) { args[argNo++] = atoi(ptr); - ptr = strtok(NULL, " "); + ptr = strtok_r(NULL, " ", &saveptr); } if (ptr != NULL || argNo != ARGS_COUNT) { @@ -1958,30 +2307,30 @@ static void cliModeColor(char *cmdline) #endif #ifdef USE_SERVOS -static void printServo(uint8_t dumpMask, servoProfile_t *defaultServoProfile) +static void printServo(uint8_t dumpMask, const servoParam_t *servoParams, const servoParam_t *defaultServoParams) { // print out servo settings const char *format = "servo %u %d %d %d %d %d %d %d\r\n"; for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - const servoParam_t *servoConf = &servoProfile()->servoConf[i]; + const servoParam_t *servoConf = &servoParams[i]; bool equalsDefault = false; - if (defaultServoProfile) { - const servoParam_t *servoConfDefault = &defaultServoProfile->servoConf[i]; - equalsDefault = servoConf->min == servoConfDefault->min - && servoConf->max == servoConfDefault->max - && servoConf->middle == servoConfDefault->middle - && servoConf->angleAtMin == servoConfDefault->angleAtMax - && servoConf->rate == servoConfDefault->rate - && servoConf->forwardFromChannel == servoConfDefault->forwardFromChannel; + if (defaultServoParams) { + const servoParam_t *defaultServoConf = &defaultServoParams[i]; + equalsDefault = servoConf->min == defaultServoConf->min + && servoConf->max == defaultServoConf->max + && servoConf->middle == defaultServoConf->middle + && servoConf->angleAtMin == defaultServoConf->angleAtMax + && servoConf->rate == defaultServoConf->rate + && servoConf->forwardFromChannel == defaultServoConf->forwardFromChannel; cliDefaultPrintf(dumpMask, equalsDefault, format, i, - servoConfDefault->min, - servoConfDefault->max, - servoConfDefault->middle, - servoConfDefault->angleAtMin, - servoConfDefault->angleAtMax, - servoConfDefault->rate, - servoConfDefault->forwardFromChannel + defaultServoConf->min, + defaultServoConf->max, + defaultServoConf->middle, + defaultServoConf->angleAtMin, + defaultServoConf->angleAtMax, + defaultServoConf->rate, + defaultServoConf->forwardFromChannel ); } cliDumpPrintf(dumpMask, equalsDefault, format, @@ -2008,7 +2357,7 @@ static void cliServo(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - printServo(DUMP_MASTER, NULL); + printServo(DUMP_MASTER, servoParams(0), NULL); } else { int validArgumentCount = 0; @@ -2047,7 +2396,7 @@ static void cliServo(char *cmdline) return; } - servo = &servoProfile()->servoConf[i]; + servo = servoParamsMutable(i); if ( arguments[MIN] < PWM_PULSE_MIN || arguments[MIN] > PWM_PULSE_MAX || @@ -2079,7 +2428,7 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) { const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { - servoMixer_t customServoMixer = *customServoMixer(i); + const servoMixer_t customServoMixer = *customServoMixers(i); if (customServoMixer.rate == 0) { break; } @@ -2123,7 +2472,7 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) // print servo directions for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { const char *format = "smix reverse %d %d r\r\n"; - const servoParam_t *servoConf = &servoProfile()->servoConf[i]; + const servoParam_t *servoConf = servoParams(i); if (defaultConfig) { const servoParam_t *servoConfDefault = &defaultConfig->servoProfile.servoConf[i]; bool equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; @@ -2148,10 +2497,8 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) static void cliServoMix(char *cmdline) { - uint8_t len; - char *ptr; int args[8], check = 0; - len = strlen(cmdline); + int len = strlen(cmdline); if (len == 0) { printServoMix(DUMP_MASTER, NULL); @@ -2159,10 +2506,10 @@ static void cliServoMix(char *cmdline) // erase custom mixer memset(masterConfig.customServoMixer, 0, sizeof(masterConfig.customServoMixer)); for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servoProfile()->servoConf[i].reversedSources = 0; + servoParamsMutable(i)->reversedSources = 0; } } else if (strncasecmp(cmdline, "load", 4) == 0) { - ptr = nextArg(cmdline); + const char *ptr = nextArg(cmdline); if (ptr) { len = strlen(ptr); for (uint32_t i = 0; ; i++) { @@ -2180,7 +2527,7 @@ static void cliServoMix(char *cmdline) } } else if (strncasecmp(cmdline, "reverse", 7) == 0) { enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT}; - ptr = strchr(cmdline, ' '); + char *ptr = strchr(cmdline, ' '); len = strlen(ptr); if (len == 0) { @@ -2192,16 +2539,17 @@ static void cliServoMix(char *cmdline) for (uint32_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { cliPrintf("%d", servoIndex); for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) - cliPrintf("\t%s ", (servoProfile()->servoConf[servoIndex].reversedSources & (1 << inputSource)) ? "r" : "n"); + cliPrintf("\t%s ", (servoParams(servoIndex)->reversedSources & (1 << inputSource)) ? "r" : "n"); cliPrintf("\r\n"); } return; } - ptr = strtok(ptr, " "); + char *saveptr; + ptr = strtok_r(ptr, " ", &saveptr); while (ptr != NULL && check < ARGS_COUNT - 1) { args[check++] = atoi(ptr); - ptr = strtok(NULL, " "); + ptr = strtok_r(NULL, " ", &saveptr); } if (ptr == NULL || check != ARGS_COUNT - 1) { @@ -2213,19 +2561,20 @@ static void cliServoMix(char *cmdline) && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT && (*ptr == 'r' || *ptr == 'n')) { if (*ptr == 'r') - servoProfile()->servoConf[args[SERVO]].reversedSources |= 1 << args[INPUT]; + servoParamsMutable(args[SERVO])->reversedSources |= 1 << args[INPUT]; else - servoProfile()->servoConf[args[SERVO]].reversedSources &= ~(1 << args[INPUT]); + servoParamsMutable(args[SERVO])->reversedSources &= ~(1 << args[INPUT]); } else cliShowParseError(); cliServoMix("reverse"); } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT}; - ptr = strtok(cmdline, " "); + char *saveptr; + char *ptr = strtok_r(cmdline, " ", &saveptr); while (ptr != NULL && check < ARGS_COUNT) { args[check++] = atoi(ptr); - ptr = strtok(NULL, " "); + ptr = strtok_r(NULL, " ", &saveptr); } if (ptr != NULL || check != ARGS_COUNT) { @@ -2242,13 +2591,13 @@ static void cliServoMix(char *cmdline) args[MIN] >= 0 && args[MIN] <= 100 && args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] && args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) { - customServoMixer(i)->targetChannel = args[TARGET]; - customServoMixer(i)->inputSource = args[INPUT]; - customServoMixer(i)->rate = args[RATE]; - customServoMixer(i)->speed = args[SPEED]; - customServoMixer(i)->min = args[MIN]; - customServoMixer(i)->max = args[MAX]; - customServoMixer(i)->box = args[BOX]; + customServoMixersMutable(i)->targetChannel = args[TARGET]; + customServoMixersMutable(i)->inputSource = args[INPUT]; + customServoMixersMutable(i)->rate = args[RATE]; + customServoMixersMutable(i)->speed = args[SPEED]; + customServoMixersMutable(i)->min = args[MIN]; + customServoMixersMutable(i)->max = args[MAX]; + customServoMixersMutable(i)->box = args[BOX]; cliServoMix(""); } else { cliShowParseError(); @@ -2268,7 +2617,8 @@ static void cliWriteBytes(const uint8_t *buffer, int count) } } -static void cliSdInfo(char *cmdline) { +static void cliSdInfo(char *cmdline) +{ UNUSED(cmdline); cliPrint("SD card: "); @@ -2346,7 +2696,7 @@ static void cliFlashErase(char *cmdline) UNUSED(cmdline); #ifndef MINIMAL_CLI - uint32_t i; + uint32_t i = 0; cliPrintf("Erasing, please wait ... \r\n"); #else cliPrintf("Erasing,\r\n"); @@ -2367,7 +2717,7 @@ static void cliFlashErase(char *cmdline) #endif delay(100); } - + beeper(BEEPER_BLACKBOX_ERASE); cliPrintf("\r\nDone.\r\n"); } @@ -2375,8 +2725,8 @@ static void cliFlashErase(char *cmdline) static void cliFlashWrite(char *cmdline) { - uint32_t address = atoi(cmdline); - char *text = strchr(cmdline, ' '); + const uint32_t address = atoi(cmdline); + const char *text = strchr(cmdline, ' '); if (!text) { cliShowParseError(); @@ -2392,19 +2742,17 @@ static void cliFlashWrite(char *cmdline) static void cliFlashRead(char *cmdline) { uint32_t address = atoi(cmdline); - uint32_t length; - uint8_t buffer[32]; - - char *nextArg = strchr(cmdline, ' '); + const char *nextArg = strchr(cmdline, ' '); if (!nextArg) { cliShowParseError(); } else { - length = atoi(nextArg); + uint32_t length = atoi(nextArg); cliPrintf("Reading %u bytes at %u:\r\n", length, address); + uint8_t buffer[32]; while (length > 0) { int bytesRead = flashfsReadAbs(address, buffer, length < sizeof(buffer) ? length : sizeof(buffer)); @@ -2465,7 +2813,7 @@ static void printVtx(uint8_t dumpMask, const master_t *defaultConfig) static void cliVtx(char *cmdline) { int i, val = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printVtx(DUMP_MASTER, NULL); @@ -2513,26 +2861,26 @@ static void cliVtx(char *cmdline) static void printName(uint8_t dumpMask) { - bool equalsDefault = strlen(masterConfig.name) == 0; - cliDumpPrintf(dumpMask, equalsDefault, "name %s\r\n", equalsDefault ? emptyName : masterConfig.name); + const bool equalsDefault = strlen(systemConfig()->name) == 0; + cliDumpPrintf(dumpMask, equalsDefault, "name %s\r\n", equalsDefault ? emptyName : systemConfig()->name); } static void cliName(char *cmdline) { - uint32_t len = strlen(cmdline); + const uint32_t len = strlen(cmdline); if (len > 0) { - memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); + memset(systemConfigMutable()->name, 0, ARRAYLEN(systemConfig()->name)); if (strncmp(cmdline, emptyName, len)) { - strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); + strncpy(systemConfigMutable()->name, cmdline, MIN(len, MAX_NAME_LENGTH)); } } printName(DUMP_MASTER); } -static void printFeature(uint8_t dumpMask, const master_t *defaultConfig) +static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfigDefault) { - uint32_t mask = featureMask(); - uint32_t defaultMask = defaultConfig->enabledFeatures; + const uint32_t mask = featureMask(); + const uint32_t defaultMask = featureConfigDefault->enabledFeatures; for (uint32_t i = 0; ; i++) { // disable all feature first if (featureNames[i] == NULL) break; @@ -2623,9 +2971,9 @@ static void cliFeature(char *cmdline) #ifdef BEEPER static void printBeeper(uint8_t dumpMask, const master_t *defaultConfig) { - uint8_t beeperCount = beeperTableEntryCount(); - uint32_t mask = getBeeperOffMask(); - uint32_t defaultMask = defaultConfig->beeper_off_flags; + const uint8_t beeperCount = beeperTableEntryCount(); + const uint32_t mask = getBeeperOffMask(); + const uint32_t defaultMask = defaultConfig->beeper_off_flags; for (int32_t i = 0; i < beeperCount - 2; i++) { const char *formatOff = "beeper -%s\r\n"; const char *formatOn = "beeper %s\r\n"; @@ -2904,7 +3252,7 @@ static void cliMixer(char *cmdline) return; } if (strncasecmp(cmdline, mixerNames[i], len) == 0) { - mixerConfig()->mixerMode = i + 1; + mixerConfigMutable()->mixerMode = i + 1; break; } } @@ -2994,13 +3342,11 @@ static void cliPlaySound(char *cmdline) static void cliProfile(char *cmdline) { - int i; - if (isEmpty(cmdline)) { cliPrintf("profile %d\r\n", getCurrentProfile()); return; } else { - i = atoi(cmdline); + const int i = atoi(cmdline); if (i >= 0 && i < MAX_PROFILE_COUNT) { masterConfig.current_profile_index = i; writeEEPROM(); @@ -3012,13 +3358,11 @@ static void cliProfile(char *cmdline) static void cliRateProfile(char *cmdline) { - int i; - if (isEmpty(cmdline)) { cliPrintf("rateprofile %d\r\n", getCurrentControlRateProfile()); return; } else { - i = atoi(cmdline); + const int i = atoi(cmdline); if (i >= 0 && i < MAX_RATEPROFILES) { changeControlRateProfile(i); cliRateProfile(""); @@ -3036,7 +3380,13 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_ cliPrintHashLine("profile"); cliProfile(""); cliPrint("\r\n"); +#ifdef USE_PARAMETER_GROUPS + (void)(defaultConfig); + dumpAllValues(PROFILE_VALUE, dumpMask); + dumpAllValues(PROFILE_RATE_VALUE, dumpMask); +#else dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); +#endif cliRateProfile(""); } @@ -3076,7 +3426,7 @@ static void cliGet(char *cmdline) const clivalue_t *val; int matchedCommands = 0; - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { if (strstr(valueTable[i].name, cmdline)) { val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); @@ -3107,7 +3457,7 @@ static void cliSet(char *cmdline) if (len == 0 || (len == 1 && cmdline[0] == '*')) { cliPrint("Current settings: \r\n"); - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui @@ -3128,7 +3478,7 @@ static void cliSet(char *cmdline) eqptr++; } - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { val = &valueTable[i]; // ensure exact match when setting to prevent setting variables with shorter names if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { @@ -3318,7 +3668,7 @@ static void cliVersion(char *cmdline) #if defined(USE_RESOURCE_MGMT) -#define MAX_RESOURCE_INDEX(x) (x == 0 ? 1 : x) +#define MAX_RESOURCE_INDEX(x) ((x) == 0 ? 1 : (x)) typedef struct { const uint8_t owner; @@ -3327,12 +3677,15 @@ typedef struct { } cliResourceValue_t; const cliResourceValue_t resourceTable[] = { +#ifdef USE_PARAMETER_GROUPS + { OWNER_MOTOR, NULL, MAX_SUPPORTED_MOTORS }, +#else #ifdef BEEPER - { OWNER_BEEPER, &beeperConfig()->ioTag, 0 }, + { OWNER_BEEPER, &beeperDevConfig()->ioTag, 0 }, #endif - { OWNER_MOTOR, &motorConfig()->ioTags[0], MAX_SUPPORTED_MOTORS }, + { OWNER_MOTOR, &motorConfig()->dev.ioTags[0], MAX_SUPPORTED_MOTORS }, #ifdef USE_SERVOS - { OWNER_SERVO, &servoConfig()->ioTags[0], MAX_SUPPORTED_SERVOS }, + { OWNER_SERVO, &servoConfig()->dev.ioTags[0], MAX_SUPPORTED_SERVOS }, #endif #if defined(USE_PWM) || defined(USE_PPM) { OWNER_PPMINPUT, &ppmConfig()->ioTag, 0 }, @@ -3347,6 +3700,7 @@ const cliResourceValue_t resourceTable[] = { #endif { OWNER_SERIAL_TX, &serialPinConfig()->ioTagTx[0], SERIAL_PORT_MAX_INDEX }, { OWNER_SERIAL_RX, &serialPinConfig()->ioTagRx[0], SERIAL_PORT_MAX_INDEX }, +#endif }; static void printResource(uint8_t dumpMask, const master_t *defaultConfig) @@ -3389,6 +3743,10 @@ static void printResourceOwner(uint8_t owner, uint8_t index) static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) { + if (!tag) { + return; + } + const char * format = "\r\nNOTE: %c%02d already assigned to "; for (int r = 0; r < (int)ARRAYLEN(resourceTable); r++) { for (int i = 0; i < MAX_RESOURCE_INDEX(resourceTable[r].maxIndex); i++) { @@ -3545,6 +3903,51 @@ static void cliResource(char *cmdline) } #endif /* USE_RESOURCE_MGMT */ +#ifdef USE_PARAMETER_GROUPS +static void backupConfigs(void) +{ + // make copies of configs to do differencing + PG_FOREACH(reg) { + // currentConfig is the copy + const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg)); + if (cliCurrentAndDefaultConfig->currentConfig) { + if (pgIsProfile(reg)) { + //memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size * MAX_PROFILE_COUNT); + } else { + memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size); + } +#ifdef SERIAL_CLI_DEBUG + } else { + cliPrintf("BACKUP %d SET UP INCORRECTLY\r\n", pgN(reg)); +#endif + } + } + const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); + memcpy(&pidProfileCopy[0], reg->address, sizeof(pidProfile_t) * MAX_PROFILE_COUNT); +} + +static void restoreConfigs(void) +{ + PG_FOREACH(reg) { + // currentConfig is the copy + const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg)); + if (cliCurrentAndDefaultConfig->currentConfig) { + if (pgIsProfile(reg)) { + //memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size * MAX_PROFILE_COUNT); + } else { + memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size); + } +#ifdef SERIAL_CLI_DEBUG + } else { + cliPrintf("RESTORE %d SET UP INCORRECTLY\r\n", pgN(reg)); +#endif + } + } + const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); + memcpy(reg->address, &pidProfileCopy[0], sizeof(pidProfile_t) * MAX_PROFILE_COUNT); +} +#endif + static void printConfig(char *cmdline, bool doDiff) { uint8_t dumpMask = DUMP_MASTER; @@ -3561,13 +3964,21 @@ static void printConfig(char *cmdline, bool doDiff) options = cmdline; } - static master_t defaultConfig; if (doDiff) { dumpMask = dumpMask | DO_DIFF; } + static master_t defaultConfig; createDefaultConfig(&defaultConfig); +#ifdef USE_PARAMETER_GROUPS + backupConfigs(); + // reset all configs to defaults to do differencing + resetConfigs(); +#if defined(TARGET_CONFIG) + targetConfiguration(&defaultConfig); +#endif +#endif if (checkCommand(options, "showdefaults")) { dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values } @@ -3602,7 +4013,7 @@ static void printConfig(char *cmdline, bool doDiff) #ifdef USE_SERVOS cliPrintHashLine("servo"); - printServo(dumpMask, &defaultConfig.servoProfile); + printServo(dumpMask, servoParams(0), defaultConfig.servoProfile.servoConf); cliPrintHashLine("servo mix"); // print custom servo mixer if exists @@ -3612,7 +4023,7 @@ static void printConfig(char *cmdline, bool doDiff) #endif cliPrintHashLine("feature"); - printFeature(dumpMask, &defaultConfig); + printFeature(dumpMask, &defaultConfig.featureConfig); #ifdef BEEPER cliPrintHashLine("beeper"); @@ -3637,13 +4048,13 @@ static void printConfig(char *cmdline, bool doDiff) #endif cliPrintHashLine("aux"); - printAux(dumpMask, modeActivationProfile(), &defaultConfig.modeActivationProfile); + printAux(dumpMask, modeActivationConditions(0), defaultConfig.modeActivationProfile.modeActivationConditions); cliPrintHashLine("adjrange"); - printAdjustmentRange(dumpMask, adjustmentProfile(), &defaultConfig.adjustmentProfile); + printAdjustmentRange(dumpMask, adjustmentRanges(0), defaultConfig.adjustmentProfile.adjustmentRanges); cliPrintHashLine("rxrange"); - printRxRange(dumpMask, rxConfig(), &defaultConfig.rxConfig); + printRxRange(dumpMask, rxConfig()->channelRanges, defaultConfig.rxConfig.channelRanges); #ifdef VTX cliPrintHashLine("vtx"); @@ -3651,7 +4062,7 @@ static void printConfig(char *cmdline, bool doDiff) #endif cliPrintHashLine("rxfail"); - printRxFail(dumpMask, rxConfig(), &defaultConfig.rxConfig); + printRxFailsafe(dumpMask, rxConfig()->failsafe_channel_configurations, defaultConfig.rxConfig.failsafe_channel_configurations); cliPrintHashLine("master"); dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); @@ -3690,6 +4101,10 @@ static void printConfig(char *cmdline, bool doDiff) if (dumpMask & DUMP_RATES) { cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); } +#ifdef USE_PARAMETER_GROUPS + // restore configs from copies + restoreConfigs(); +#endif } static void cliDump(char *cmdline) @@ -3787,7 +4202,7 @@ const clicmd_t cmdTable[] = { #if defined(USE_RESOURCE_MGMT) CLI_COMMAND_DEF("resource", "show/set resources", NULL, cliResource), #endif - CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail), + CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFailsafe), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), #ifdef USE_SDCARD @@ -3816,13 +4231,11 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), #endif }; -#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t)) - static void cliHelp(char *cmdline) { UNUSED(cmdline); - for (uint32_t i = 0; i < CMD_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) { cliPrint(cmdTable[i].name); #ifndef MINIMAL_CLI if (cmdTable[i].description) { @@ -3851,7 +4264,7 @@ void cliProcess(void) // do tab completion const clicmd_t *cmd, *pstart = NULL, *pend = NULL; uint32_t i = bufferIndex; - for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { + for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0)) continue; if (!pstart) @@ -3912,12 +4325,12 @@ void cliProcess(void) const clicmd_t *cmd; char *options; - for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { + for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { if ((options = checkCommand(cliBuffer, cmd->name))) { break; - } + } } - if(cmd < cmdTable + CMD_COUNT) + if(cmd < cmdTable + ARRAYLEN(cmdTable)) cmd->func(options); else cliPrint("Unknown command, try 'help'"); @@ -3965,8 +4378,9 @@ void cliEnter(serialPort_t *serialPort) ENABLE_ARMING_FLAG(PREVENT_ARMING); } -void cliInit(serialConfig_t *serialConfig) +void cliInit(const serialConfig_t *serialConfig) { UNUSED(serialConfig); + BUILD_BUG_ON(LOOKUP_TABLE_COUNT != ARRAYLEN(lookupTables)); } #endif // USE_CLI diff --git a/src/main/fc/cli.h b/src/main/fc/cli.h index dfa2d169d7..a0ad8bee53 100644 --- a/src/main/fc/cli.h +++ b/src/main/fc/cli.h @@ -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); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 425fd33fcc..cb77c798bc 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -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, - ¤tProfile->pidProfile - ); - -#ifdef TELEMETRY - telemetryUseConfig(&masterConfig.telemetryConfig); -#endif + useRcControlsConfig(modeActivationConditions(0), ¤tProfile->pidProfile); + useAdjustmentConfig(¤tProfile->pidProfile); #ifdef GPS - gpsUseProfile(&masterConfig.gpsProfile); gpsUsePIDs(¤tProfile->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, - ¤tProfile->pidProfile, - throttleCorrectionConfig()->throttle_correction_angle - ); + imuConfigure(throttleCorrectionConfig()->throttle_correction_angle); - configureAltitudeHold( - ¤tProfile->pidProfile, - &masterConfig.barometerConfig, - &masterConfig.rcControlsConfig, - &masterConfig.motorConfig - ); - -#ifdef BARO - useBarometerConfig(&masterConfig.barometerConfig); -#endif + configureAltitudeHold(¤tProfile->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(); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 0fe4f9e616..0e49440cbd 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -17,8 +17,18 @@ #pragma once +#include #include +#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); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index c0e9abeebe..3a561ad83b 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -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 diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 5e2766d604..1b83302eb4 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -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(); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 797beabf3e..f57f050f52 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -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(), - ¤tProfile->pidProfile - ); + gpsInit(); + navigationInit(¤tProfile->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)) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6e7adf2027..3e6640ea46 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -17,7 +17,6 @@ #include #include -#include #include #include @@ -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, ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationConditions(0), ¤tProfile->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; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index ec12ed8090..ecbb07659d 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -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 diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c new file mode 100644 index 0000000000..31c365a28a --- /dev/null +++ b/src/main/fc/rc_adjustments.c @@ -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 . + */ + +#include +#include +#include + +#include + +#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; +} diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h new file mode 100644 index 0000000000..6067109f8a --- /dev/null +++ b/src/main/fc/rc_adjustments.h @@ -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 . + */ + +#pragma once + +#include +#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); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0f8a79b01e..8cba180f3b 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -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)); -} - diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index e9c88f874b..19d649588f 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -19,6 +19,8 @@ #include +#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); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 954ae59b11..05edd90bb2 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -21,25 +21,27 @@ #include #include - #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 diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 3201f71b9a..f1275a50e0 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -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); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index c0603b4f01..ba604c309c 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -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); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index d166e1c360..d11d61421a 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -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); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index a25d992285..97da5647b3 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -21,37 +21,40 @@ #include #include -#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]; } diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 9525c251ea..e36d7d47e6 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 838c98cf16..5c2a44a1ad 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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; } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 8a84ea8acb..8047d945fc 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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); diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index dffd456893..4b6650faa2 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -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 diff --git a/src/main/flight/navigation.h b/src/main/flight/navigation.h index 33bb184f23..af49cfd263 100644 --- a/src/main/flight/navigation.h +++ b/src/main/flight/navigation.h @@ -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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 25faca7079..04937c42be 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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" diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9c045245c6..c6fd9035ae 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 13ab2fc658..73712a8799 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -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; } diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index e53b840fb2..25db91ba70 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -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); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 13eefa691a..6f0e0ce6ee 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -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; diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 15a40eef5d..645ff45d71 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -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 diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 5aed763456..0a38cfe88f 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -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); diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h index 571de11973..357f0778be 100644 --- a/src/main/io/dashboard.h +++ b/src/main/io/dashboard.h @@ -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); diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index 79e1815dae..e8540cc52a 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -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); diff --git a/src/main/io/displayport_max7456.h b/src/main/io/displayport_max7456.h index 1b523ee6a6..f02611e135 100644 --- a/src/main/io/displayport_max7456.h +++ b/src/main/io/displayport_max7456.h @@ -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); diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index 426d91c258..ecf063f9e8 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -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; diff --git a/src/main/io/displayport_msp.h b/src/main/io/displayport_msp.h index 8ac290a65f..8730f19f79 100644 --- a/src/main/io/displayport_msp.h +++ b/src/main/io/displayport_msp.h @@ -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); diff --git a/src/main/io/gimbal.h b/src/main/io/gimbal.h index 914ce05ba5..969242fee3 100644 --- a/src/main/io/gimbal.h +++ b/src/main/io/gimbal.h @@ -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); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index c4925d8f4e..6c0f0fd10f 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -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 diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 484f7bf52a..81ee218e40 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -19,6 +19,8 @@ #include "common/time.h" +#include "config/parameter_group.h" + #define LAT 0 #define LON 1 @@ -68,6 +70,8 @@ typedef struct gpsConfig_s { gpsAutoBaud_e autoBaud; } gpsConfig_t; +PG_DECLARE(gpsConfig_t, gpsConfig); + typedef struct gpsCoordinateDDDMMmmmm_s { int16_t dddmm; int16_t mmmm; @@ -117,8 +121,7 @@ extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Str #define GPS_DBHZ_MIN 0 #define GPS_DBHZ_MAX 55 -struct serialConfig_s; -void gpsInit(struct serialConfig_s *serialConfig, gpsConfig_t *initialGpsConfig); +void gpsInit(void); void gpsUpdate(timeUs_t currentTimeUs); bool gpsNewFrame(uint8_t c); struct serialPort_s; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index fa5be713f2..50cd421856 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -31,6 +31,9 @@ #include "common/maths.h" #include "common/typeconversion.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/light_ws2811strip.h" #include "drivers/system.h" #include "drivers/serial.h" @@ -44,6 +47,11 @@ #include "common/axis.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 "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -73,10 +81,6 @@ #include "telemetry/telemetry.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - /* PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); @@ -1080,14 +1084,11 @@ void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); } -void ledStripInit(ledStripConfig_t *ledStripConfig) +void ledStripInit() { - currentLedStripConfig = ledStripConfig; - - ledConfigs = currentLedStripConfig->ledConfigs; - colors = currentLedStripConfig->colors; - modeColors = currentLedStripConfig->modeColors; - specialColors = currentLedStripConfig->specialColors; + colors = ledStripConfigMutable()->colors; + modeColors = ledStripConfig()->modeColors; + specialColors = ledStripConfig()->specialColors; ledStripInitialised = false; } diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 4362a3d555..0cac24fc20 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -19,6 +19,7 @@ #include "common/color.h" #include "common/time.h" +#include "config/parameter_group.h" #include "drivers/io_types.h" #define LED_MAX_STRIP_LENGTH 32 @@ -147,9 +148,11 @@ typedef struct ledStripConfig_s { ioTag_t ioTag; } ledStripConfig_t; +PG_DECLARE(ledStripConfig_t, ledStripConfig); + ledConfig_t *ledConfigs; hsvColor_t *colors; -modeColorIndexes_t *modeColors; +const modeColorIndexes_t *modeColors; specialColorIndexes_t specialColors; #define LF(name) LED_FUNCTION_ ## name @@ -180,7 +183,7 @@ bool parseLedStripConfig(int ledIndex, const char *config); void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t bufferSize); void reevaluateLedConfig(void); -void ledStripInit(ledStripConfig_t *ledStripConfig); +void ledStripInit(void); void ledStripEnable(void); void ledStripUpdate(timeUs_t currentTimeUs); diff --git a/src/main/io/motors.h b/src/main/io/motors.h index 8486d507d2..e563a48c45 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -16,17 +16,3 @@ */ #pragma once - -#include "drivers/io_types.h" -#include "flight/mixer.h" - -typedef struct motorConfig_s { - 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 - uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) - uint8_t motorPwmProtocol; // Pwm Protocol - uint8_t useUnsyncedPwm; - float digitalIdleOffsetPercent; - ioTag_t ioTags[MAX_SUPPORTED_MOTORS]; -} motorConfig_t; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index bf1fd6e382..5f1a054616 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -24,6 +24,8 @@ #include #include +#include +#include #include #include @@ -31,12 +33,23 @@ #ifdef OSD +#include "blackbox/blackbox.h" +#include "blackbox/blackbox_io.h" + #include "build/debug.h" #include "build/version.h" #include "common/printf.h" +#include "common/maths.h" +#include "common/typeconversion.h" #include "common/utils.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/max7456_symbols.h" #include "drivers/display.h" #include "drivers/system.h" @@ -48,19 +61,23 @@ #include "cms/cms_types.h" #include "cms/cms_menu_osd.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" +#include "io/gps.h" #include "io/osd.h" - #include "io/vtx.h" - #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" +#include "flight/imu.h" + +#include "rx/rx.h" + +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/sensors.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -123,7 +140,7 @@ static displayPort_t *osdDisplayPort; */ static char osdGetAltitudeSymbol() { - switch (osdProfile()->units) { + switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: return 0xF; default: @@ -137,7 +154,7 @@ static char osdGetAltitudeSymbol() */ static int32_t osdGetAltitude(int32_t alt) { - switch (osdProfile()->units) { + switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: return (alt * 328) / 100; // Convert to feet / 100 default: @@ -147,11 +164,11 @@ static int32_t osdGetAltitude(int32_t alt) static void osdDrawSingleElement(uint8_t item) { - if (!VISIBLE(osdProfile()->item_pos[item]) || BLINK(item)) + if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) return; - uint8_t elemPosX = OSD_X(osdProfile()->item_pos[item]); - uint8_t elemPosY = OSD_Y(osdProfile()->item_pos[item]); + uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]); + uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]); char buff[32]; switch(item) { @@ -244,12 +261,12 @@ static void osdDrawSingleElement(uint8_t item) case OSD_CRAFT_NAME: { - if (strlen(masterConfig.name) == 0) + if (strlen(systemConfig()->name) == 0) strcpy(buff, "CRAFT_NAME"); else { for (uint8_t i = 0; i < MAX_NAME_LENGTH; i++) { - buff[i] = toupper((unsigned char)masterConfig.name[i]); - if (masterConfig.name[i] == 0) + buff[i] = toupper((unsigned char)systemConfig()->name[i]); + if (systemConfig()->name[i] == 0) break; } } @@ -376,8 +393,8 @@ static void osdDrawSingleElement(uint8_t item) case OSD_PIDRATE_PROFILE: { - uint8_t profileIndex = masterConfig.current_profile_index; - uint8_t rateProfileIndex = masterConfig.profile[profileIndex].activeRateProfile; + const uint8_t profileIndex = masterConfig.current_profile_index; + const uint8_t rateProfileIndex = masterConfig.profile[profileIndex].activeRateProfile; sprintf(buff, "%d-%d", profileIndex + 1, rateProfileIndex + 1); break; } @@ -524,15 +541,13 @@ void osdInit(displayPort_t *osdDisplayPortToUse) void osdUpdateAlarms(void) { - osd_profile_t *pOsdProfile = &masterConfig.osdProfile; - // This is overdone? - // uint16_t *itemPos = osdProfile()->item_pos; + // uint16_t *itemPos = osdConfig()->item_pos; int32_t alt = osdGetAltitude(baro.BaroAlt) / 100; statRssi = rssi * 100 / 1024; - if (statRssi < pOsdProfile->rssi_alarm) + if (statRssi < osdConfig()->rssi_alarm) SET_BLINK(OSD_RSSI_VALUE); else CLR_BLINK(OSD_RSSI_VALUE); @@ -550,17 +565,17 @@ void osdUpdateAlarms(void) else CLR_BLINK(OSD_GPS_SATS); - if (flyTime / 60 >= pOsdProfile->time_alarm && ARMING_FLAG(ARMED)) + if (flyTime / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) SET_BLINK(OSD_FLYTIME); else CLR_BLINK(OSD_FLYTIME); - if (mAhDrawn >= pOsdProfile->cap_alarm) + if (mAhDrawn >= osdConfig()->cap_alarm) SET_BLINK(OSD_MAH_DRAWN); else CLR_BLINK(OSD_MAH_DRAWN); - if (alt >= pOsdProfile->alt_alarm) + if (alt >= osdConfig()->alt_alarm) SET_BLINK(OSD_ALTITUDE); else CLR_BLINK(OSD_ALTITUDE); @@ -610,6 +625,47 @@ static void osdUpdateStats(void) stats.max_altitude = baro.BaroAlt; } +static void osdGetBlackboxStatusString(char * buff, uint8_t len) +{ + bool storageDeviceIsWorking = false; + uint32_t storageUsed = 0; + uint32_t storageTotal = 0; + + switch (blackboxConfig()->device) + { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + storageDeviceIsWorking = sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY); + if (storageDeviceIsWorking) { + storageTotal = sdcard_getMetadata()->numBlocks / 2000; + storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000); + } + break; +#endif + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + storageDeviceIsWorking = flashfsIsReady(); + if (storageDeviceIsWorking) { + const flashGeometry_t *geometry = flashfsGetGeometry(); + storageTotal = geometry->totalSize / 1024; + storageUsed = flashfsGetOffset() / 1024; + } + break; +#endif + + default: + storageDeviceIsWorking = true; + } + + if (storageDeviceIsWorking) { + uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal; + snprintf(buff, len, "%d%%", storageUsedPercent); + } else { + snprintf(buff, len, "FAULT"); + } +} + static void osdShowStats(void) { uint8_t top = 2; @@ -650,6 +706,12 @@ static void osdShowStats(void) sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); displayWrite(osdDisplayPort, 22, top++, buff); + if (feature(FEATURE_BLACKBOX) && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) { + displayWrite(osdDisplayPort, 2, top, "BLACKBOX :"); + osdGetBlackboxStatusString(buff, 10); + displayWrite(osdDisplayPort, 22, top++, buff); + } + refreshTimeout = 60 * REFRESH_1S; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index c6539b42d7..00b6abc163 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -18,6 +18,7 @@ #pragma once #include "common/time.h" +#include "config/parameter_group.h" #define VISIBLE_FLAG 0x0800 #define VISIBLE(x) (x & VISIBLE_FLAG) @@ -67,6 +68,9 @@ typedef struct osd_profile_s { osd_unit_e units; } osd_profile_t; +// !!TODO change to osdConfig_t +PG_DECLARE(osd_profile_t, osdConfig); + struct displayPort_s; void osdInit(struct displayPort_s *osdDisplayPort); void osdResetConfig(osd_profile_t *osdProfile); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 1d3982154f..3890794c64 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -25,6 +25,9 @@ #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/serial.h" #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2) @@ -42,7 +45,8 @@ #endif #include "io/serial.h" -#include "fc/cli.h" // for cliEnter() + +#include "fc/cli.h" #include "msp/msp_serial.h" @@ -50,7 +54,6 @@ #include "telemetry/telemetry.h" #endif -static serialConfig_t *serialConfig; static serialPortUsage_t serialPortUsageList[SERIAL_PORT_COUNT]; const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = { @@ -157,7 +160,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function) { while (findSerialPortConfigState.lastIndex < SERIAL_PORT_COUNT) { - serialPortConfig_t *candidate = &serialConfig->portConfigs[findSerialPortConfigState.lastIndex++]; + serialPortConfig_t *candidate = &serialConfigMutable()->portConfigs[findSerialPortConfigState.lastIndex++]; if (candidate->functionMask & function) { return candidate; @@ -170,7 +173,7 @@ typedef struct findSharedSerialPortState_s { uint8_t lastIndex; } findSharedSerialPortState_t; -portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function) +portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function) { if (!portConfig || (portConfig->functionMask & function) == 0) { return PORTSHARING_UNUSED; @@ -178,7 +181,7 @@ portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFun return portConfig->functionMask == function ? PORTSHARING_NOT_SHARED : PORTSHARING_SHARED; } -bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction) +bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction) { return (portConfig) && (portConfig->functionMask & sharedWithFunction) && (portConfig->functionMask & functionMask); } @@ -195,10 +198,10 @@ serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e s serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction) { while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) { - serialPortConfig_t *candidate = &serialConfig->portConfigs[findSharedSerialPortState.lastIndex++]; + const serialPortConfig_t *candidate = &serialConfig()->portConfigs[findSharedSerialPortState.lastIndex++]; if (isSerialPortShared(candidate, functionMask, sharedWithFunction)) { - serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier); + const serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier); if (!serialPortUsage) { continue; } @@ -215,7 +218,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction #define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX) #endif -bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) +bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) { UNUSED(serialConfigToCheck); /* @@ -229,9 +232,8 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) */ uint8_t mspPortCount = 0; - uint8_t index; - for (index = 0; index < SERIAL_PORT_COUNT; index++) { - serialPortConfig_t *portConfig = &serialConfigToCheck->portConfigs[index]; + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { + const serialPortConfig_t *portConfig = &serialConfigToCheck->portConfigs[index]; if (portConfig->functionMask & FUNCTION_MSP) { mspPortCount++; @@ -265,9 +267,8 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier) { - uint8_t index; - for (index = 0; index < SERIAL_PORT_COUNT; index++) { - serialPortConfig_t *candidate = &serialConfig->portConfigs[index]; + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { + serialPortConfig_t *candidate = &serialConfigMutable()->portConfigs[index]; if (candidate->identifier == identifier) { return candidate; } @@ -392,16 +393,12 @@ void closeSerialPort(serialPort_t *serialPort) serialPortUsage->serialPort = NULL; } -void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) +void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) { - uint8_t index; - - serialConfig = initialSerialConfig; - serialPortCount = SERIAL_PORT_COUNT; memset(&serialPortUsageList, 0, sizeof(serialPortUsageList)); - for (index = 0; index < SERIAL_PORT_COUNT; index++) { + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { serialPortUsageList[index].identifier = serialPortIdentifiers[index]; if (serialPortToDisable != SERIAL_PORT_NONE) { @@ -467,7 +464,7 @@ void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar) cliEnter(serialPort); } #endif - if (receivedChar == serialConfig->reboot_character) { + if (receivedChar == serialConfig()->reboot_character) { systemResetToBootloader(); } } @@ -484,8 +481,7 @@ static void nopConsumer(uint8_t data) arbitrary serial passthrough "proxy". Optional callbacks can be given to allow for specialized data processing. */ -void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer - *leftC, serialConsumer *rightC) +void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC) { waitForSerialPortToFinishTransmitting(left); waitForSerialPortToFinishTransmitting(right); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 3522e2d37c..8d9e19f066 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -17,6 +17,10 @@ #pragma once +#include +#include + +#include "config/parameter_group.h" #include "drivers/serial.h" typedef enum { @@ -113,23 +117,25 @@ typedef struct serialConfig_s { uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else. } serialConfig_t; +PG_DECLARE(serialConfig_t, serialConfig); + typedef void serialConsumer(uint8_t); // // configuration // -void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); +void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); void serialRemovePort(serialPortIdentifier_e identifier); uint8_t serialGetAvailablePortCount(void); bool serialIsPortAvailable(serialPortIdentifier_e identifier); -bool isSerialConfigValid(serialConfig_t *serialConfig); +bool isSerialConfigValid(const serialConfig_t *serialConfig); serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier); bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier); serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function); serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function); -portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function); -bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction); +portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function); +bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction); serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier); int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier); diff --git a/src/main/io/servos.h b/src/main/io/servos.h index 711ceec755..dd9f1aa1ca 100644 --- a/src/main/io/servos.h +++ b/src/main/io/servos.h @@ -17,12 +17,3 @@ #pragma once -#include "drivers/io_types.h" -#include "flight/servos.h" - -typedef struct servoConfig_s { - // PWM values, in milliseconds, common range is 1000-2000 (1 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]; -} servoConfig_t; diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 8db4c4279b..cf43e539a0 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -26,10 +26,16 @@ #include "io/osd.h" //External dependencies -#include "config/config_master.h" +#include "common/maths.h" + #include "config/config_eeprom.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/vtx_rtc6705.h" + #include "fc/runtime_config.h" + #include "io/beeper.h" diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 9d675c721f..c5e6467a21 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -19,32 +19,36 @@ #include #include +#include #include #include "platform.h" #if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) +#include "build/build_config.h" + #include "cms/cms.h" #include "cms/cms_types.h" -#include "string.h" #include "common/printf.h" #include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/vtx_common.h" -#include "io/serial.h" -#include "io/vtx_smartaudio.h" -#include "io/vtx_string.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" #include "flight/pid.h" -#include "config/config_master.h" -#include "build/build_config.h" +#include "io/serial.h" +#include "io/vtx_smartaudio.h" +#include "io/vtx_string.h" //#define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 63d2b70bdb..d2c368f28a 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 31 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) +#define API_VERSION_MINOR 32 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) #define API_VERSION_LENGTH 2 diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 942e4ce7ec..c077fb6e22 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -407,10 +407,8 @@ static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uin ----------------------------------------------- */ -void initJetiExBusTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initJetiExBusTelemetry(void) { - UNUSED(initialTelemetryConfig); - // Init Ex Bus Frame header jetiExBusTelemetryFrame[EXBUS_HEADER_SYNC] = 0x3B; // Startbytes jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01; @@ -427,7 +425,6 @@ void initJetiExBusTelemetry(telemetryConfig_t *initialTelemetryConfig) jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00 } - void createExTelemetrieTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor) { uint8_t labelLength = strlen(sensor->label); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 897e8bc0bd..309f220822 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -30,6 +30,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/adc.h" #include "drivers/rx_pwm.h" @@ -90,7 +92,6 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent) rxRuntimeConfig_t rxRuntimeConfig; -static const rxConfig_t *rxConfig; static uint8_t rcSampleIndex = 0; static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) @@ -108,7 +109,7 @@ static uint8_t nullFrameStatus(void) void useRxConfig(const rxConfig_t *rxConfigToUse) { - rxConfig = rxConfigToUse; + (void)(rxConfigToUse); } #define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels @@ -126,8 +127,8 @@ STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void) STATIC_UNIT_TESTED bool isPulseValid(uint16_t pulseDuration) { - return pulseDuration >= rxConfig->rx_min_usec && - pulseDuration <= rxConfig->rx_max_usec; + return pulseDuration >= rxConfig()->rx_min_usec && + pulseDuration <= rxConfig()->rx_max_usec; } // pulse duration is in micro seconds (usec) @@ -139,12 +140,12 @@ STATIC_UNIT_TESTED void rxUpdateFlightChannelStatus(uint8_t channel, bool valid) } } -void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration) { +void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig) { // set default calibration to full range and 1:1 mapping for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { - rxChannelRangeConfiguration->min = PWM_RANGE_MIN; - rxChannelRangeConfiguration->max = PWM_RANGE_MAX; - rxChannelRangeConfiguration++; + rxChannelRangeConfig->min = PWM_RANGE_MIN; + rxChannelRangeConfig->max = PWM_RANGE_MAX; + rxChannelRangeConfig++; } } @@ -204,20 +205,20 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig } #endif -void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeActivationConditions) +void rxInit(const rxConfig_t *initialRxConfig, const modeActivationCondition_t *modeActivationConditions) { - useRxConfig(rxConfig); + useRxConfig(initialRxConfig); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; rcSampleIndex = 0; needRxSignalMaxDelayUs = DELAY_10_HZ; for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rcData[i] = rxConfig->midrc; + rcData[i] = rxConfig()->midrc; rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; } - rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec; + rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; // Initialize ARM switch to OFF position when arming via switch is defined for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { @@ -237,7 +238,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { - const bool enabled = serialRxInit(rxConfig, &rxRuntimeConfig); + const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SERIAL); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -248,14 +249,14 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #ifdef USE_RX_MSP if (feature(FEATURE_RX_MSP)) { - rxMspInit(rxConfig, &rxRuntimeConfig); + rxMspInit(rxConfig(), &rxRuntimeConfig); needRxSignalMaxDelayUs = DELAY_5_HZ; } #endif #ifdef USE_RX_SPI if (feature(FEATURE_RX_SPI)) { - const bool enabled = rxSpiInit(rxConfig, &rxRuntimeConfig); + const bool enabled = rxSpiInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SPI); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -266,7 +267,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #if defined(USE_PWM) || defined(USE_PPM) if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { - rxPwmInit(rxConfig, &rxRuntimeConfig); + rxPwmInit(rxConfig(), &rxRuntimeConfig); } #endif } @@ -376,20 +377,20 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) static uint16_t getRxfailValue(uint8_t channel) { - const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; + const rxFailsafeChannelConfig_t *channelFailsafeConfig = &rxConfig()->failsafe_channel_configurations[channel]; - switch(channelFailsafeConfiguration->mode) { + switch(channelFailsafeConfig->mode) { case RX_FAILSAFE_MODE_AUTO: switch (channel) { case ROLL: case PITCH: case YAW: - return rxConfig->midrc; + return rxConfig()->midrc; case THROTTLE: if (feature(FEATURE_3D)) - return rxConfig->midrc; + return rxConfig()->midrc; else - return rxConfig->rx_min_usec; + return rxConfig()->rx_min_usec; } /* no break */ @@ -399,11 +400,11 @@ static uint16_t getRxfailValue(uint8_t channel) return rcData[channel]; case RX_FAILSAFE_MODE_SET: - return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step); + return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step); } } -STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfiguration_t range) +STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfig_t range) { // Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT if (sample == PPM_RCVR_TIMEOUT) { @@ -420,7 +421,7 @@ static uint8_t getRxChannelCount(void) { static uint8_t maxChannelsAllowed; if (!maxChannelsAllowed) { - uint8_t maxChannels = rxConfig->max_aux_channel + NON_AUX_CHANNEL_COUNT; + uint8_t maxChannels = rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT; if (maxChannels > rxRuntimeConfig.channelCount) { maxChannelsAllowed = rxRuntimeConfig.channelCount; } else { @@ -436,14 +437,14 @@ static void readRxChannelsApplyRanges(void) const int channelCount = getRxChannelCount(); for (int channel = 0; channel < channelCount; channel++) { - const uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); + const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); // sample the channel uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel); // apply the rx calibration if (channel < NON_AUX_CHANNEL_COUNT) { - sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[channel]); + sample = applyRxChannelRangeConfiguraton(sample, rxConfig()->channelRanges[channel]); } rcRaw[channel] = sample; @@ -548,10 +549,10 @@ static void updateRSSIPWM(void) { int16_t pwmRssi = 0; // Read value of AUX channel as rssi - pwmRssi = rcData[rxConfig->rssi_channel - 1]; + pwmRssi = rcData[rxConfig()->rssi_channel - 1]; // RSSI_Invert option - if (rxConfig->rssi_ppm_invert) { + if (rxConfig()->rssi_ppm_invert) { pwmRssi = ((2000 - pwmRssi) + 1000); } @@ -578,7 +579,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) int16_t adcRssiMean = 0; uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); - uint8_t rssiPercentage = adcRssiSample / rxConfig->rssi_scale; + uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale; adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT; @@ -599,7 +600,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) void updateRSSI(timeUs_t currentTimeUs) { - if (rxConfig->rssi_channel > 0) { + if (rxConfig()->rssi_channel > 0) { updateRSSIPWM(); } else if (feature(FEATURE_RSSI_ADC)) { updateRSSIADC(currentTimeUs); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 6d19d3c40a..6e588acf2d 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -18,6 +18,7 @@ #pragma once #include "common/time.h" +#include "config/parameter_group.h" #define STICK_CHANNEL_COUNT 4 @@ -100,15 +101,19 @@ typedef enum { #define RX_FAILSAFE_TYPE_COUNT 2 -typedef struct rxFailsafeChannelConfiguration_s { +typedef struct rxFailsafeChannelConfig_s { uint8_t mode; // See rxFailsafeChannelMode_e uint8_t step; -} rxFailsafeChannelConfiguration_t; +} rxFailsafeChannelConfig_t; -typedef struct rxChannelRangeConfiguration_s { +PG_DECLARE_ARRAY(rxFailsafeChannelConfig_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs); + +typedef struct rxChannelRangeConfig_s { uint16_t min; uint16_t max; -} rxChannelRangeConfiguration_t; +} rxChannelRangeConfig_t; + +PG_DECLARE_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs); typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order @@ -135,11 +140,13 @@ typedef struct rxConfig_s { uint16_t rx_min_usec; uint16_t rx_max_usec; - rxFailsafeChannelConfiguration_t failsafe_channel_configurations[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + rxFailsafeChannelConfig_t failsafe_channel_configurations[MAX_SUPPORTED_RC_CHANNEL_COUNT]; - rxChannelRangeConfiguration_t channelRanges[NON_AUX_CHANNEL_COUNT]; + rxChannelRangeConfig_t channelRanges[NON_AUX_CHANNEL_COUNT]; } rxConfig_t; +PG_DECLARE(rxConfig_t, rxConfig); + #define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0])) struct rxRuntimeConfig_s; @@ -166,7 +173,7 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); void parseRcChannels(const char *input, rxConfig_t *rxConfig); void updateRSSI(timeUs_t currentTimeUs); -void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration); +void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig); void suspendRxSignal(void); void resumeRxSignal(void); diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index 4fcfed7c5c..dec8412aed 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -28,4 +28,4 @@ void spektrumBind(rxConfig_t *rxConfig); bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); void srxlRxWriteTelemetryData(const void *data, int len); -bool srxlRxIsActive(void); \ No newline at end of file +bool srxlRxIsActive(void); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index ded23de3cf..c664aa2b5f 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -27,23 +27,26 @@ #include "common/axis.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" #include "drivers/accgyro_bma280.h" #include "drivers/accgyro_fake.h" #include "drivers/accgyro_l3g4200d.h" +#include "drivers/accgyro_l3gd20.h" +#include "drivers/accgyro_lsm303dlhc.h" #include "drivers/accgyro_mma845x.h" #include "drivers/accgyro_mpu.h" #include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu6050.h" #include "drivers/accgyro_mpu6500.h" -#include "drivers/accgyro_l3gd20.h" -#include "drivers/accgyro_lsm303dlhc.h" -#include "drivers/bus_spi.h" #include "drivers/accgyro_spi_icm20689.h" #include "drivers/accgyro_spi_mpu6000.h" #include "drivers/accgyro_spi_mpu6500.h" #include "drivers/accgyro_spi_mpu9250.h" +#include "drivers/bus_spi.h" #include "drivers/system.h" #include "fc/config.h" @@ -95,11 +98,7 @@ retry: #ifdef USE_ACC_ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently -#ifdef NAZE - if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, dev)) { -#else if (adxl345Detect(&acc_params, dev)) { -#endif #ifdef ACC_ADXL345_ALIGN dev->accAlign = ACC_ADXL345_ALIGN; #endif @@ -132,12 +131,7 @@ retry: ; // fallthrough case ACC_MMA8452: // MMA8452 #ifdef USE_ACC_MMA8452 -#ifdef NAZE - // Not supported with this frequency - if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) { -#else if (mma8452Detect(dev)) { -#endif #ifdef ACC_MMA8452_ALIGN dev->accAlign = ACC_MMA8452_ALIGN; #endif @@ -243,13 +237,13 @@ retry: return true; } -bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval) +bool accInit(uint32_t gyroSamplingInverval) { memset(&acc, 0, sizeof(acc)); // copy over the common gyro mpu settings - acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration; - acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult; - if (!accDetect(&acc.dev, accelerometerConfig->acc_hardware)) { + acc.dev.mpuConfiguration = *gyroMpuConfiguration(); + acc.dev.mpuDetectionResult = *gyroMpuDetectionResult(); + if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { return false; } acc.dev.acc_1G = 256; // set default @@ -275,6 +269,9 @@ bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamp biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval); } } + if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { + acc.dev.accAlign = accelerometerConfig()->acc_align; + } return true; } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index fda259e940..35de76374e 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/accgyro.h" #include "sensors/sensors.h" @@ -66,7 +67,9 @@ typedef struct accelerometerConfig_s { rollAndPitchTrims_t accelerometerTrims; } accelerometerConfig_t; -bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime); +PG_DECLARE(accelerometerConfig_t, accelerometerConfig); + +bool accInit(uint32_t gyroTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index bac86ad16c..d5a130df4e 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -23,6 +23,9 @@ #include "common/maths.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/barometer.h" #include "drivers/barometer_bmp085.h" #include "drivers/barometer_bmp280.h" @@ -51,8 +54,6 @@ static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; static uint32_t baroPressureSum = 0; -static const barometerConfig_t *barometerConfig; - bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) { // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function @@ -70,12 +71,6 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) bmp085Config = &defaultBMP085Config; #endif -#ifdef NAZE - if (hardwareRevision == NAZE32) { - bmp085Disable(bmp085Config); - } -#endif - #endif switch (baroHardware) { @@ -119,11 +114,6 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) return true; } -void useBarometerConfig(const barometerConfig_t *barometerConfigToUse) -{ - barometerConfig = barometerConfigToUse; -} - bool isBaroCalibrationComplete(void) { return calibratingB == 0; @@ -160,7 +150,7 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading) return newPressureReading; } -#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1) +#define PRESSURE_SAMPLE_COUNT (barometerConfig()->baro_sample_count - 1) static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading) { @@ -213,7 +203,7 @@ uint32_t baroUpdate(void) baro.dev.get_up(); baro.dev.start_ut(); baro.dev.calculate(&baroPressure, &baroTemperature); - baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure); + baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure); state = BAROMETER_NEEDS_SAMPLES; return baro.dev.ut_delay; break; @@ -228,7 +218,7 @@ int32_t baroCalculateAltitude(void) // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; - baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise + baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise return baro.BaroAlt; } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 536e6c453a..b753ef5eaa 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/barometer.h" typedef enum { @@ -37,6 +38,8 @@ typedef struct barometerConfig_s { float baro_cf_alt; // apply CF to use ACC for height estimation } barometerConfig_t; +PG_DECLARE(barometerConfig_t, barometerConfig); + typedef struct baro_s { baroDev_t dev; int32_t BaroAlt; @@ -46,7 +49,6 @@ typedef struct baro_s { extern baro_t baro; bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse); -void useBarometerConfig(const barometerConfig_t *barometerConfigToUse); bool isBaroCalibrationComplete(void); void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); uint32_t baroUpdate(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index f79e581d2e..6a780536aa 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -22,26 +22,26 @@ #include "build/debug.h" -#include "common/maths.h" #include "common/filter.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/adc.h" #include "drivers/system.h" #include "fc/config.h" +#include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "config/feature.h" +#include "io/beeper.h" #include "sensors/battery.h" #include "sensors/esc_sensor.h" -#include "fc/rc_controls.h" -#include "io/beeper.h" - -#include "rx/rx.h" - -#include "common/utils.h" #define VBAT_LPF_FREQ 0.4f #define IBAT_LPF_FREQ 0.4f @@ -61,7 +61,7 @@ uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) uint16_t vbatLatest = 0; // most recent unsmoothed value int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) -int32_t amperageLatest = 0; // most recent value +int32_t amperageLatest = 0; // most recent value int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start @@ -72,7 +72,7 @@ static uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V - return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier); + return ((((uint32_t)src * batteryConfig()->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig()->vbatresdivval))/batteryConfig()->vbatresdivmultiplier); } static void updateBatteryVoltage(void) @@ -86,7 +86,7 @@ static void updateBatteryVoltage(void) } #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { + if (feature(FEATURE_ESC_SENSOR) && batteryConfig()->batteryMeterType == BATTERY_SENSOR_ESC) { escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); vbatLatest = escData->dataAge <= MAX_ESC_BATTERY_AGE ? escData->voltage / 10 : 0; if (debugMode == DEBUG_BATTERY) { @@ -134,20 +134,20 @@ void updateBattery(void) uint16_t vBatMeasured = vbatLatest; /* battery has just been connected*/ - if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { + if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ vBatState = BATTERY_OK; - unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1; + unsigned cells = (vBatMeasured / batteryConfig()->vbatmaxcellvoltage) + 1; if (cells > 8) { // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; } batteryCellCount = cells; - batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; - batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; - /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */ - } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { + batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage; + batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage; + /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */ + } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { vBatState = BATTERY_NOT_PRESENT; batteryCellCount = 0; batteryWarningVoltage = 0; @@ -159,16 +159,16 @@ void updateBattery(void) debug[3] = batteryCellCount; } - if (batteryConfig->useVBatAlerts) { + if (batteryConfig()->useVBatAlerts) { switch(vBatState) { case BATTERY_OK: - if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { + if (vbat <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) { vBatState = BATTERY_WARNING; } break; case BATTERY_WARNING: - if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { + if (vbat <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) { vBatState = BATTERY_CRITICAL; } else if (vbat > batteryWarningVoltage) { vBatState = BATTERY_OK; @@ -206,9 +206,8 @@ const char * getBatteryStateString(void) return batteryStateStrings[getBatteryState()]; } -void batteryInit(batteryConfig_t *initialBatteryConfig) +void batteryInit(void) { - batteryConfig = initialBatteryConfig; vBatState = BATTERY_NOT_PRESENT; consumptionState = BATTERY_OK; batteryCellCount = 0; @@ -221,9 +220,9 @@ static int32_t currentSensorToCentiamps(uint16_t src) int32_t millivolts; millivolts = ((uint32_t)src * ADCVREF) / 4096; - millivolts -= batteryConfig->currentMeterOffset; + millivolts -= batteryConfig()->currentMeterOffset; - return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps + return (millivolts * 1000) / (int32_t)batteryConfig()->currentMeterScale; // current in 0.01A steps } static void updateBatteryCurrent(void) @@ -251,10 +250,10 @@ static void updateCurrentDrawn(int32_t lastUpdateAt) void updateConsumptionWarning(void) { - if (batteryConfig->useConsumptionAlerts && batteryConfig->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { + if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { if (calculateBatteryPercentage() == 0) { vBatState = BATTERY_CRITICAL; - } else if (calculateBatteryPercentage() <= batteryConfig->consumptionWarningPercentage) { + } else if (calculateBatteryPercentage() <= batteryConfig()->consumptionWarningPercentage) { consumptionState = BATTERY_WARNING; } else { consumptionState = BATTERY_OK; @@ -264,35 +263,29 @@ void updateConsumptionWarning(void) } } -void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void updateCurrentMeter(int32_t lastUpdateAt) { if (getBatteryState() != BATTERY_NOT_PRESENT) { - switch(batteryConfig->currentMeterType) { + switch(batteryConfig()->currentMeterType) { case CURRENT_SENSOR_ADC: updateBatteryCurrent(); - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_VIRTUAL: - amperageLatest = (int32_t)batteryConfig->currentMeterOffset; + amperageLatest = (int32_t)batteryConfig()->currentMeterOffset; if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) { throttleOffset = 0; } int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); - amperageLatest += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + amperageLatest += throttleFactor * (int32_t)batteryConfig()->currentMeterScale / 1000; } amperage = amperageLatest; - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_ESC: #ifdef USE_ESC_SENSOR @@ -328,7 +321,7 @@ float calculateVbatPidCompensation(void) { float batteryScaler = 1.0f; if (feature(FEATURE_VBAT) && batteryCellCount > 0) { // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference - batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); + batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); } return batteryScaler; } @@ -337,11 +330,11 @@ uint8_t calculateBatteryPercentage(void) { uint8_t batteryPercentage = 0; if (batteryCellCount > 0) { - uint16_t batteryCapacity = batteryConfig->batteryCapacity; + uint16_t batteryCapacity = batteryConfig()->batteryCapacity; if (batteryCapacity > 0) { batteryPercentage = constrain(((float)batteryCapacity - mAhDrawn) * 100 / batteryCapacity, 0, 100); } else { - batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); + batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100); } } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 3eb68940c8..91b59568ce 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -17,7 +17,7 @@ #pragma once -#include "common/maths.h" // for fix12_t +#include "config/parameter_group.h" #ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 110 @@ -63,6 +63,8 @@ typedef struct batteryConfig_s { uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning } batteryConfig_t; +PG_DECLARE(batteryConfig_t, batteryConfig); + typedef enum { BATTERY_OK = 0, BATTERY_WARNING, @@ -81,11 +83,10 @@ extern int32_t mAhDrawn; batteryState_e getBatteryState(void); const char * getBatteryStateString(void); void updateBattery(void); -void batteryInit(batteryConfig_t *initialBatteryConfig); -batteryConfig_t *batteryConfig; +void batteryInit(void); struct rxConfig_s; -void updateCurrentMeter(int32_t lastUpdateAt, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void updateCurrentMeter(int32_t lastUpdateAt); int32_t currentMeterToCentiamps(uint16_t src); float calculateVbatPidCompensation(void); diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index c4f1988274..0b45b8ffe8 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -23,6 +23,9 @@ #include "common/maths.h" #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/sensor.h" #include "boardalignment.h" diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index b1ab74cdf9..8c224f24e2 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -17,11 +17,15 @@ #pragma once +#include "config/parameter_group.h" + typedef struct boardAlignment_s { int32_t rollDegrees; int32_t pitchDegrees; int32_t yawDegrees; } boardAlignment_t; +PG_DECLARE(boardAlignment_t, boardAlignment); + void alignSensors(int32_t *dest, uint8_t rotation); void initBoardAlignment(const boardAlignment_t *boardAlignment); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 9cc1e29fc3..33393463cc 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -22,6 +22,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/compass.h" #include "drivers/compass_ak8975.h" #include "drivers/compass_ak8963.h" @@ -41,6 +44,7 @@ #include "hardware_revision.h" #endif +magDev_t magDev; mag_t mag; // mag access functions #ifdef MAG @@ -52,33 +56,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) { magSensor_e magHardware; -#ifdef USE_MAG_HMC5883 - const hmc5883Config_t *hmc5883Config = 0; - -#ifdef NAZE // TODO remove this target specific define - static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { - .intTag = IO_TAG(PB12) /* perhaps disabled? */ - }; - static const hmc5883Config_t nazeHmc5883Config_v5 = { - .intTag = IO_TAG(MAG_INT_EXTI) - }; - if (hardwareRevision < NAZE32_REV5) { - hmc5883Config = &nazeHmc5883Config_v1_v4; - } else { - hmc5883Config = &nazeHmc5883Config_v5; - } -#endif - -#ifdef MAG_INT_EXTI - static const hmc5883Config_t extiHmc5883Config = { - .intTag = IO_TAG(MAG_INT_EXTI) - }; - - hmc5883Config = &extiHmc5883Config; -#endif - -#endif - retry: dev->magAlign = ALIGN_DEFAULT; @@ -89,7 +66,7 @@ retry: case MAG_HMC5883: #ifdef USE_MAG_HMC5883 - if (hmc5883lDetect(dev, hmc5883Config)) { + if (hmc5883lDetect(dev, compassConfig()->interruptTag)) { #ifdef MAG_HMC5883_ALIGN dev->magAlign = MAG_HMC5883_ALIGN; #endif @@ -143,17 +120,25 @@ retry: return true; } -void compassInit(const compassConfig_t *compassConfig) +bool compassInit(void) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // calculate magnetic declination - const int16_t deg = compassConfig->mag_declination / 100; - const int16_t min = compassConfig->mag_declination % 100; + mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. + if (!compassDetect(&magDev, compassConfig()->mag_hardware)) { + return false; + } + const int16_t deg = compassConfig()->mag_declination / 100; + const int16_t min = compassConfig()->mag_declination % 100; mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units LED1_ON; - mag.dev.init(); + magDev.init(); LED1_OFF; magInit = 1; + if (compassConfig()->mag_align != ALIGN_DEFAULT) { + magDev.magAlign = compassConfig()->mag_align; + } + return true; } void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) @@ -162,11 +147,11 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; - mag.dev.read(magADCRaw); + magDev.read(magADCRaw); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { mag.magADC[axis] = magADCRaw[axis]; } - alignSensors(mag.magADC, mag.dev.magAlign); + alignSensors(mag.magADC, magDev.magAlign); if (STATE(CALIBRATE_MAG)) { tCal = currentTime; diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 4e534fea08..6194743330 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -17,11 +17,12 @@ #pragma once -#include "drivers/compass.h" +#include "config/parameter_group.h" +#include "drivers/io.h" +#include "drivers/sensor.h" #include "sensors/sensors.h" - // Type of magnetometer used/detected typedef enum { MAG_DEFAULT = 0, @@ -32,7 +33,6 @@ typedef enum { } magSensor_e; typedef struct mag_s { - magDev_t dev; int32_t magADC[XYZ_AXIS_COUNT]; float magneticDeclination; } mag_t; @@ -44,11 +44,13 @@ typedef struct compassConfig_s { // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. sensor_align_e mag_align; // mag alignment uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device + ioTag_t interruptTag; flightDynamicsTrims_t magZero; } compassConfig_t; -bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); -void compassInit(const compassConfig_t *compassConfig); +PG_DECLARE(compassConfig_t, compassConfig); + +bool compassInit(void); union flightDynamicsTrims_u; void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero); diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index aad7b1ea4a..0e4fce0455 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -21,26 +21,31 @@ #include -#include "fc/config.h" -#include "config/feature.h" -#include "config/config_master.h" +#ifdef USE_DSHOT +#include "build/debug.h" + +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "common/maths.h" #include "common/utils.h" -#include "drivers/system.h" +#include "drivers/pwm_output.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" -#include "drivers/pwm_output.h" +#include "drivers/system.h" -#include "io/serial.h" +#include "esc_sensor.h" + +#include "fc/config.h" #include "flight/mixer.h" #include "sensors/battery.h" -#include "build/debug.h" - -#include "esc_sensor.h" +#include "io/serial.h" /* KISS ESC TELEMETRY PROTOCOL @@ -69,7 +74,6 @@ set debug_mode = DEBUG_ESC_TELEMETRY in cli */ -#ifdef USE_DSHOT enum { DEBUG_ESC_MOTOR_INDEX = 0, DEBUG_ESC_NUM_TIMEOUTS = 1, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 32d2864525..c06b4b0869 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -28,18 +28,21 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" #include "drivers/accgyro_bma280.h" #include "drivers/accgyro_fake.h" #include "drivers/accgyro_l3g4200d.h" +#include "drivers/accgyro_l3gd20.h" +#include "drivers/accgyro_lsm303dlhc.h" #include "drivers/accgyro_mma845x.h" #include "drivers/accgyro_mpu.h" #include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu6050.h" #include "drivers/accgyro_mpu6500.h" -#include "drivers/accgyro_l3gd20.h" -#include "drivers/accgyro_lsm303dlhc.h" #include "drivers/accgyro_spi_icm20689.h" #include "drivers/accgyro_spi_mpu6000.h" #include "drivers/accgyro_spi_mpu6500.h" @@ -56,20 +59,22 @@ #include "scheduler/scheduler.h" -#include "sensors/sensors.h" #include "sensors/boardalignment.h" #include "sensors/gyro.h" +#include "sensors/sensors.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif -gyro_t gyro; // gyro access functions +gyro_t gyro; + +STATIC_UNIT_TESTED gyroDev_t gyroDev0; +static int16_t gyroTemperature0; static int32_t gyroADC[XYZ_AXIS_COUNT]; -static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; -static const gyroConfig_t *gyroConfig; +STATIC_UNIT_TESTED int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -81,6 +86,30 @@ static void *notchFilter2[3]; #define DEBUG_GYRO_CALIBRATION 3 +#ifdef STM32F10X +#define GYRO_SYNC_DENOM_DEFAULT 8 +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) +#define GYRO_SYNC_DENOM_DEFAULT 1 +#else +#define GYRO_SYNC_DENOM_DEFAULT 4 +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); + +PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, + .gyro_lpf = GYRO_LPF_256HZ, + .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT, + .gyro_soft_lpf_type = FILTER_PT1, + .gyro_soft_lpf_hz = 90, + .gyro_soft_notch_hz_1 = 400, + .gyro_soft_notch_cutoff_1 = 300, + .gyro_soft_notch_hz_2 = 200, + .gyro_soft_notch_cutoff_2 = 100, + .gyro_align = ALIGN_DEFAULT, + .gyroMovementCalibrationThreshold = 32 +); + +#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) static const extiConfig_t *selectMPUIntExtiConfig(void) { #if defined(MPU_INT_EXTI) @@ -92,8 +121,18 @@ static const extiConfig_t *selectMPUIntExtiConfig(void) return NULL; #endif } +#endif -static bool gyroDetect(gyroDev_t *dev) +const mpuConfiguration_t *gyroMpuConfiguration(void) +{ + return &gyroDev0.mpuConfiguration; +} +const mpuDetectionResult_t *gyroMpuDetectionResult(void) +{ + return &gyroDev0.mpuDetectionResult; +} + +STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) { gyroSensor_e gyroHardware = GYRO_DEFAULT; @@ -221,36 +260,29 @@ static bool gyroDetect(gyroDev_t *dev) gyroHardware = GYRO_NONE; } + if (gyroHardware != GYRO_NONE) { + detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; + sensorsSet(SENSOR_GYRO); + } + + + return gyroHardware; +} + +bool gyroInit(void) +{ + memset(&gyro, 0, sizeof(gyro)); +#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) + gyroDev0.mpuIntExtiConfig = selectMPUIntExtiConfig(); + mpuDetect(&gyroDev0); + mpuResetFn = gyroDev0.mpuConfiguration.resetFn; +#endif + const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0); if (gyroHardware == GYRO_NONE) { return false; } - detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; - sensorsSet(SENSOR_GYRO); - - return true; -} - -bool gyroInit(const gyroConfig_t *gyroConfigToUse) -{ - gyroConfig = gyroConfigToUse; - memset(&gyro, 0, sizeof(gyro)); -#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) - gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig(); - mpuDetect(&gyro.dev); - mpuReset = gyro.dev.mpuConfiguration.reset; -#endif - - if (!gyroDetect(&gyro.dev)) { - return false; - } - - switch (detectedSensors[SENSOR_INDEX_GYRO]) { - default: - // gyro does not support 32kHz - // cast away constness, legitimate as this is cross-validation - ((gyroConfig_t*)gyroConfig)->gyro_use_32khz = false; - break; + switch (gyroHardware) { case GYRO_MPU6500: case GYRO_MPU9250: case GYRO_ICM20689: @@ -258,12 +290,19 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) case GYRO_ICM20602: // do nothing, as gyro supports 32kHz break; + default: + // gyro does not support 32kHz + gyroConfigMutable()->gyro_use_32khz = false; + break; } // Must set gyro sample rate before initialisation - gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz); - gyro.dev.lpf = gyroConfig->gyro_lpf; - gyro.dev.init(&gyro.dev); + gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); + gyroDev0.lpf = gyroConfig()->gyro_lpf; + gyroDev0.init(&gyroDev0); + if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { + gyroDev0.gyroAlign = gyroConfig()->gyro_align; + } gyroInitFilters(); return true; } @@ -282,43 +321,43 @@ void gyroInitFilters(void) uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed - if (gyroConfig->gyro_soft_lpf_hz && gyroConfig->gyro_soft_lpf_hz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known - if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { + if (gyroConfig()->gyro_soft_lpf_hz && gyroConfig()->gyro_soft_lpf_hz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known + if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterLPF[axis]; - biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); + biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime); } - } else if (gyroConfig->gyro_soft_lpf_type == FILTER_PT1) { + } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterPt1[axis]; - pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt); + pt1FilterInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyroDt); } } else { softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroDenoiseState[axis]; - firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); + firFilterDenoiseInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime); } } } - if (gyroConfig->gyro_soft_notch_hz_1 && gyroConfig->gyro_soft_notch_hz_1 <= gyroFrequencyNyquist) { + if (gyroConfig()->gyro_soft_notch_hz_1 && gyroConfig()->gyro_soft_notch_hz_1 <= gyroFrequencyNyquist) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; - const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); + const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); + biquadFilterInit(notchFilter1[axis], gyroConfig()->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); } } - if (gyroConfig->gyro_soft_notch_hz_2 && gyroConfig->gyro_soft_notch_hz_2 <= gyroFrequencyNyquist) { + if (gyroConfig()->gyro_soft_notch_hz_2 && gyroConfig()->gyro_soft_notch_hz_2 <= gyroFrequencyNyquist) { notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; - const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); + const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); for (int axis = 0; axis < 3; axis++) { notchFilter2[axis] = &gyroFilterNotch_2[axis]; - biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); + biquadFilterInit(notchFilter2[axis], gyroConfig()->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); } } } @@ -348,7 +387,7 @@ void gyroSetCalibrationCycles(void) calibratingG = gyroCalculateCalibratingCycles(); } -static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold) +STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold) { static int32_t g[3]; static stdev_t var[3]; @@ -424,27 +463,27 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev) void gyroUpdate(void) { // range: +/- 8192; +/- 2000 deg/sec - if (gyro.dev.update) { + if (gyroDev0.update) { // if the gyro update function is set then return, since the gyro is read in gyroUpdateISR return; } - if (!gyro.dev.read(&gyro.dev)) { + if (!gyroDev0.read(&gyroDev0)) { return; } - gyro.dev.dataReady = false; + gyroDev0.dataReady = false; // move gyro data into 32-bit variables to avoid overflows in calculations - gyroADC[X] = gyro.dev.gyroADCRaw[X]; - gyroADC[Y] = gyro.dev.gyroADCRaw[Y]; - gyroADC[Z] = gyro.dev.gyroADCRaw[Z]; + gyroADC[X] = gyroDev0.gyroADCRaw[X]; + gyroADC[Y] = gyroDev0.gyroADCRaw[Y]; + gyroADC[Z] = gyroDev0.gyroADCRaw[Z]; - alignSensors(gyroADC, gyro.dev.gyroAlign); + alignSensors(gyroADC, gyroDev0.gyroAlign); const bool calibrationComplete = isGyroCalibrationComplete(); if (calibrationComplete) { #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) // SPI-based gyro so can read and update in ISR - if (gyroConfig->gyro_isr_update) { - mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR); + if (gyroConfig()->gyro_isr_update) { + mpuGyroSetIsrUpdate(&gyroDev0, gyroUpdateISR); return; } #endif @@ -452,13 +491,13 @@ void gyroUpdate(void) debug[3] = (uint16_t)(micros() & 0xffff); #endif } else { - performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); + performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold); } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADC[axis] -= gyroZero[axis]; // scale gyro output to degrees per second - float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale; + float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale; // Apply LPF DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); @@ -472,8 +511,25 @@ void gyroUpdate(void) } if (!calibrationComplete) { - gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyro.dev.scale); - gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyro.dev.scale); - gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyro.dev.scale); + gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale); + gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale); + gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale); } } + +void gyroReadTemperature(void) +{ + if (gyroDev0.temperature) { + gyroDev0.temperature(&gyroDev0, &gyroTemperature0); + } +} + +int16_t gyroGetTemperature(void) +{ + return gyroTemperature0; +} + +int16_t gyroRateDps(int axis) +{ + return lrintf(gyro.gyroADCf[axis] / gyroDev0.scale); +} diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 1fdf8820a6..5dd80d3a48 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -17,8 +17,9 @@ #pragma once -#include "drivers/accgyro.h" +#include "config/parameter_group.h" #include "common/axis.h" +#include "drivers/sensor.h" typedef enum { GYRO_NONE = 0, @@ -37,7 +38,6 @@ typedef enum { } gyroSensor_e; typedef struct gyro_s { - gyroDev_t dev; uint32_t targetLooptime; float gyroADCf[XYZ_AXIS_COUNT]; } gyro_t; @@ -59,8 +59,17 @@ typedef struct gyroConfig_s { uint16_t gyro_soft_notch_cutoff_2; } gyroConfig_t; -void gyroSetCalibrationCycles(void); -bool gyroInit(const gyroConfig_t *gyroConfigToUse); +PG_DECLARE(gyroConfig_t, gyroConfig); + +bool gyroInit(void); void gyroInitFilters(void); void gyroUpdate(void); +struct mpuConfiguration_s; +const struct mpuConfiguration_s *gyroMpuConfiguration(void); +struct mpuDetectionResult_s; +const struct mpuDetectionResult_s *gyroMpuDetectionResult(void); +void gyroSetCalibrationCycles(void); bool isGyroCalibrationComplete(void); +void gyroReadTemperature(void); +int16_t gyroGetTemperature(void); +int16_t gyroRateDps(int axis); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 01b396b2c1..bbccc94143 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,6 +24,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -52,51 +54,28 @@ static bool sonarDetect(void) } #endif -bool sensorsAutodetect(const gyroConfig_t *gyroConfig, - const accelerometerConfig_t *accelerometerConfig, - const compassConfig_t *compassConfig, - const barometerConfig_t *barometerConfig, - const sonarConfig_t *sonarConfig) +bool sensorsAutodetect(void) { // gyro must be initialised before accelerometer - if (!gyroInit(gyroConfig)) { + if (!gyroInit()) { return false; } - accInit(accelerometerConfig, gyro.targetLooptime); + accInit(gyro.targetLooptime); - mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. #ifdef MAG - if (compassDetect(&mag.dev, compassConfig->mag_hardware)) { - compassInit(compassConfig); - } -#else - UNUSED(compassConfig); + compassInit(); #endif #ifdef BARO - baroDetect(&baro.dev, barometerConfig->baro_hardware); -#else - UNUSED(barometerConfig); + baroDetect(&baro.dev, barometerConfig()->baro_hardware); #endif #ifdef SONAR if (sonarDetect()) { - sonarInit(sonarConfig); + sonarInit(sonarConfig()); } -#else - UNUSED(sonarConfig); #endif - if (gyroConfig->gyro_align != ALIGN_DEFAULT) { - gyro.dev.gyroAlign = gyroConfig->gyro_align; - } - if (accelerometerConfig->acc_align != ALIGN_DEFAULT) { - acc.dev.accAlign = accelerometerConfig->acc_align; - } - if (compassConfig->mag_align != ALIGN_DEFAULT) { - mag.dev.magAlign = compassConfig->mag_align; - } - return true; } diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index e656294c6f..9b5d045046 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -17,8 +17,4 @@ #pragma once -bool sensorsAutodetect(const gyroConfig_t *gyroConfig, - const accelerometerConfig_t *accConfig, - const compassConfig_t *compassConfig, - const barometerConfig_t *baroConfig, - const sonarConfig_t *sonarConfig); +bool sensorsAutodetect(void); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index a7e6f10308..a93446511a 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -25,7 +25,6 @@ typedef enum { SENSOR_INDEX_COUNT } sensorIndex_e; -extern int16_t telemTemperature1; //FIXME move to temp sensor...? extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; typedef struct int16_flightDynamicsTrims_s { diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index e8c78c6087..0e3d0768a0 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -29,6 +29,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "fc/runtime_config.h" diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 16151fc2f9..d123679394 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -17,10 +17,9 @@ #pragma once +#include "config/parameter_group.h" #include "common/time.h" - #include "drivers/sonar_hcsr04.h" - #include "sensors/battery.h" #define SONAR_OUT_OF_RANGE (-1) diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index a6b455f81b..d39e00200e 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -21,17 +21,18 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), // PWM1 - PA4 - *TIM3_CH2 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1), // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1), // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1), // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index effa6ea316..5560a598d0 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -87,9 +87,6 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -//#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) -#define SENSORS_SET (SENSOR_ACC) - #undef GPS #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 693c42ebd1..d5bb6bf671 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -44,7 +44,7 @@ void targetConfiguration(master_t *config) config->rxConfig.spektrum_sat_bind_autoreset = 1; if (hardwareMotorType == MOTOR_BRUSHED) { - config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + config->motorConfig.dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; } config->profile[0].pidProfile.P8[ROLL] = 90; diff --git a/src/main/target/ALIENFLIGHTF1/target.mk b/src/main/target/ALIENFLIGHTF1/target.mk index 844b4982e7..cc553693d2 100644 --- a/src/main/target/ALIENFLIGHTF1/target.mk +++ b/src/main/target/ALIENFLIGHTF1/target.mk @@ -1,5 +1,4 @@ F1_TARGETS += $(TARGET) -FEATURES = HIGHEND TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 3499fbade4..a9cff7bfb9 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -86,7 +86,7 @@ void targetConfiguration(master_t *config) config->compassConfig.mag_hardware = MAG_NONE; // disabled by default if (hardwareMotorType == MOTOR_BRUSHED) { - config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + config->motorConfig.dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; config->pidConfig.pid_process_denom = 1; } diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index e2b5831003..0f56688c2f 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -63,7 +63,7 @@ void targetConfiguration(master_t *config) config->compassConfig.mag_hardware = MAG_NONE; // disabled by default if (hardwareMotorType == MOTOR_BRUSHED) { - config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + config->motorConfig.dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; config->pidConfig.pid_process_denom = 1; } @@ -76,7 +76,7 @@ void targetConfiguration(master_t *config) config->rxConfig.sbus_inversion = 0; config->serialConfig.portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; config->telemetryConfig.telemetry_inversion = 0; - intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY, &config->enabledFeatures); + intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY, &config->featureConfig.enabledFeatures); } config->profile[0].pidProfile.P8[ROLL] = 53; diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index c2542cc6b4..2ea9ca20ef 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -138,8 +138,6 @@ #define I2C_DEVICE (I2CDEV_4) //#define I2C_DEVICE_EXT (I2CDEV_2) -#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) - #define USE_ADC #define VBAT_ADC_PIN PC0 #define CURRENT_METER_ADC_PIN PC1 diff --git a/src/main/target/BETAFLIGHTF3/config.c b/src/main/target/BETAFLIGHTF3/config.c index f0d840b06e..5b7ecc8ce2 100755 --- a/src/main/target/BETAFLIGHTF3/config.c +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -21,20 +21,8 @@ #include #ifdef TARGET_CONFIG -#include "common/utils.h" - -#include "drivers/io.h" - -#include "fc/rc_controls.h" - -#include "flight/failsafe.h" -#include "flight/mixer.h" -#include "flight/pid.h" - -#include "rx/rx.h" - -#include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" void targetConfiguration(master_t *config) { diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index b7f3a1a01c..a3043b4971 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -128,7 +128,7 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 #define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_CURRENT_METER | FEATURE_TELEMETRY ) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_OSD) #define SPEKTRUM_BIND #define BIND_PIN UART2_RX_PIN diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 9a1c7df207..077287fcfe 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -36,11 +36,11 @@ void targetConfiguration(master_t *config) if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { config->gyroConfig.gyro_align = CW180_DEG; config->accelerometerConfig.acc_align = CW180_DEG; - config->beeperConfig.ioTag = IO_TAG(BEEPER_OPT); + config->beeperDevConfig.ioTag = IO_TAG(BEEPER_OPT); } if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) { - intFeatureClear(FEATURE_SDCARD, &config->enabledFeatures); + intFeatureClear(FEATURE_SDCARD, &config->featureConfig.enabledFeatures); } if (hardwareRevision == BJF4_MINI_REV3A) { @@ -52,7 +52,7 @@ void targetValidateConfiguration(master_t *config) { /* make sure the SDCARD cannot be turned on */ if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) { - intFeatureClear(FEATURE_SDCARD, &config->enabledFeatures); + intFeatureClear(FEATURE_SDCARD, &config->featureConfig.enabledFeatures); if (config->blackboxConfig.device == BLACKBOX_DEVICE_SDCARD) { config->blackboxConfig.device = BLACKBOX_DEVICE_FLASH; diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index d8c53e4787..7c638f767f 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -33,9 +33,6 @@ #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 -#define USE_I2C -#define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 - #define MPU6000_CS_GPIO GPIOA #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 @@ -58,6 +55,9 @@ // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL +//#define USE_I2C +//#define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 + // External I2C BARO //#define BARO //#define USE_BARO_MS5611 diff --git a/src/main/target/CC3D/target.mk b/src/main/target/CC3D/target.mk index 5682d063da..a9cdd0ad7c 100644 --- a/src/main/target/CC3D/target.mk +++ b/src/main/target/CC3D/target.mk @@ -1,5 +1,5 @@ F1_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH HIGHEND VCP +FEATURES = ONBOARDFLASH VCP TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index c39067af48..3129a4fa5d 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -120,8 +120,6 @@ #define I2C3_SCL PA8 #define I2C3_SDA PC9 -#define SENSORS_SET (SENSOR_ACC) - #define LED_STRIP // alternative defaults for Colibri/Gemini target diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c index dc1c2da645..2ae165a846 100644 --- a/src/main/target/COLIBRI_RACE/config.c +++ b/src/main/target/COLIBRI_RACE/config.c @@ -15,20 +15,24 @@ * along with Cleanflight. If not, see . */ -#include #include #include #include #ifdef TARGET_CONFIG + +#include "common/maths.h" +#include "common/utils.h" + +#include "config/config_master.h" +#include "config/feature.h" + +#include "io/ledstrip.h" #include "io/motors.h" #include "sensors/battery.h" -#include "config/config_master.h" -#include "config/feature.h" -#include "io/ledstrip.h" void targetApplyDefaultLedStripConfig(ledConfig_t *ledConfigs) { @@ -53,7 +57,7 @@ void targetConfiguration(master_t *config) config->motorConfig.minthrottle = 1025; config->motorConfig.maxthrottle = 1980; config->motorConfig.mincommand = 1000; - config->servoConfig.servoCenterPulse = 1500; + config->servoConfig.dev.servoCenterPulse = 1500; config->batteryConfig.vbatmaxcellvoltage = 45; config->batteryConfig.vbatmincellvoltage = 30; @@ -97,4 +101,16 @@ void targetConfiguration(master_t *config) targetApplyDefaultLedStripConfig(config->ledStripConfig.ledConfigs); } + +void targetValidateConfiguration(master_t *config) +{ + UNUSED(config); + + 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 diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 18717103cd..b6070664c2 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -70,15 +70,9 @@ #include "config/config_master.h" #include "config/feature.h" -#ifdef NAZE -#include "hardware_revision.h" -#endif - #include "bus_bst.h" #include "i2c_bst.h" -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse); - #define BST_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made @@ -615,7 +609,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16(acc.accSmooth[i] / scale); } for (i = 0; i < 3; i++) { - bstWrite16(lrintf(gyro.gyroADCf[i] /gyro.dev.scale)); + bstWrite16(gyroRateDps(i)); } for (i = 0; i < 3; i++) { bstWrite16(mag.magADC[i]); @@ -640,13 +634,13 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_SERVO_MIX_RULES: for (i = 0; i < MAX_SERVO_RULES; i++) { - bstWrite8(customServoMixer(i)->targetChannel); - bstWrite8(customServoMixer(i)->inputSource); - bstWrite8(customServoMixer(i)->rate); - bstWrite8(customServoMixer(i)->speed); - bstWrite8(customServoMixer(i)->min); - bstWrite8(customServoMixer(i)->max); - bstWrite8(customServoMixer(i)->box); + bstWrite8(customServoMixers(i)->targetChannel); + bstWrite8(customServoMixers(i)->inputSource); + bstWrite8(customServoMixers(i)->rate); + bstWrite8(customServoMixers(i)->speed); + bstWrite8(customServoMixers(i)->min); + bstWrite8(customServoMixers(i)->max); + bstWrite8(customServoMixers(i)->box); } break; #endif @@ -1067,7 +1061,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) mac->range.startStep = bstRead8(); mac->range.endStep = bstRead8(); - useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationProfile()->modeActivationConditions, ¤tProfile->pidProfile); } else { ret = BST_FAILED; } @@ -1179,13 +1173,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) if (i >= MAX_SERVO_RULES) { ret = BST_FAILED; } else { - customServoMixer(i)->targetChannel = bstRead8(); - customServoMixer(i)->inputSource = bstRead8(); - customServoMixer(i)->rate = bstRead8(); - customServoMixer(i)->speed = bstRead8(); - customServoMixer(i)->min = bstRead8(); - customServoMixer(i)->max = bstRead8(); - customServoMixer(i)->box = bstRead8(); + customServoMixers(i)->targetChannel = bstRead8(); + customServoMixers(i)->inputSource = bstRead8(); + customServoMixers(i)->rate = bstRead8(); + customServoMixers(i)->speed = bstRead8(); + customServoMixers(i)->min = bstRead8(); + customServoMixers(i)->max = bstRead8(); + customServoMixers(i)->box = bstRead8(); loadCustomServoMixer(); } #endif diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 42d28fd8a5..3565c7a28b 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -21,6 +21,7 @@ #define BST_DEVICE_NAME "COLIBRI RACE" #define BST_DEVICE_NAME_LENGTH 12 #define TARGET_CONFIG +#define TARGET_VALIDATECONFIG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index d830cec728..4697c3f180 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -136,7 +136,6 @@ // *************** Others ************************** #define DISPLAY #define LED_STRIP -#define OSD #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index d34516cb2d..2e9c3a7f81 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -137,8 +137,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 434c740e84..b2fcbb4871 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -130,7 +130,7 @@ #define LED_STRIP -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART3 diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index 8642a48e36..302500538a 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -119,7 +119,7 @@ #define RSSI_ADC_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_OSD) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/MICROSCISKY/target.mk b/src/main/target/MICROSCISKY/target.mk index 88034cc60a..274eb198da 100644 --- a/src/main/target/MICROSCISKY/target.mk +++ b/src/main/target/MICROSCISKY/target.mk @@ -1,5 +1,4 @@ F1_TARGETS += $(TARGET) -FEATURES = HIGHEND TARGET_SRC = \ drivers/accgyro_mpu.c \ diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 79053bf713..f736df072a 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -87,8 +87,6 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define SENSORS_SET (SENSOR_ACC) - #undef GPS #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index e335f672ad..6c8e8c595c 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -22,6 +22,7 @@ #ifdef TARGET_CONFIG #include "common/axis.h" +#include "common/maths.h" #include "drivers/sensor.h" #include "drivers/compass.h" @@ -70,7 +71,7 @@ void targetConfiguration(master_t *config) config->failsafeConfig.failsafe_delay = 2; config->failsafeConfig.failsafe_off_delay = 0; - config->motorConfig.motorPwmRate = 17000; + config->motorConfig.dev.motorPwmRate = 17000; config->gyroConfig.gyro_sync_denom = 4; config->pidConfig.pid_process_denom = 1; diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index ee3ad4db62..7abbabf3b4 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -44,7 +44,7 @@ void targetConfiguration(master_t *config) #ifdef BEEBRAIN // alternative defaults settings for Beebrain target - config->motorConfig.motorPwmRate = 4000; + config->motorConfig.dev.motorPwmRate = 4000; config->failsafeConfig.failsafe_delay = 2; config->failsafeConfig.failsafe_off_delay = 0; @@ -89,13 +89,28 @@ void targetConfiguration(master_t *config) #if !defined(AFROMINI) && !defined(BEEBRAIN) if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. - config->beeperConfig.isOpenDrain = false; - config->beeperConfig.isInverted = true; + config->beeperDevConfig.isOpenDrain = false; + config->beeperDevConfig.isInverted = true; } else { - config->beeperConfig.isOpenDrain = true; - config->beeperConfig.isInverted = false; + config->beeperDevConfig.isOpenDrain = true; + config->beeperDevConfig.isInverted = false; config->flashConfig.csTag = IO_TAG_NONE; } #endif + +#ifdef MAG_INT_EXTI + if (hardwareRevision < NAZE32_REV5) { + config->compassConfig.interruptTag = IO_TAG(PB12); + } +#endif +} + +void targetValidateConfiguration(master_t *config) +{ + UNUSED(config); + + if (hardwareRevision < NAZE32_REV5 && config->accelerometerConfig.acc_hardware == ACC_ADXL345) { + config->accelerometerConfig.acc_hardware = ACC_NONE; + } } #endif diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 9f992744c1..2d77fc283e 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -18,6 +18,7 @@ #pragma once #define TARGET_CONFIG +#define TARGET_VALIDATECONFIG #define USE_HARDWARE_REVISION_DETECTION #define TARGET_BUS_INIT @@ -49,16 +50,10 @@ #define USE_EXTI #define MAG_INT_EXTI PC14 #define MPU_INT_EXTI PC13 -//#define DEBUG_MPU_DATA_READY_INTERRUPT -#define USE_MPU_DATA_READY_SIGNAL -//#define DEBUG_MAG_DATA_READY_INTERRUPT -#define USE_MAG_DATA_READY_SIGNAL +#define MMA8451_INT_PIN PA5 -// SPI2 -// PB15 28 SPI2_MOSI -// PB14 27 SPI2_MISO -// PB13 26 SPI2_SCK -// PB12 25 SPI2_NSS +#define USE_MPU_DATA_READY_SIGNAL +#define USE_MAG_DATA_READY_SIGNAL #define USE_SPI #define USE_SPI_DEVICE_2 @@ -101,8 +96,7 @@ #define ACC_MPU6500_ALIGN CW0_DEG #define BARO -#define USE_BARO_MS5611 -#define USE_BARO_BMP085 +#define USE_BARO_MS5611 // needed for Flip32 board #define USE_BARO_BMP280 /* diff --git a/src/main/target/NAZE/target.mk b/src/main/target/NAZE/target.mk index 48f6e46cf4..d9371ed744 100644 --- a/src/main/target/NAZE/target.mk +++ b/src/main/target/NAZE/target.mk @@ -1,5 +1,5 @@ F1_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH HIGHEND +FEATURES = ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_adxl345.c \ diff --git a/src/main/target/NUCLEOF7/target.h b/src/main/target/NUCLEOF7/target.h index 97071b4392..810e628171 100644 --- a/src/main/target/NUCLEOF7/target.h +++ b/src/main/target/NUCLEOF7/target.h @@ -139,8 +139,6 @@ //#define I2C_DEVICE_EXT (I2CDEV_2) -#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) - #define USE_ADC #define VBAT_ADC_PIN PA3 #define CURRENT_METER_ADC_PIN PC0 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 6284ddaadb..4072ffea50 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -162,7 +162,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_OSD) #define BUTTONS #define BUTTON_A_PIN PB1 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 872a7bad04..6820c2f629 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -167,10 +167,8 @@ #define LED_STRIP -#define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_VBAT) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD) #define AVOID_UART1_FOR_PWM_PPM @@ -179,10 +177,10 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) +#define TARGET_IO_PORTB (0xffff & ~(BIT(2))) +#define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14)|BIT(13))) +#define TARGET_IO_PORTD BIT(2) #define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 11706ee900..92235bc5c1 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -71,8 +71,6 @@ #define USE_SPI #define USE_SPI_DEVICE_2 -#define SENSORS_SET (SENSOR_ACC) - #define TELEMETRY #define BLACKBOX #define SERIAL_RX diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index 1d825ecd4e..3855d9c25b 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -109,7 +109,7 @@ #define USE_SERVOS #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_VBAT | FEATURE_RSSI_ADC) +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_VBAT | FEATURE_RSSI_ADC | FEATURE_OSD) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index 5e910d8ff6..28bec2ccea 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -26,11 +26,11 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // PWM3 - PA8 - DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, 1), // PWM2 - PA7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM4 - PB0 - DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), // PWM1 - PA4 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM5 - PB1 - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, 0), // PWM6 - PPM - DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), // PWM6 - PPM + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, 1), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, 0), + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), }; diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index c3379802bd..3c226b2c8c 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -95,8 +95,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) - #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 06096aff11..97b0caa096 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -189,8 +189,6 @@ #define LED_STRIP -#define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #if defined(PODIUMF4) #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/RG_SSD_F3/config.c b/src/main/target/RG_SSD_F3/config.c index e58067221e..56fd96c4cc 100644 --- a/src/main/target/RG_SSD_F3/config.c +++ b/src/main/target/RG_SSD_F3/config.c @@ -24,6 +24,6 @@ void targetConfiguration(master_t *config) { config->batteryConfig.vbatmaxcellvoltage = 45; - config->batteryConfig.currentMeterScale = 119; + config->batteryConfig.vbatscale = VBAT_SCALE_DEFAULT; } #endif diff --git a/src/main/target/RG_SSD_F3/target.h b/src/main/target/RG_SSD_F3/target.h index 51b43634cb..74f2b9ff81 100644 --- a/src/main/target/RG_SSD_F3/target.h +++ b/src/main/target/RG_SSD_F3/target.h @@ -52,6 +52,7 @@ #define USE_ACC_SPI_MPU6000 #define ACC_MPU6000_ALIGN CW180_DEG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT +#define TARGET_CONFIG #define USE_EXTI #define MPU_INT_EXTI PC13 @@ -122,7 +123,7 @@ #define VBAT_ADC_PIN PA4 #define CURRENT_METER_ADC_PIN PA5 - +#define VBAT_SCALE_DEFAULT 119 #define LED_STRIP #define TRANSPONDER diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index cc82da505a..cdb6fb86ce 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -28,9 +28,6 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -// early prototype had slightly different pin mappings. -//#define SPRACINGF3MINI_MKII_REVA - #define LED0 PB3 #endif @@ -39,36 +36,35 @@ #define USE_EXTI #define MPU_INT_EXTI PC13 -#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH - #define GYRO #define ACC -#define BARO -#define USE_BARO_BMP280 - #ifdef TINYBEEF3 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT + #define USE_GYRO_SPI_MPU6500 #define GYRO_MPU6500_ALIGN CW270_DEG #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW270_DEG - -#define MAG_AK8963_ALIGN CW90_DEG_FLIP #else -//#define USE_FAKE_GYRO +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + #define USE_GYRO_MPU6500 #define GYRO_MPU6500_ALIGN CW180_DEG -//#define USE_FAKE_ACC #define USE_ACC_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG +#define BARO +#define USE_BARO_BMP280 + #define MAG #define USE_MPU9250_MAG // Enables bypass configuration #define USE_MAG_AK8975 @@ -80,14 +76,10 @@ //#define SONAR_ECHO_PIN PB1 //#define SONAR_TRIGGER_PIN PB0 +#define BRUSHED_ESC_AUTODETECT + #define USB_IO -#ifndef TINYBEEF3 -#define USB_CABLE_DETECTION - -#define USB_DETECT_PIN PB5 -#endif - #define USE_VCP #define USE_UART1 #define USE_UART2 @@ -104,6 +96,8 @@ #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 @@ -113,6 +107,7 @@ #define UART3_TX_PIN PB10 // PB10 (AF7) #define UART3_RX_PIN PB11 // PB11 (AF7) + #ifndef TINYBEEF3 #define SOFTSERIAL1_RX_PIN PA0 // PA0 / PAD3 #define SOFTSERIAL1_TX_PIN PA1 // PA1 / PAD4 @@ -120,16 +115,10 @@ #define SONAR_SOFTSERIAL1_EXCLUSIVE -#define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define SERIAL_PORT_COUNT 5 +#endif #define USE_SPI -#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 #ifdef TINYBEEF3 #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) @@ -139,9 +128,18 @@ #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 -#define MPU6500_CS_PIN PB9 +#define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_INSTANCE SPI1 -#endif +#else +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 #define USE_SDCARD #define USE_SDCARD_SPI2 @@ -164,6 +162,9 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#endif + #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC #define ADC_INSTANCE ADC2 @@ -177,16 +178,11 @@ #define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#ifdef TINYBEEF3 -#define BRUSHED_ESC_AUTODETECT -#else -#define DEFAULT_FEATURES FEATURE_BLACKBOX -#endif #ifndef TINYBEEF3 +#define DEFAULT_FEATURES FEATURE_BLACKBOX + #define BUTTONS #define BUTTON_A_PIN PB1 #define BUTTON_B_PIN PB0 @@ -195,12 +191,6 @@ #define BINDPLUG_PIN PB0 #endif -#define SPEKTRUM_BIND -// USART3, -#define BIND_PIN PB11 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 6f1f506bff..181527be32 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -1,5 +1,5 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD HIGHEND +FEATURES = VCP SDCARD TARGET_SRC = \ drivers/accgyro_adxl345.c \ diff --git a/src/main/target/TINYFISH/config.c b/src/main/target/TINYFISH/config.c index 6ae8488d87..009ce10b3f 100644 --- a/src/main/target/TINYFISH/config.c +++ b/src/main/target/TINYFISH/config.c @@ -53,6 +53,6 @@ void targetConfiguration(master_t *config) config->telemetryConfig.telemetry_inversion = 0; config->rxConfig.sbus_inversion = 0; - intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT, &config->enabledFeatures); + intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT, &config->featureConfig.enabledFeatures); } #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index 65eb2bd9de..5c1a538dcf 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -17,6 +17,7 @@ #pragma once +//#define USE_PARAMETER_GROUPS // type conversion warnings. // -Wconversion can be turned on to enable the process of eliminating these warnings //#pragma GCC diagnostic warning "-Wconversion" diff --git a/src/main/target/link/stm32_flash.ld b/src/main/target/link/stm32_flash.ld index 2cc81c9fd5..eaf65e7b9a 100644 --- a/src/main/target/link/stm32_flash.ld +++ b/src/main/target/link/stm32_flash.ld @@ -81,6 +81,19 @@ SECTIONS KEEP (*(SORT(.fini_array.*))) PROVIDE_HIDDEN (__fini_array_end = .); } >FLASH + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >FLASH + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >FLASH /* used by the startup to initialize data */ _sidata = .; diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d62326e113..b8bbb9f008 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -32,11 +32,10 @@ #define CLEANFLIGHT #endif -#ifdef CLEANFLIGHT #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#endif +#include "common/maths.h" #include "common/streambuf.h" #include "common/utils.h" @@ -184,7 +183,7 @@ void crsfFrameBatterySensor(sbuf_t *dst) const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage(); #else crsfSerialize16(dst, amperage / 10); - const uint32_t batteryCapacity = batteryConfig->batteryCapacity; + const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); #endif crsfSerialize8(dst, (batteryCapacity >> 16)); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index f696419016..932f4bdc0e 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -33,6 +33,8 @@ #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/sensor.h" @@ -72,13 +74,10 @@ static serialPortConfig_t *portConfig; #define FRSKY_BAUDRATE 9600 #define FRSKY_INITIAL_PORT_MODE MODE_TX -static telemetryConfig_t *telemetryConfig; static bool frskyTelemetryEnabled = false; static portSharing_e frskyPortSharing; -extern batteryConfig_t *batteryConfig; - #define CYCLETIME 125 #define PROTOCOL_HEADER 0x5E @@ -196,24 +195,21 @@ static void sendGpsAltitude(void) } #endif -static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +static void sendThrottleOrBatterySizeAsRpm(void) { sendDataHead(ID_RPM); #ifdef USE_ESC_SENSOR - UNUSED(rxConfig); - UNUSED(deadband3d_throttle); - escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0); #else if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; serialize16(throttleForRPM); } else { - serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER)); + serialize16((batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER)); } #endif } @@ -227,7 +223,7 @@ static void sendTemperature1(void) #elif defined(BARO) serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf #else - serialize16(telemTemperature1 / 10); + serialize16(gyroGetTemperature() / 10); #endif } @@ -240,7 +236,7 @@ static void sendSatalliteSignalQualityAsTemperature2(void) } sendDataHead(ID_TEMPRATURE2); - if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) { + if (telemetryConfig()->frsky_unit == FRSKY_UNIT_METRICS) { serialize16(satellite); } else { float tmp = (satellite - 32) / 1.8f; @@ -286,7 +282,7 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 min = absgps / GPS_DEGREES_DIVIDER; // minutes left - if (telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS) { + if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) { result->dddmm = deg * 100 + min; } else { result->dddmm = deg * 60 + min; @@ -332,8 +328,8 @@ static void sendFakeLatLong(void) // Heading is only displayed on OpenTX if non-zero lat/long is also sent int32_t coord[2] = {0,0}; - coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); - coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); + coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); + coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); sendLatLong(coord); } @@ -409,7 +405,7 @@ static void sendVoltage(void) */ static void sendVoltageAmp(void) { - if (telemetryConfig->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { + if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { /* * Use new ID 0x39 to send voltage directly in 0.1 volts resolution */ @@ -418,7 +414,7 @@ static void sendVoltageAmp(void) } else { uint16_t voltage = (getVbat() * 110) / 21; uint16_t vfasVoltage; - if (telemetryConfig->frsky_vfas_cell_voltage) { + if (telemetryConfig()->frsky_vfas_cell_voltage) { vfasVoltage = voltage / batteryCellCount; } else { vfasVoltage = voltage; @@ -440,7 +436,7 @@ static void sendFuelLevel(void) { sendDataHead(ID_FUEL_LEVEL); - if (batteryConfig->batteryCapacity > 0) { + if (batteryConfig()->batteryCapacity > 0) { serialize16((uint16_t)calculateBatteryPercentage()); } else { serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); @@ -455,9 +451,8 @@ static void sendHeading(void) serialize16(0); } -void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initFrSkyTelemetry(void) { - telemetryConfig = initialTelemetryConfig; portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY); frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY); } @@ -475,7 +470,7 @@ void configureFrSkyTelemetryPort(void) return; } - frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); + frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); if (!frskyPort) { return; } @@ -509,7 +504,7 @@ void checkFrSkyTelemetryState(void) } } -void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void handleFrSkyTelemetry(void) { if (!frskyTelemetryEnabled) { return; @@ -540,7 +535,7 @@ void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); - sendThrottleOrBatterySizeAsRpm(rxConfig, deadband3d_throttle); + sendThrottleOrBatterySizeAsRpm(); if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) { sendVoltage(); diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 89ba617c8f..255dae90de 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -22,12 +22,10 @@ typedef enum { FRSKY_VFAS_PRECISION_HIGH } frskyVFasPrecision_e; -struct rxConfig_s; -void handleFrSkyTelemetry(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void handleFrSkyTelemetry(void); void checkFrSkyTelemetryState(void); -struct telemetryConfig_s; -void initFrSkyTelemetry(struct telemetryConfig_s *telemetryConfig); +void initFrSkyTelemetry(void); void configureFrSkyTelemetryPort(void); void freeFrSkyTelemetryPort(void); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index c43822b2a3..dfae08c7e8 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -65,6 +65,10 @@ #include "common/axis.h" #include "common/time.h" +#include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" @@ -108,7 +112,6 @@ static uint8_t hottMsgCrc; static serialPort_t *hottPort = NULL; static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; static bool hottTelemetryEnabled = false; static portSharing_e hottPortSharing; @@ -219,7 +222,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) static bool shouldTriggerBatteryAlarmNow(void) { - return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND)); + return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig()->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND)); } static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) @@ -289,9 +292,8 @@ void freeHoTTTelemetryPort(void) hottTelemetryEnabled = false; } -void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initHoTTTelemetry(void) { - telemetryConfig = initialTelemetryConfig; portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT); hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT); diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index 5370dc24f3..660c4266ab 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -490,7 +490,7 @@ typedef struct HOTT_AIRESC_MSG_s { void handleHoTTTelemetry(timeUs_t currentTimeUs); void checkHoTTTelemetryState(void); -void initHoTTTelemetry(telemetryConfig_t *telemetryConfig); +void initHoTTTelemetry(void); void configureHoTTTelemetryPort(void); void freeHoTTTelemetryPort(void); diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index 8e6341e90b..42c8b40d85 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -32,7 +32,8 @@ #if defined(TELEMETRY) && defined(TELEMETRY_IBUS) -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "common/axis.h" @@ -299,7 +300,7 @@ static void dispatchMeasurementReply(ibusAddress_t address) #ifdef BARO value = (baro.baroTemperature + 5) / 10; // +5 to make integer division rounding correct #else - value = telemTemperature1 * 10; + value = gyroGetTemperature() * 10; #endif sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET); break; diff --git a/src/main/telemetry/ibus.h b/src/main/telemetry/ibus.h index 8bbd30c5d8..3688ea2ffc 100644 --- a/src/main/telemetry/ibus.h +++ b/src/main/telemetry/ibus.h @@ -17,13 +17,11 @@ #pragma once -/* typedef struct ibusTelemetryConfig_s { uint8_t report_cell_voltage; // report vbatt divided with cellcount } ibusTelemetryConfig_t; PG_DECLARE(ibusTelemetryConfig_t, ibusTelemetryConfig); -*/ void initIbusTelemetry(void); @@ -31,4 +29,4 @@ void handleIbusTelemetry(void); bool checkIbusTelemetryState(void); void configureIbusTelemetryPort(void); -void freeIbusTelemetryPort(void); \ No newline at end of file +void freeIbusTelemetryPort(void); diff --git a/src/main/telemetry/jetiexbus.h b/src/main/telemetry/jetiexbus.h index 79440c0df2..9c8327db4a 100644 --- a/src/main/telemetry/jetiexbus.h +++ b/src/main/telemetry/jetiexbus.h @@ -17,7 +17,6 @@ #pragma once -struct telemetryConfig_s; -void initJetiExBusTelemetry(struct telemetryConfig_s *initialTelemetryConfig); +void initJetiExBusTelemetry(void); void checkJetiExBusTelemetryState(void); void handleJetiExBusTelemetry(void); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 038bbf497c..fec48d7b75 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -40,6 +40,7 @@ #include "common/maths.h" #include "common/axis.h" #include "common/color.h" +#include "common/utils.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -80,7 +81,6 @@ static serialPort_t *ltmPort; static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; static bool ltmEnabled; static portSharing_e ltmPortSharing; static uint8_t ltm_crc; @@ -268,9 +268,8 @@ void freeLtmTelemetryPort(void) ltmEnabled = false; } -void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initLtmTelemetry(void) { - telemetryConfig = initialTelemetryConfig; portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM); ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM); } diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index e5b4790c5a..892d0efdd8 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -19,8 +19,7 @@ #pragma once -struct telemetryConfig_s; -void initLtmTelemetry(struct telemetryConfig_s *initialTelemetryConfig); +void initLtmTelemetry(void); void handleLtmTelemetry(void); void checkLtmTelemetryState(void); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 7dbf92a1e2..5423919613 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -32,26 +32,18 @@ #include "common/axis.h" #include "common/color.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "fc/config.h" #include "fc/rc_controls.h" - -#include "io/serial.h" -#include "io/gimbal.h" -#include "io/gps.h" -#include "io/ledstrip.h" -#include "io/motors.h" - -#include "sensors/sensors.h" -#include "sensors/acceleration.h" -#include "sensors/gyro.h" -#include "sensors/barometer.h" -#include "sensors/boardalignment.h" -#include "sensors/battery.h" - -#include "rx/rx.h" +#include "fc/runtime_config.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -60,16 +52,24 @@ #include "flight/navigation.h" #include "flight/altitudehold.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/motors.h" + +#include "rx/rx.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + #include "telemetry/telemetry.h" #include "telemetry/mavlink.h" -#include "fc/config.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - -#include "fc/runtime_config.h" - // mavlink library uses unnames unions that's causes GCC to complain if -Wpedantic is used // until this is resolved in mavlink library - ignore -Wpedantic for mavlink code #pragma GCC diagnostic push diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 5b047861e6..7698b4e385 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -16,6 +16,11 @@ #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/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" @@ -52,9 +57,6 @@ #include "telemetry/telemetry.h" #include "telemetry/smartport.h" -#include "config/config_profile.h" -#include "config/feature.h" - #include "msp/msp.h" extern profile_t *currentProfile; @@ -148,7 +150,6 @@ const uint16_t frSkyDataIdTable[] = { static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port. static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; static bool smartPortTelemetryEnabled = false; static portSharing_e smartPortPortSharing; @@ -302,9 +303,8 @@ static void smartPortSendPackage(uint16_t id, uint32_t val) smartPortSendPackageEx(FSSP_DATA_FRAME,payload); } -void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initSmartPortTelemetry(void) { - telemetryConfig = initialTelemetryConfig; portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT); smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT); } @@ -326,11 +326,11 @@ void configureSmartPortTelemetryPort(void) portOptions_t portOptions = 0; - if (telemetryConfig->sportHalfDuplex) { + if (telemetryConfig()->sportHalfDuplex) { portOptions |= SERIAL_BIDIR; } - if (telemetryConfig->telemetry_inversion) { + if (telemetryConfig()->telemetry_inversion) { portOptions |= SERIAL_INVERTED; } @@ -622,7 +622,7 @@ void handleSmartPortTelemetry(void) case FSSP_DATAID_VFAS : if (feature(FEATURE_VBAT) && batteryCellCount > 0) { uint16_t vfasVoltage; - if (telemetryConfig->frsky_vfas_cell_voltage) { + if (telemetryConfig()->frsky_vfas_cell_voltage) { vfasVoltage = getVbat() / batteryCellCount; } else { vfasVoltage = getVbat(); @@ -752,7 +752,7 @@ void handleSmartPortTelemetry(void) } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; - } else if (telemetryConfig->pidValuesAsTelemetry){ + } else if (telemetryConfig()->pidValuesAsTelemetry){ switch (t2Cnt) { case 0: tmp2 = currentProfile->pidProfile.P8[ROLL]; diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index d50c1fd575..35f18fc4ae 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -7,7 +7,7 @@ #pragma once -void initSmartPortTelemetry(telemetryConfig_t *); +void initSmartPortTelemetry(void); void handleSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 928acdc2b7..f1cda235a9 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -25,6 +25,9 @@ #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" @@ -50,29 +53,23 @@ #include "telemetry/srxl.h" #include "telemetry/ibus.h" -static telemetryConfig_t *telemetryConfig; - -void telemetryUseConfig(telemetryConfig_t *telemetryConfigToUse) -{ - telemetryConfig = telemetryConfigToUse; -} void telemetryInit(void) { #ifdef TELEMETRY_FRSKY - initFrSkyTelemetry(telemetryConfig); + initFrSkyTelemetry(); #endif #ifdef TELEMETRY_HOTT - initHoTTTelemetry(telemetryConfig); + initHoTTTelemetry(); #endif #ifdef TELEMETRY_SMARTPORT - initSmartPortTelemetry(telemetryConfig); + initSmartPortTelemetry(); #endif #ifdef TELEMETRY_LTM - initLtmTelemetry(telemetryConfig); + initLtmTelemetry(); #endif #ifdef TELEMETRY_JETIEXBUS - initJetiExBusTelemetry(telemetryConfig); + initJetiExBusTelemetry(); #endif #ifdef TELEMETRY_MAVLINK initMAVLinkTelemetry(); @@ -95,7 +92,7 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) bool enabled = portSharing == PORTSHARING_NOT_SHARED; if (portSharing == PORTSHARING_SHARED) { - if (telemetryConfig->telemetry_switch) + if (telemetryConfig()->telemetry_switch) enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY); else enabled = ARMING_FLAG(ARMED); @@ -142,13 +139,10 @@ void telemetryCheckState(void) #endif } -void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void telemetryProcess(uint32_t currentTime) { #ifdef TELEMETRY_FRSKY - handleFrSkyTelemetry(rxConfig, deadband3d_throttle); -#else - UNUSED(rxConfig); - UNUSED(deadband3d_throttle); + handleFrSkyTelemetry(); #endif #ifdef TELEMETRY_HOTT handleHoTTTelemetry(currentTime); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index aa60912147..0fc00cae57 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -24,6 +24,9 @@ #pragma once +#include "config/parameter_group.h" +#include "io/serial.h" + typedef enum { FRSKY_FORMAT_DMS = 0, FRSKY_FORMAT_NMEA @@ -35,11 +38,11 @@ typedef enum { } frskyUnit_e; typedef struct telemetryConfig_s { + float gpsNoFixLatitude; + float gpsNoFixLongitude; uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inversion; // also shared with smartport inversion uint8_t sportHalfDuplex; - float gpsNoFixLatitude; - float gpsNoFixLongitude; frskyGpsCoordFormat_e frsky_coordinate_format; frskyUnit_e frsky_unit; uint8_t frsky_vfas_precision; @@ -49,18 +52,18 @@ typedef struct telemetryConfig_s { uint8_t report_cell_voltage; } telemetryConfig_t; -void telemetryInit(void); -bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); -extern serialPort_t *telemetrySharedPort; - -void telemetryCheckState(void); -struct rxConfig_s; -void telemetryProcess(uint32_t currentTime, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); - -bool telemetryDetermineEnabledState(portSharing_e portSharing); - -void telemetryUseConfig(telemetryConfig_t *telemetryConfig); +PG_DECLARE(telemetryConfig_t, telemetryConfig); #define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) +extern serialPort_t *telemetrySharedPort; + +void telemetryInit(void); +bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); + +void telemetryCheckState(void); +void telemetryProcess(uint32_t currentTime); + +bool telemetryDetermineEnabledState(portSharing_e portSharing); + void releaseSharedTelemetryPorts(void); diff --git a/src/test/Makefile b/src/test/Makefile index 9f1e83940d..da633d8636 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -256,24 +256,24 @@ $(OBJECT_DIR)/altitude_hold_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/flight/gps_conversion.o : \ - $(USER_DIR)/flight/gps_conversion.c \ - $(USER_DIR)/flight/gps_conversion.h \ +$(OBJECT_DIR)/common/gps_conversion.o : \ + $(USER_DIR)/common/gps_conversion.c \ + $(USER_DIR)/common/gps_conversion.h \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/gps_conversion.c -o $@ $(OBJECT_DIR)/gps_conversion_unittest.o : \ $(TEST_DIR)/gps_conversion_unittest.cc \ - $(USER_DIR)/flight/gps_conversion.h \ + $(USER_DIR)/common/gps_conversion.h \ $(GTEST_HEADERS) @mkdir -p $(dir $@) $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@ $(OBJECT_DIR)/gps_conversion_unittest : \ - $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/common/gps_conversion.o \ $(OBJECT_DIR)/gps_conversion_unittest.o \ $(OBJECT_DIR)/gtest_main.a @@ -359,7 +359,7 @@ $(OBJECT_DIR)/telemetry_hott_unittest.o : \ $(OBJECT_DIR)/telemetry_hott_unittest : \ $(OBJECT_DIR)/telemetry/hott.o \ $(OBJECT_DIR)/telemetry_hott_unittest.o \ - $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/common/gps_conversion.o \ $(OBJECT_DIR)/gtest_main.a $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ @@ -537,7 +537,7 @@ $(OBJECT_DIR)/telemetry_crsf_unittest : \ $(OBJECT_DIR)/telemetry_crsf_unittest.o \ $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/common/streambuf.o \ - $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/common/gps_conversion.o \ $(OBJECT_DIR)/fc/runtime_config.o \ $(OBJECT_DIR)/gtest_main.a @@ -675,6 +675,60 @@ $(OBJECT_DIR)/alignsensor_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/build/debug.o : \ + $(USER_DIR)/build/debug.c \ + $(USER_DIR)/build/debug.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/build/debug.c -o $@ + +$(OBJECT_DIR)/drivers/gyro_sync.o : \ + $(USER_DIR)/drivers/gyro_sync.c \ + $(USER_DIR)/drivers/gyro_sync.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/gyro_sync.c -o $@ + +$(OBJECT_DIR)/drivers/accgyro_fake.o : \ + $(USER_DIR)/drivers/accgyro_fake.c \ + $(USER_DIR)/drivers/accgyro_fake.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/accgyro_fake.c -o $@ + +$(OBJECT_DIR)/sensors/gyro.o : \ + $(USER_DIR)/sensors/gyro.c \ + $(USER_DIR)/sensors/gyro.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/gyro.c -o $@ + +$(OBJECT_DIR)/sensor_gyro_unittest.o : \ + $(TEST_DIR)/sensor_gyro_unittest.cc \ + $(USER_DIR)/sensors/gyro.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/sensor_gyro_unittest.cc -o $@ + +$(OBJECT_DIR)/sensor_gyro_unittest : \ + $(OBJECT_DIR)/build/debug.o \ + $(OBJECT_DIR)/common/filter.o \ + $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/drivers/accgyro_fake.o \ + $(OBJECT_DIR)/drivers/gyro_sync.o \ + $(OBJECT_DIR)/sensors/boardalignment.o \ + $(OBJECT_DIR)/sensors/gyro.o \ + $(OBJECT_DIR)/sensor_gyro_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + + $(OBJECT_DIR)/build/version.o : \ $(USER_DIR)/build/version.c \ $(USER_DIR)/build/version.h \ diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc index b839912c00..c422e24731 100644 --- a/src/test/unit/cms_unittest.cc +++ b/src/test/unit/cms_unittest.cc @@ -26,6 +26,7 @@ #define BARO extern "C" { + #include "platform.h" #include "target.h" #include "drivers/display.h" #include "cms/cms.h" diff --git a/src/test/unit/parameter_groups_unittest.cc b/src/test/unit/parameter_groups_unittest.cc index df45a1829c..4a0543bd32 100644 --- a/src/test/unit/parameter_groups_unittest.cc +++ b/src/test/unit/parameter_groups_unittest.cc @@ -22,16 +22,14 @@ #include extern "C" { - #include "build/debug.h" - #include - + #include "build/debug.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" - #include "io/motors.h" + #include "flight/mixer.h" -PG_DECLARE(motorConfig_t, motorConfig); +//PG_DECLARE(motorConfig_t, motorConfig); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1); @@ -39,7 +37,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .minthrottle = 1150, .maxthrottle = 1850, .mincommand = 1000, - .motorPwmRate = 400, + .dev = {.motorPwmRate = 400} ); } @@ -49,32 +47,32 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, TEST(ParameterGroupsfTest, Test_pgResetAll) { - memset(motorConfig(), 0, sizeof(motorConfig_t)); + memset(motorConfigMutable(), 0, sizeof(motorConfig_t)); pgResetAll(0); EXPECT_EQ(1150, motorConfig()->minthrottle); EXPECT_EQ(1850, motorConfig()->maxthrottle); EXPECT_EQ(1000, motorConfig()->mincommand); - EXPECT_EQ(400, motorConfig()->motorPwmRate); + EXPECT_EQ(400, motorConfig()->dev.motorPwmRate); } TEST(ParameterGroupsfTest, Test_pgFind) { - memset(motorConfig(), 0, sizeof(motorConfig_t)); + memset(motorConfigMutable(), 0, sizeof(motorConfig_t)); const pgRegistry_t *pgRegistry = pgFind(PG_MOTOR_CONFIG); pgResetCurrent(pgRegistry); EXPECT_EQ(1150, motorConfig()->minthrottle); EXPECT_EQ(1850, motorConfig()->maxthrottle); EXPECT_EQ(1000, motorConfig()->mincommand); - EXPECT_EQ(400, motorConfig()->motorPwmRate); + EXPECT_EQ(400, motorConfig()->dev.motorPwmRate); motorConfig_t motorConfig2; memset(&motorConfig2, 0, sizeof(motorConfig_t)); - motorConfig()->motorPwmRate = 500; + motorConfigMutable()->dev.motorPwmRate = 500; pgStore(pgRegistry, &motorConfig2, sizeof(motorConfig_t), 0); EXPECT_EQ(1150, motorConfig2.minthrottle); EXPECT_EQ(1850, motorConfig2.maxthrottle); EXPECT_EQ(1000, motorConfig2.mincommand); - EXPECT_EQ(500, motorConfig2.motorPwmRate); + EXPECT_EQ(500, motorConfig2.dev.motorPwmRate); motorConfig_t motorConfig3; memset(&motorConfig3, 0, sizeof(motorConfig_t)); @@ -82,7 +80,7 @@ TEST(ParameterGroupsfTest, Test_pgFind) EXPECT_EQ(1150, motorConfig3.minthrottle); EXPECT_EQ(1850, motorConfig3.maxthrottle); EXPECT_EQ(1000, motorConfig3.mincommand); - EXPECT_EQ(400, motorConfig3.motorPwmRate); + EXPECT_EQ(400, motorConfig3.dev.motorPwmRate); } // STUBS diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index c5c400aa85..2179e912b7 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -17,6 +17,8 @@ #pragma once +#define USE_PARAMETER_GROUPS + #define U_ID_0 0 #define U_ID_1 1 #define U_ID_2 2 @@ -30,8 +32,6 @@ #define USE_SERVOS #define TRANSPONDER -#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 - typedef enum { Mode_TEST = 0x0, @@ -57,6 +57,15 @@ typedef struct void* test; } TIM_TypeDef; +typedef struct +{ + void* test; +} TIM_OCInitTypeDef; + +typedef struct { + void* test; +} DMA_TypeDef; + typedef struct { void* test; } DMA_Channel_TypeDef; @@ -65,6 +74,11 @@ uint8_t DMA_GetFlagStatus(void *); void DMA_Cmd(DMA_Channel_TypeDef*, FunctionalState ); void DMA_ClearFlag(uint32_t); +typedef struct +{ + void* test; +} SPI_TypeDef; + typedef struct { void* test; diff --git a/src/test/unit/target.h b/src/test/unit/target.h index cc8149c790..e4d6073426 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -19,6 +19,7 @@ #define CMS #define CMS_MAX_DEVICE 4 +#define USE_FAKE_GYRO #define MAG #define BARO #define GPS @@ -56,8 +57,6 @@ #define SERIAL_PORT_COUNT 8 -#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 - #define TARGET_BOARD_IDENTIFIER "TEST" #define LED_STRIP_TIMER 1 diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index bb1a526d7e..4fb0cea52f 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -22,40 +22,42 @@ #include extern "C" { - #include "build/debug.h" - #include + #include "build/debug.h" + #include "common/axis.h" #include "common/filter.h" + #include "common/gps_conversion.h" #include "common/maths.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" - #include "drivers/system.h" #include "drivers/serial.h" + #include "drivers/system.h" #include "fc/runtime_config.h" + #include "flight/pid.h" + #include "flight/imu.h" + #include "io/gps.h" #include "io/serial.h" #include "rx/crsf.h" - #include "sensors/sensors.h" #include "sensors/battery.h" + #include "sensors/sensors.h" - #include "telemetry/telemetry.h" #include "telemetry/crsf.h" - - #include "flight/pid.h" - #include "flight/imu.h" - #include "flight/gps_conversion.h" + #include "telemetry/telemetry.h" bool airMode; uint16_t vbat; serialPort_t *telemetrySharedPort; + PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); } #include "unittest_macros.h" @@ -135,10 +137,7 @@ TEST(TelemetryCrsfTest, TestGPS) TEST(TelemetryCrsfTest, TestBattery) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; - batteryConfig_t workingBatteryConfig; - batteryConfig = &workingBatteryConfig; - memset(batteryConfig, 0, sizeof(batteryConfig_t)); vbat = 0; // 0.1V units int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); @@ -157,7 +156,7 @@ TEST(TelemetryCrsfTest, TestBattery) vbat = 33; // 3.3V = 3300 mv amperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps - batteryConfig->batteryCapacity = 1234; + batteryConfigMutable()->batteryCapacity = 1234; frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); voltage = frame[3] << 8 | frame[4]; // mV * 100 EXPECT_EQ(33, voltage); @@ -307,7 +306,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;} bool telemetryDetermineEnabledState(portSharing_e) {return true;} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} -portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} +portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} uint8_t batteryCapacityRemainingPercentage(void) {return 67;} uint8_t calculateBatteryCapacityRemainingPercentage(void) {return 67;} diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index ff984b097b..29d9cd59cf 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -27,24 +27,30 @@ extern "C" { #include "build/debug.h" #include "common/axis.h" + #include "common/gps_conversion.h" + + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" #include "drivers/system.h" #include "drivers/serial.h" + #include "drivers/system.h" - #include "sensors/sensors.h" - #include "sensors/battery.h" - #include "sensors/barometer.h" + #include "fc/runtime_config.h" + + #include "flight/pid.h" - #include "io/serial.h" #include "io/gps.h" + #include "io/serial.h" + + #include "sensors/barometer.h" + #include "sensors/battery.h" + #include "sensors/sensors.h" #include "telemetry/telemetry.h" #include "telemetry/hott.h" - #include "flight/pid.h" - #include "flight/gps_conversion.h" - - #include "fc/runtime_config.h" + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); } #include "unittest_macros.h" @@ -246,7 +252,7 @@ bool telemetryDetermineEnabledState(portSharing_e) return true; } -portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) +portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) { return PORTSHARING_NOT_SHARED; }