Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'betaflight/master' into bfdev-smartaudio

Browse source
This commit is contained in:
jflyper 2016-12-30 00:11:57 +09:00
commit 86bb650617
109 changed files with 3283 additions and 1706 deletions
Download patch file Download diff file Expand all files Collapse all files

9
Makefile
View file

@ -505,8 +505,8 @@ COMMON_SRC = \
drivers/rx_xn297.c \ drivers/rx_xn297.c \
drivers/pwm_esc_detect.c \ drivers/pwm_esc_detect.c \
drivers/pwm_output.c \ drivers/pwm_output.c \
drivers/pwm_rx.c \
drivers/rcc.c \ drivers/rcc.c \
drivers/rx_pwm.c \
drivers/serial.c \ drivers/serial.c \
drivers/serial_uart.c \ drivers/serial_uart.c \
drivers/sound_beeper.c \ drivers/sound_beeper.c \
@ -592,6 +592,7 @@ HIGHEND_SRC = \
sensors/barometer.c \ sensors/barometer.c \
telemetry/telemetry.c \ telemetry/telemetry.c \
telemetry/crsf.c \ telemetry/crsf.c \
telemetry/srxl.c \
telemetry/frsky.c \ telemetry/frsky.c \
telemetry/hott.c \ telemetry/hott.c \
telemetry/smartport.c \ telemetry/smartport.c \
@ -623,8 +624,8 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
drivers/rx_spi.c \ drivers/rx_spi.c \
drivers/rx_xn297.c \ drivers/rx_xn297.c \
drivers/pwm_output.c \ drivers/pwm_output.c \
drivers/pwm_rx.c \
drivers/rcc.c \ drivers/rcc.c \
drivers/rx_pwm.c \
drivers/serial.c \ drivers/serial.c \
drivers/serial_uart.c \ drivers/serial_uart.c \
drivers/sound_beeper.c \ drivers/sound_beeper.c \
@ -719,7 +720,7 @@ VCP_SRC = \
vcp_hal/usbd_desc.c \ vcp_hal/usbd_desc.c \
vcp_hal/usbd_conf.c \ vcp_hal/usbd_conf.c \
vcp_hal/usbd_cdc_interface.c \ vcp_hal/usbd_cdc_interface.c \
drivers/serial_usb_vcp_hal.c drivers/serial_usb_vcp.c
else else
VCP_SRC = \ VCP_SRC = \
vcp/hw_config.c \ vcp/hw_config.c \
@ -786,7 +787,6 @@ STM32F7xx_COMMON_SRC = \
drivers/pwm_output_stm32f7xx.c \ drivers/pwm_output_stm32f7xx.c \
drivers/timer_hal.c \ drivers/timer_hal.c \
drivers/timer_stm32f7xx.c \ drivers/timer_stm32f7xx.c \
drivers/pwm_output_hal.c \
drivers/system_stm32f7xx.c \ drivers/system_stm32f7xx.c \
drivers/serial_uart_stm32f7xx.c \ drivers/serial_uart_stm32f7xx.c \
drivers/serial_uart_hal.c drivers/serial_uart_hal.c
@ -794,7 +794,6 @@ STM32F7xx_COMMON_SRC = \
F7EXCLUDES = drivers/bus_spi.c \ F7EXCLUDES = drivers/bus_spi.c \
drivers/bus_i2c.c \ drivers/bus_i2c.c \
drivers/timer.c \ drivers/timer.c \
drivers/pwm_output.c \
drivers/serial_uart.c drivers/serial_uart.c
# check if target.mk supplied # check if target.mk supplied

8
lib/main/CMSIS/CM1/CoreSupport/core_cmInstr.h
View file

@ -440,7 +440,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t val
__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value) __attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value)
{ {
#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8) #if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
return (short)__builtin_bswap16(value); return (short)__builtin_bswap16((uint16_t)value);
#else #else
uint32_t result; uint32_t result;
@ -512,7 +512,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uin
*/ */
__ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" ); __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
#endif #endif
return(result); return((uint8_t)result);
} }
@ -535,7 +535,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile ui
*/ */
__ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" ); __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
#endif #endif
return(result); return((uint16_t)result);
} }
@ -664,7 +664,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value)
uint32_t result; uint32_t result;
__ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) ); __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
return(result); return((uint8_t)result);
} }
#endif /* (__CORTEX_M >= 0x03) */ #endif /* (__CORTEX_M >= 0x03) */

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

Binary file not shown.

822
reference/spektrum/spektrumTelemetrySensors.h Normal file
View file

@ -0,0 +1,822 @@
//////////////////////////////////////////////////////////////////
//
// 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

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

@ -289,9 +289,6 @@ typedef struct blackboxSlowState_s {
bool rxFlightChannelsValid; bool rxFlightChannelsValid;
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
//From mixer.c:
extern uint8_t motorCount;
//From rc_controls.c //From rc_controls.c
extern uint32_t rcModeActivationMask; extern uint32_t rcModeActivationMask;
@ -367,7 +364,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1; return getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER: case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
return mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI; return mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI;
@ -472,7 +469,6 @@ static void blackboxSetState(BlackboxState newState)
static void writeIntraframe(void) static void writeIntraframe(void)
{ {
blackboxMainState_t *blackboxCurrent = blackboxHistory[0]; blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
int x;
blackboxWrite('I'); blackboxWrite('I');
@ -483,7 +479,7 @@ static void writeIntraframe(void)
blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT);
// Don't bother writing the current D term if the corresponding PID setting is zero // Don't bother writing the current D term if the corresponding PID setting is zero
for (x = 0; x < XYZ_AXIS_COUNT; x++) { for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) { if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) {
blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]); blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]);
} }
@ -543,7 +539,8 @@ static void writeIntraframe(void)
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle); blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle);
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others //Motors tend to be similar to each other so use the first motor's value as a predictor of the others
for (x = 1; x < motorCount; x++) { const int motorCount = getMotorCount();
for (int x = 1; x < motorCount; x++) {
blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
} }
@ -667,7 +664,7 @@ static void writeInterframe(void)
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT); blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accSmooth), XYZ_AXIS_COUNT); blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accSmooth), XYZ_AXIS_COUNT);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), 4); blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), 4);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), motorCount); blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), getMotorCount());
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
@ -820,7 +817,7 @@ void startBlackbox(void)
*/ */
blackboxBuildConditionCache(); blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(masterConfig.modeActivationConditions, BOXBLACKBOX); blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationProfile()->modeActivationConditions, BOXBLACKBOX);
blackboxIteration = 0; blackboxIteration = 0;
blackboxPFrameIndex = 0; blackboxPFrameIndex = 0;
@ -1002,6 +999,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->debug[i] = debug[i]; blackboxCurrent->debug[i] = debug[i];
} }
const int motorCount = getMotorCount();
for (i = 0; i < motorCount; i++) { for (i = 0; i < motorCount; i++) {
blackboxCurrent->motor[i] = motor[i]; blackboxCurrent->motor[i] = motor[i];
} }
@ -1202,63 +1200,65 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime); BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", gyroConfig()->gyro_sync_denom); BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", gyroConfig()->gyro_sync_denom);
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", pidConfig()->pid_process_denom); BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", pidConfig()->pid_process_denom);
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); const profile_t *currentProfile = &masterConfig.profile[masterConfig.current_profile_index];
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); const controlRateConfig_t *currentControlRateProfile = &currentProfile->controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile];
BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8); BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", currentControlRateProfile->rcRate8);
BLACKBOX_PRINT_HEADER_LINE("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8); BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", currentControlRateProfile->rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("thrMid:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrMid8); BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", currentControlRateProfile->rcYawRate8);
BLACKBOX_PRINT_HEADER_LINE("thrExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrExpo8); BLACKBOX_PRINT_HEADER_LINE("rcYawExpo:%d", currentControlRateProfile->rcYawExpo8);
BLACKBOX_PRINT_HEADER_LINE("dynThrPID:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].dynThrPID); BLACKBOX_PRINT_HEADER_LINE("thrMid:%d", currentControlRateProfile->thrMid8);
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint); BLACKBOX_PRINT_HEADER_LINE("thrExpo:%d", currentControlRateProfile->thrExpo8);
BLACKBOX_PRINT_HEADER_LINE("rates:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL], BLACKBOX_PRINT_HEADER_LINE("dynThrPID:%d", currentControlRateProfile->dynThrPID);
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH], BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint:%d", currentControlRateProfile->tpa_breakpoint);
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]); BLACKBOX_PRINT_HEADER_LINE("rates:%d,%d,%d", currentControlRateProfile->rates[ROLL],
BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL], currentControlRateProfile->rates[PITCH],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL], currentControlRateProfile->rates[YAW]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]); BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", currentProfile->pidProfile.P8[ROLL],
BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PITCH], currentProfile->pidProfile.I8[ROLL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PITCH], currentProfile->pidProfile.D8[ROLL]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PITCH]); BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", currentProfile->pidProfile.P8[PITCH],
BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[YAW], currentProfile->pidProfile.I8[PITCH],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[YAW], currentProfile->pidProfile.D8[PITCH]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[YAW]); BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", currentProfile->pidProfile.P8[YAW],
BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDALT], currentProfile->pidProfile.I8[YAW],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDALT], currentProfile->pidProfile.D8[YAW]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDALT]); BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDALT],
BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOS], currentProfile->pidProfile.I8[PIDALT],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOS], currentProfile->pidProfile.D8[PIDALT]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOS]); BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDPOS],
BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOSR], currentProfile->pidProfile.I8[PIDPOS],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOSR], currentProfile->pidProfile.D8[PIDPOS]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOSR]); BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDPOSR],
BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDNAVR], currentProfile->pidProfile.I8[PIDPOSR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDNAVR], currentProfile->pidProfile.D8[PIDPOSR]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDNAVR]); BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDNAVR],
BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL], currentProfile->pidProfile.I8[PIDNAVR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL], currentProfile->pidProfile.D8[PIDNAVR]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]); BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDLEVEL],
BLACKBOX_PRINT_HEADER_LINE("magPID:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDMAG]); currentProfile->pidProfile.I8[PIDLEVEL],
BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL], currentProfile->pidProfile.D8[PIDLEVEL]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL], BLACKBOX_PRINT_HEADER_LINE("magPID:%d", currentProfile->pidProfile.P8[PIDMAG]);
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]); BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDVEL],
BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_filter_type); currentProfile->pidProfile.I8[PIDVEL],
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz); currentProfile->pidProfile.D8[PIDVEL]);
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type:%d", currentProfile->pidProfile.dterm_filter_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", currentProfile->pidProfile.dterm_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_cutoff); BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", currentProfile->pidProfile.yaw_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", currentProfile->pidProfile.dterm_notch_hz);
BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", currentProfile->pidProfile.dterm_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", currentProfile->pidProfile.rollPitchItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", currentProfile->pidProfile.yawItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", currentProfile->pidProfile.yaw_p_limit);
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidAtMinThrottle); BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", currentProfile->pidProfile.dterm_average_count);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", currentProfile->pidProfile.vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", currentProfile->pidProfile.pidAtMinThrottle);
// Betaflight PID controller parameters // Betaflight PID controller parameters
BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermThrottleGain); BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", currentProfile->pidProfile.itermThrottleGain);
BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.setpointRelaxRatio); BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d", currentProfile->pidProfile.setpointRelaxRatio);
BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight); BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", currentProfile->pidProfile.dtermSetpointWeight);
BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", currentProfile->pidProfile.yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", currentProfile->pidProfile.rateAccelLimit);
// End of Betaflight controller parameters // End of Betaflight controller parameters
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", rcControlsConfig()->deadband); BLACKBOX_PRINT_HEADER_LINE("deadband:%d", rcControlsConfig()->deadband);

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

@ -36,6 +36,8 @@
#include "cms/cms_types.h" #include "cms/cms_types.h"
#include "cms/cms_menu_blackbox.h" #include "cms/cms_menu_blackbox.h"
#include "common/utils.h"
#include "config/config_profile.h" #include "config/config_profile.h"
#include "config/config_master.h" #include "config/config_master.h"
#include "config/feature.h" #include "config/feature.h"

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

@ -31,12 +31,12 @@
#include "drivers/system.h" #include "drivers/system.h"
//#include "common/typeconversion.h"
#include "cms/cms.h" #include "cms/cms.h"
#include "cms/cms_types.h" #include "cms/cms_types.h"
#include "cms/cms_menu_imu.h" #include "cms/cms_menu_imu.h"
#include "common/utils.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"

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

@ -71,7 +71,7 @@ OSD_Entry menuOsdActiveElemsEntries[] =
{"NAME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CRAFT_NAME], 0}, {"NAME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CRAFT_NAME], 0},
{"THROTTLE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_THROTTLE_POS], 0}, {"THROTTLE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_THROTTLE_POS], 0},
#ifdef VTX #ifdef VTX
{"VTX CHAN", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_VTX_CHANNEL]}, {"VTX CHAN", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_VTX_CHANNEL], 0},
#endif // VTX #endif // VTX
{"CURRENT (A)", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CURRENT_DRAW], 0}, {"CURRENT (A)", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CURRENT_DRAW], 0},
{"USED MAH", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAH_DRAWN], 0}, {"USED MAH", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAH_DRAWN], 0},

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

@ -27,6 +27,8 @@
#include "cms/cms_types.h" #include "cms/cms_types.h"
#include "cms/cms_menu_vtx.h" #include "cms/cms_menu_vtx.h"
#include "common/utils.h"
#include "config/config_profile.h" #include "config/config_profile.h"
#include "config/config_master.h" #include "config/config_master.h"
#include "config/feature.h" #include "config/feature.h"

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

@ -70,8 +70,8 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
const typeof( ((type *)0)->member ) *__mptr = (ptr); \ const typeof( ((type *)0)->member ) *__mptr = (ptr); \
(type *)( (char *)__mptr - offsetof(type,member) );})) (type *)( (char *)__mptr - offsetof(type,member) );}))
static inline int16_t cmp16(uint16_t a, uint16_t b) { return a-b; } static inline int16_t cmp16(uint16_t a, uint16_t b) { return (int16_t)(a-b); }
static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; } static inline int32_t cmp32(uint32_t a, uint32_t b) { return (int32_t)(a-b); }
// using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions // using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions
// than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions) // than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions)

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

@ -26,7 +26,7 @@
#include "cms/cms.h" #include "cms/cms.h"
#include "drivers/adc.h" #include "drivers/adc.h"
#include "drivers/pwm_rx.h" #include "drivers/rx_pwm.h"
#include "drivers/sound_beeper.h" #include "drivers/sound_beeper.h"
#include "drivers/sonar_hcsr04.h" #include "drivers/sonar_hcsr04.h"
#include "drivers/sdcard.h" #include "drivers/sdcard.h"
@ -99,6 +99,11 @@
#define blackboxConfig(x) (&masterConfig.blackboxConfig) #define blackboxConfig(x) (&masterConfig.blackboxConfig)
#define flashConfig(x) (&masterConfig.flashConfig) #define flashConfig(x) (&masterConfig.flashConfig)
#define pidConfig(x) (&masterConfig.pidConfig) #define pidConfig(x) (&masterConfig.pidConfig)
#define adjustmentProfile(x) (&masterConfig.adjustmentProfile)
#define modeActivationProfile(x) (&masterConfig.modeActivationProfile)
#define servoProfile(x) (&masterConfig.servoProfile)
#define customMotorMixer(i) (&masterConfig.customMotorMixer[i])
#define customServoMixer(i) (&masterConfig.customServoMixer[i])
// System-wide // System-wide
@ -119,7 +124,7 @@ typedef struct master_s {
servoMixerConfig_t servoMixerConfig; servoMixerConfig_t servoMixerConfig;
servoMixer_t customServoMixer[MAX_SERVO_RULES]; servoMixer_t customServoMixer[MAX_SERVO_RULES];
// Servo-related stuff // Servo-related stuff
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration servoProfile_t servoProfile;
// gimbal-related configuration // gimbal-related configuration
gimbalConfig_t gimbalConfig; gimbalConfig_t gimbalConfig;
#endif #endif
@ -128,8 +133,6 @@ typedef struct master_s {
imuConfig_t imuConfig; imuConfig_t imuConfig;
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
pidConfig_t pidConfig; pidConfig_t pidConfig;
uint8_t debug_mode; // Processing denominator for PID controller vs gyro sampling rate uint8_t debug_mode; // Processing denominator for PID controller vs gyro sampling rate
@ -215,9 +218,8 @@ typedef struct master_s {
profile_t profile[MAX_PROFILE_COUNT]; profile_t profile[MAX_PROFILE_COUNT];
uint8_t current_profile_index; uint8_t current_profile_index;
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; modeActivationProfile_t modeActivationProfile;
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; adjustmentProfile_t adjustmentProfile;
#ifdef VTX #ifdef VTX
uint8_t vtx_band; //1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Raceband uint8_t vtx_band; //1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Raceband
uint8_t vtx_channel; //1-8 uint8_t vtx_channel; //1-8

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

@ -71,6 +71,16 @@ void pgResetCurrent(const pgRegistry_t *reg)
} }
} }
bool pgResetCopy(void *copy, pgn_t pgn)
{
const pgRegistry_t *reg = pgFind(pgn);
if (reg) {
pgResetInstance(reg, copy);
return true;
}
return false;
}
void pgLoad(const pgRegistry_t* reg, int profileIndex, const void *from, int size, int version) void pgLoad(const pgRegistry_t* reg, int profileIndex, const void *from, int size, int version)
{ {
pgResetInstance(reg, pgOffset(reg, profileIndex)); pgResetInstance(reg, pgOffset(reg, profileIndex));

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

@ -17,6 +17,9 @@
#pragma once #pragma once
#include <stdint.h>
#include <stdbool.h>
typedef uint16_t pgn_t; typedef uint16_t pgn_t;
// parameter group registry flags // parameter group registry flags
@ -225,5 +228,6 @@ void pgLoad(const pgRegistry_t* reg, int profileIndex, const void *from, int siz
int pgStore(const pgRegistry_t* reg, void *to, int size, uint8_t profileIndex); int pgStore(const pgRegistry_t* reg, void *to, int size, uint8_t profileIndex);
void pgResetAll(int profileCount); void pgResetAll(int profileCount);
void pgResetCurrent(const pgRegistry_t *reg); void pgResetCurrent(const pgRegistry_t *reg);
bool pgResetCopy(void *copy, pgn_t pgn);
void pgReset(const pgRegistry_t* reg, int profileIndex); void pgReset(const pgRegistry_t* reg, int profileIndex);
void pgActivateProfile(int profileIndex); void pgActivateProfile(int profileIndex);

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

@ -20,6 +20,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "drivers/exti.h" #include "drivers/exti.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro_mpu.h"
#ifndef MPU_I2C_INSTANCE #ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE #define MPU_I2C_INSTANCE I2C_DEVICE
@ -45,6 +46,9 @@ typedef struct gyroDev_s {
uint16_t lpf; uint16_t lpf;
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT];
sensor_align_e gyroAlign; sensor_align_e gyroAlign;
const extiConfig_t *mpuIntExtiConfig;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
} gyroDev_t; } gyroDev_t;
typedef struct accDev_s { typedef struct accDev_s {
@ -54,4 +58,6 @@ typedef struct accDev_s {
int16_t ADCRaw[XYZ_AXIS_COUNT]; int16_t ADCRaw[XYZ_AXIS_COUNT];
char revisionCode; // a revision code for the sensor, if known char revisionCode; // a revision code for the sensor, if known
sensor_align_e accAlign; sensor_align_e accAlign;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
} accDev_t; } accDev_t;

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

@ -25,7 +25,6 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h" #include "common/utils.h"
@ -49,24 +48,21 @@
//#define DEBUG_MPU_DATA_READY_INTERRUPT //#define DEBUG_MPU_DATA_READY_INTERRUPT
mpuResetFuncPtr mpuReset;
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data); static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data); static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
static void mpu6050FindRevision(void); static void mpu6050FindRevision(gyroDev_t *gyro);
#ifdef USE_SPI #ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void); static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro);
#endif #endif
#ifndef MPU_I2C_INSTANCE #ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE #define MPU_I2C_INSTANCE I2C_DEVICE
#endif #endif
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
static const extiConfig_t *mpuIntExtiConfig = NULL;
#define MPU_ADDRESS 0x68 #define MPU_ADDRESS 0x68
// WHO_AM_I register contents for MPU3050, 6050 and 6500 // WHO_AM_I register contents for MPU3050, 6050 and 6500
@ -75,13 +71,8 @@ static const extiConfig_t *mpuIntExtiConfig = NULL;
#define MPU_INQUIRY_MASK 0x7E #define MPU_INQUIRY_MASK 0x7E
mpuDetectionResult_t *mpuDetect(const extiConfig_t *configToUse) mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
{ {
memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult));
memset(&mpuConfiguration, 0, sizeof(mpuConfiguration));
mpuIntExtiConfig = configToUse;
bool ack; bool ack;
uint8_t sig; uint8_t sig;
uint8_t inquiryResult; uint8_t inquiryResult;
@ -96,93 +87,94 @@ mpuDetectionResult_t *mpuDetect(const extiConfig_t *configToUse)
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
#endif #endif
if (ack) { if (ack) {
mpuConfiguration.read = mpuReadRegisterI2C; gyro->mpuConfiguration.read = mpuReadRegisterI2C;
mpuConfiguration.write = mpuWriteRegisterI2C; gyro->mpuConfiguration.write = mpuWriteRegisterI2C;
} else { } else {
#ifdef USE_SPI #ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(); bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
UNUSED(detectedSpiSensor); UNUSED(detectedSpiSensor);
#endif #endif
return &mpuDetectionResult; return &gyro->mpuDetectionResult;
} }
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
// If an MPU3050 is connected sig will contain 0. // If an MPU3050 is connected sig will contain 0.
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
inquiryResult &= MPU_INQUIRY_MASK; inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_3050; gyro->mpuDetectionResult.sensor = MPU_3050;
mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &mpuDetectionResult; return &gyro->mpuDetectionResult;
} }
sig &= MPU_INQUIRY_MASK; sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) { if (sig == MPUx0x0_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_60x0; gyro->mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision(); mpu6050FindRevision(gyro);
} else if (sig == MPU6500_WHO_AM_I_CONST) { } else if (sig == MPU6500_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_65xx_I2C; gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
} }
return &mpuDetectionResult; return &gyro->mpuDetectionResult;
} }
#ifdef USE_SPI #ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void) static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{ {
#ifdef USE_GYRO_SPI_MPU6500 #ifdef USE_GYRO_SPI_MPU6500
if (mpu6500SpiDetect()) { if (mpu6500SpiDetect()) {
mpuDetectionResult.sensor = MPU_65xx_SPI; gyro->mpuDetectionResult.sensor = MPU_65xx_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu6500ReadRegister; gyro->mpuConfiguration.read = mpu6500ReadRegister;
mpuConfiguration.write = mpu6500WriteRegister; gyro->mpuConfiguration.write = mpu6500WriteRegister;
return true; return true;
} }
#endif #endif
#ifdef USE_GYRO_SPI_ICM20689 #ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiDetect()) { if (icm20689SpiDetect()) {
mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = icm20689ReadRegister; gyro->mpuConfiguration.read = icm20689ReadRegister;
mpuConfiguration.write = icm20689WriteRegister; gyro->mpuConfiguration.write = icm20689WriteRegister;
return true; return true;
} }
#endif #endif
#ifdef USE_GYRO_SPI_MPU6000 #ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) { if (mpu6000SpiDetect()) {
mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu6000ReadRegister; gyro->mpuConfiguration.read = mpu6000ReadRegister;
mpuConfiguration.write = mpu6000WriteRegister; gyro->mpuConfiguration.write = mpu6000WriteRegister;
return true; return true;
} }
#endif #endif
#ifdef USE_GYRO_SPI_MPU9250 #ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiDetect()) { if (mpu9250SpiDetect()) {
mpuDetectionResult.sensor = MPU_9250_SPI; gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu9250ReadRegister; gyro->mpuConfiguration.read = mpu9250ReadRegister;
mpuConfiguration.slowread = mpu9250SlowReadRegister; gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister;
mpuConfiguration.verifywrite = verifympu9250WriteRegister; gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister;
mpuConfiguration.write = mpu9250WriteRegister; gyro->mpuConfiguration.write = mpu9250WriteRegister;
mpuConfiguration.reset = mpu9250ResetGyro; gyro->mpuConfiguration.reset = mpu9250ResetGyro;
return true; return true;
} }
#endif #endif
UNUSED(gyro);
return false; return false;
} }
#endif #endif
static void mpu6050FindRevision(void) static void mpu6050FindRevision(gyroDev_t *gyro)
{ {
bool ack; bool ack;
UNUSED(ack); UNUSED(ack);
@ -195,28 +187,28 @@ static void mpu6050FindRevision(void)
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision // determine product ID and accel revision
ack = mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer); ack = gyro->mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer);
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) { if (revision) {
/* Congrats, these parts are better. */ /* Congrats, these parts are better. */
if (revision == 1) { if (revision == 1) {
mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else if (revision == 2) { } else if (revision == 2) {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} else if ((revision == 3) || (revision == 7)) { } else if ((revision == 3) || (revision == 7)) {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} else { } else {
failureMode(FAILURE_ACC_INCOMPATIBLE); failureMode(FAILURE_ACC_INCOMPATIBLE);
} }
} else { } else {
ack = mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId); ack = gyro->mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F; revision = productId & 0x0F;
if (!revision) { if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE); failureMode(FAILURE_ACC_INCOMPATIBLE);
} else if (revision == 4) { } else if (revision == 4) {
mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else { } else {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} }
} }
} }
@ -243,13 +235,11 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb)
static void mpuIntExtiInit(gyroDev_t *gyro) static void mpuIntExtiInit(gyroDev_t *gyro)
{ {
#if defined(MPU_INT_EXTI) #if defined(MPU_INT_EXTI)
static bool mpuExtiInitDone = false; if (!gyro->mpuIntExtiConfig) {
if (mpuExtiInitDone || !mpuIntExtiConfig) {
return; return;
} }
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag); IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiConfig->tag);
#ifdef ENSURE_MPU_DATA_READY_IS_LOW #ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = IORead(mpuIntIO); uint8_t status = IORead(mpuIntIO);
@ -272,7 +262,6 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
EXTIEnable(mpuIntIO, true); EXTIEnable(mpuIntIO, true);
#endif #endif
mpuExtiInitDone = true;
#else #else
UNUSED(gyro); UNUSED(gyro);
#endif #endif
@ -294,7 +283,7 @@ bool mpuAccRead(accDev_t *acc)
{ {
uint8_t data[6]; uint8_t data[6];
bool ack = mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data); bool ack = acc->mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) { if (!ack) {
return false; return false;
} }
@ -310,7 +299,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{ {
uint8_t data[6]; uint8_t data[6];
const bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data); const bool ack = gyro->mpuConfiguration.read(gyro->mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) { if (!ack) {
return false; return false;
} }

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

@ -121,6 +121,8 @@ typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data); typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFuncPtr)(void); typedef void(*mpuResetFuncPtr)(void);
extern mpuResetFuncPtr mpuReset;
typedef struct mpuConfiguration_s { typedef struct mpuConfiguration_s {
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
mpuReadRegisterFunc read; mpuReadRegisterFunc read;
@ -130,8 +132,6 @@ typedef struct mpuConfiguration_s {
mpuResetFuncPtr reset; mpuResetFuncPtr reset;
} mpuConfiguration_t; } mpuConfiguration_t;
extern mpuConfiguration_t mpuConfiguration;
enum gyro_fsr_e { enum gyro_fsr_e {
INV_FSR_250DPS = 0, INV_FSR_250DPS = 0,
INV_FSR_500DPS, INV_FSR_500DPS,
@ -181,13 +181,10 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution; mpu6050Resolution_e resolution;
} mpuDetectionResult_t; } mpuDetectionResult_t;
extern mpuDetectionResult_t mpuDetectionResult;
void mpuConfigureDataReadyInterruptHandling(void);
struct gyroDev_s; struct gyroDev_s;
void mpuGyroInit(struct gyroDev_s *gyro); void mpuGyroInit(struct gyroDev_s *gyro);
struct accDev_s; struct accDev_s;
bool mpuAccRead(struct accDev_s *acc); bool mpuAccRead(struct accDev_s *acc);
bool mpuGyroRead(struct gyroDev_s *gyro); bool mpuGyroRead(struct gyroDev_s *gyro);
mpuDetectionResult_t *mpuDetect(const extiConfig_t *configToUse); mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro);
bool mpuCheckDataReady(struct gyroDev_s *gyro); bool mpuCheckDataReady(struct gyroDev_s *gyro);

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

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

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

@ -52,7 +52,7 @@
static void mpu6050AccInit(accDev_t *acc) static void mpu6050AccInit(accDev_t *acc)
{ {
switch (mpuDetectionResult.resolution) { switch (acc->mpuDetectionResult.resolution) {
case MPU_HALF_RESOLUTION: case MPU_HALF_RESOLUTION:
acc->acc_1G = 256 * 4; acc->acc_1G = 256 * 4;
break; break;
@ -64,13 +64,13 @@ static void mpu6050AccInit(accDev_t *acc)
bool mpu6050AccDetect(accDev_t *acc) bool mpu6050AccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_60x0) { if (acc->mpuDetectionResult.sensor != MPU_60x0) {
return false; return false;
} }
acc->init = mpu6050AccInit; acc->init = mpu6050AccInit;
acc->read = mpuAccRead; acc->read = mpuAccRead;
acc->revisionCode = (mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES. acc->revisionCode = (acc->mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
return true; return true;
} }
@ -79,29 +79,29 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100); delay(100);
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_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //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 delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
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_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff. // ACC Init stuff.
// Accel scale 8g (4096 LSB/g) // Accel scale 8g (4096 LSB/g)
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, gyro->mpuConfiguration.write(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 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 #ifdef USE_MPU_DATA_READY_SIGNAL
mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif #endif
} }
bool mpu6050GyroDetect(gyroDev_t *gyro) bool mpu6050GyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_60x0) { if (gyro->mpuDetectionResult.sensor != MPU_60x0) {
return false; return false;
} }
gyro->init = mpu6050GyroInit; gyro->init = mpu6050GyroInit;

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

@ -41,7 +41,7 @@ void mpu6500AccInit(accDev_t *acc)
bool mpu6500AccDetect(accDev_t *acc) bool mpu6500AccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_I2C) { if (acc->mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false; return false;
} }
@ -55,40 +55,40 @@ void mpu6500GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100); delay(100);
mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100); delay(100);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
delay(100); delay(100);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15); delay(15);
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delay(15); delay(15);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15); delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15); delay(15);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
delay(100); delay(100);
// Data ready interrupt configuration // Data ready interrupt configuration
#ifdef USE_MPU9250_MAG #ifdef USE_MPU9250_MAG
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
#else #else
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
#endif #endif
delay(15); delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL #ifdef USE_MPU_DATA_READY_SIGNAL
mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
#endif #endif
delay(15); delay(15);
} }
bool mpu6500GyroDetect(gyroDev_t *gyro) bool mpu6500GyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_I2C) { if (gyro->mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false; return false;
} }

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

@ -114,7 +114,7 @@ void icm20689AccInit(accDev_t *acc)
bool icm20689SpiAccDetect(accDev_t *acc) bool icm20689SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != ICM_20689_SPI) { if (acc->mpuDetectionResult.sensor != ICM_20689_SPI) {
return false; return false;
} }
@ -130,31 +130,31 @@ void icm20689GyroInit(gyroDev_t *gyro)
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100); delay(100);
mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03); gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100); delay(100);
// mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); // gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
// delay(100); // delay(100);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15); delay(15);
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delay(15); delay(15);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15); delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15); delay(15);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
delay(100); delay(100);
// Data ready interrupt configuration // Data ready interrupt configuration
// 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, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15); delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL #ifdef USE_MPU_DATA_READY_SIGNAL
mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif #endif
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
@ -162,7 +162,7 @@ void icm20689GyroInit(gyroDev_t *gyro)
bool icm20689SpiGyroDetect(gyroDev_t *gyro) bool icm20689SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != ICM_20689_SPI) { if (gyro->mpuDetectionResult.sensor != ICM_20689_SPI) {
return false; return false;
} }

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

@ -256,7 +256,7 @@ static void mpu6000AccAndGyroInit(void)
bool mpu6000SpiAccDetect(accDev_t *acc) bool mpu6000SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) { if (acc->mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false; return false;
} }
@ -268,7 +268,7 @@ bool mpu6000SpiAccDetect(accDev_t *acc)
bool mpu6000SpiGyroDetect(gyroDev_t *gyro) bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) { if (gyro->mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false; return false;
} }

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

@ -116,7 +116,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
bool mpu6500SpiAccDetect(accDev_t *acc) bool mpu6500SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_SPI) { if (acc->mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false; return false;
} }
@ -128,7 +128,7 @@ bool mpu6500SpiAccDetect(accDev_t *acc)
bool mpu6500SpiGyroDetect(gyroDev_t *gyro) bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_SPI) { if (gyro->mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false; return false;
} }

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

@ -211,7 +211,7 @@ bool mpu9250SpiDetect(void)
bool mpu9250SpiAccDetect(accDev_t *acc) bool mpu9250SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_9250_SPI) { if (acc->mpuDetectionResult.sensor != MPU_9250_SPI) {
return false; return false;
} }
@ -223,7 +223,7 @@ bool mpu9250SpiAccDetect(accDev_t *acc)
bool mpu9250SpiGyroDetect(gyroDev_t *gyro) bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_9250_SPI) { if (gyro->mpuDetectionResult.sensor != MPU_9250_SPI) {
return false; return false;
} }

9
src/main/drivers/bus_i2c_hal.c
View file

@ -231,7 +231,14 @@ void i2cInit(I2CDevice device)
i2cHandle[device].Handle.Instance = i2cHardwareMap[device].dev; i2cHandle[device].Handle.Instance = i2cHardwareMap[device].dev;
/// TODO: HAL check if I2C timing is correct /// TODO: HAL check if I2C timing is correct
i2cHandle[device].Handle.Init.Timing = 0x00B01B59;
if (i2c->overClock) {
// 800khz Maximum speed tested on various boards without issues
i2cHandle[device].Handle.Init.Timing = 0x00500D1D;
} else {
//i2cHandle[device].Handle.Init.Timing = 0x00500B6A;
i2cHandle[device].Handle.Init.Timing = 0x00500C6F;
}
//i2cHandle[device].Handle.Init.Timing = 0x00D00E28; /* (Rise time = 120ns, Fall time = 25ns) */ //i2cHandle[device].Handle.Init.Timing = 0x00D00E28; /* (Rise time = 120ns, Fall time = 25ns) */
i2cHandle[device].Handle.Init.OwnAddress1 = 0x0; i2cHandle[device].Handle.Init.OwnAddress1 = 0x0;
i2cHandle[device].Handle.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; i2cHandle[device].Handle.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;

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

@ -126,10 +126,8 @@ void SPI4_IRQHandler(void)
void spiInitDevice(SPIDevice device) void spiInitDevice(SPIDevice device)
{ {
static SPI_InitTypeDef spiInit;
spiDevice_t *spi = &(spiHardwareMap[device]); spiDevice_t *spi = &(spiHardwareMap[device]);
#ifdef SDCARD_SPI_INSTANCE #ifdef SDCARD_SPI_INSTANCE
if (spi->dev == SDCARD_SPI_INSTANCE) { if (spi->dev == SDCARD_SPI_INSTANCE) {
spi->leadingEdge = true; spi->leadingEdge = true;
@ -155,6 +153,7 @@ void spiInitDevice(SPIDevice device)
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
if (spi->nss) { if (spi->nss) {
IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af);
} }
#endif #endif
@ -164,6 +163,7 @@ void spiInitDevice(SPIDevice device)
IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG); IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG);
if (spi->nss) { if (spi->nss) {
IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG);
} }
#endif #endif
@ -171,27 +171,25 @@ void spiInitDevice(SPIDevice device)
// Init SPI hardware // Init SPI hardware
HAL_SPI_DeInit(&spiHardwareMap[device].hspi); HAL_SPI_DeInit(&spiHardwareMap[device].hspi);
spiInit.Mode = SPI_MODE_MASTER; spiHardwareMap[device].hspi.Init.Mode = SPI_MODE_MASTER;
spiInit.Direction = SPI_DIRECTION_2LINES; spiHardwareMap[device].hspi.Init.Direction = SPI_DIRECTION_2LINES;
spiInit.DataSize = SPI_DATASIZE_8BIT; spiHardwareMap[device].hspi.Init.DataSize = SPI_DATASIZE_8BIT;
spiInit.NSS = SPI_NSS_SOFT; spiHardwareMap[device].hspi.Init.NSS = SPI_NSS_SOFT;
spiInit.FirstBit = SPI_FIRSTBIT_MSB; spiHardwareMap[device].hspi.Init.FirstBit = SPI_FIRSTBIT_MSB;
spiInit.CRCPolynomial = 7; spiHardwareMap[device].hspi.Init.CRCPolynomial = 7;
spiInit.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
spiInit.CRCCalculation = SPI_CRCCALCULATION_DISABLE; spiHardwareMap[device].hspi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
spiInit.TIMode = SPI_TIMODE_DISABLED; spiHardwareMap[device].hspi.Init.TIMode = SPI_TIMODE_DISABLED;
if (spi->leadingEdge) { if (spi->leadingEdge) {
spiInit.CLKPolarity = SPI_POLARITY_LOW; spiHardwareMap[device].hspi.Init.CLKPolarity = SPI_POLARITY_LOW;
spiInit.CLKPhase = SPI_PHASE_1EDGE; spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_1EDGE;
} }
else { else {
spiInit.CLKPolarity = SPI_POLARITY_HIGH; spiHardwareMap[device].hspi.Init.CLKPolarity = SPI_POLARITY_HIGH;
spiInit.CLKPhase = SPI_PHASE_2EDGE; spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_2EDGE;
} }
spiHardwareMap[device].hspi.Init = spiInit;
if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK) if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK)
{ {
if (spi->nss) { if (spi->nss) {
@ -392,6 +390,7 @@ DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channe
// DMA TX Interrupt // DMA TX Interrupt
dmaSetHandler(spiHardwareMap[device].dmaIrqHandler, dmaSPIIRQHandler, NVIC_BUILD_PRIORITY(3, 0), (uint32_t)device); dmaSetHandler(spiHardwareMap[device].dmaIrqHandler, dmaSPIIRQHandler, NVIC_BUILD_PRIORITY(3, 0), (uint32_t)device);
HAL_CLEANCACHE(pData,Size);
// And Transmit // And Transmit
HAL_SPI_Transmit_DMA(&spiHardwareMap[device].hspi, pData, Size); HAL_SPI_Transmit_DMA(&spiHardwareMap[device].hspi, pData, Size);

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

@ -26,7 +26,7 @@
#include "display_ug2864hsweg01.h" #include "display_ug2864hsweg01.h"
#ifndef OLED_I2C_INSTANCE #ifndef OLED_I2C_INSTANCE
#define OLED_I2C_INSTANCE I2CDEV_1 #define OLED_I2C_INSTANCE I2C_DEVICE
#endif #endif
#define INVERSE_CHAR_FORMAT 0x7f // 0b01111111 #define INVERSE_CHAR_FORMAT 0x7f // 0b01111111

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

@ -40,6 +40,9 @@ typedef struct dmaChannelDescriptor_s {
#if defined(STM32F4) || defined(STM32F7) #if defined(STM32F4) || defined(STM32F7)
#define HAL_CLEANINVALIDATECACHE(addr, size) (SCB_CleanInvalidateDCache_by_Addr((uint32_t*)((uint32_t)addr & ~0x1f), ((uint32_t)(addr + size + 0x1f) & ~0x1f) - ((uint32_t)addr & ~0x1f)))
#define HAL_CLEANCACHE(addr, size) (SCB_CleanDCache_by_Addr((uint32_t*)((uint32_t)addr & ~0x1f), ((uint32_t)(addr + size + 0x1f) & ~0x1f) - ((uint32_t)addr & ~0x1f)))
uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream); uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream);
typedef enum { typedef enum {

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

@ -34,7 +34,8 @@
extern uint16_t maxScreenSize; extern uint16_t maxScreenSize;
void max7456Init(const vcdProfile_t *vcdProfile); struct vcdProfile_s;
void max7456Init(const struct vcdProfile_s *vcdProfile);
void max7456DrawScreen(void); void max7456DrawScreen(void);
void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data); void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data);
uint8_t max7456GetRowsCount(void); uint8_t max7456GetRowsCount(void);

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

@ -28,6 +28,7 @@
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) #define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
static pwmWriteFuncPtr pwmWritePtr;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL; static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
@ -35,10 +36,31 @@ static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
#endif #endif
bool pwmMotorsEnabled = true; bool pwmMotorsEnabled = false;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{ {
#if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
if(Handle == NULL) return;
TIM_OC_InitTypeDef TIM_OCInitStructure;
TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
if (output & TIMER_OUTPUT_N_CHANNEL) {
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
} else {
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
}
TIM_OCInitStructure.Pulse = value;
TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
HAL_TIM_PWM_ConfigChannel(Handle, &TIM_OCInitStructure, channel);
#else
TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCStructInit(&TIM_OCInitStructure);
@ -57,15 +79,26 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
timerOCInit(tim, channel, &TIM_OCInitStructure); timerOCInit(tim, channel, &TIM_OCInitStructure);
timerOCPreloadConfig(tim, channel, TIM_OCPreload_Enable); timerOCPreloadConfig(tim, channel, TIM_OCPreload_Enable);
#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)
{ {
#if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
if(Handle == NULL) return;
#endif
configTimeBase(timerHardware->tim, period, mhz); configTimeBase(timerHardware->tim, period, mhz);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output); pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
#if defined(USE_HAL_DRIVER)
HAL_TIM_PWM_Start(Handle, timerHardware->channel);
HAL_TIM_Base_Start(Handle);
#else
TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE); TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
TIM_Cmd(timerHardware->tim, ENABLE); TIM_Cmd(timerHardware->tim, ENABLE);
#endif
port->ccr = timerChCCR(timerHardware); port->ccr = timerChCCR(timerHardware);
port->period = period; port->period = period;
@ -101,9 +134,7 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value)
void pwmWriteMotor(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value)
{ {
if (index < MAX_SUPPORTED_MOTORS && pwmMotorsEnabled && motors[index].pwmWritePtr) { pwmWritePtr(index, value);
motors[index].pwmWritePtr(index, value);
}
} }
void pwmShutdownPulsesForAllMotors(uint8_t motorCount) void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
@ -127,6 +158,11 @@ void pwmEnableMotors(void)
pwmMotorsEnabled = true; pwmMotorsEnabled = true;
} }
bool pwmAreMotorsEnabled(void)
{
return pwmMotorsEnabled;
}
static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{ {
for (int index = 0; index < motorCount; index++) { for (int index = 0; index < motorCount; index++) {
@ -157,7 +193,6 @@ void pwmCompleteMotorUpdate(uint8_t motorCount)
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{ {
uint32_t timerMhzCounter = 0; uint32_t timerMhzCounter = 0;
pwmWriteFuncPtr pwmWritePtr;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
bool isDigital = false; bool isDigital = false;
@ -191,6 +226,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150: case PWM_TYPE_DSHOT150:
pwmWritePtr = pwmWriteDigital;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
isDigital = true; isDigital = true;
break; break;
@ -220,16 +256,18 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
#ifdef USE_DSHOT #ifdef USE_DSHOT
if (isDigital) { if (isDigital) {
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol); pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
motors[motorIndex].pwmWritePtr = pwmWriteDigital;
motors[motorIndex].enabled = true; motors[motorIndex].enabled = true;
continue; continue;
} }
#endif #endif
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
#if defined(USE_HAL_DRIVER)
IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
#else
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP); IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
#endif
motors[motorIndex].pwmWritePtr = pwmWritePtr;
if (useUnsyncedPwm) { if (useUnsyncedPwm) {
const uint32_t hz = timerMhzCounter * 1000000; const uint32_t hz = timerMhzCounter * 1000000;
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse); pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse);
@ -238,6 +276,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
} }
motors[motorIndex].enabled = true; motors[motorIndex].enabled = true;
} }
pwmMotorsEnabled = true;
} }
pwmOutputPort_t *pwmGetMotors(void) pwmOutputPort_t *pwmGetMotors(void)
@ -265,9 +304,13 @@ void servoInit(const servoConfig_t *servoConfig)
servos[servoIndex].io = IOGetByTag(tag); servos[servoIndex].io = IOGetByTag(tag);
IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex)); IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY); const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);
#if defined(USE_HAL_DRIVER)
IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
#else
IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
#endif
if (timer == NULL) { if (timer == NULL) {
/* flag failure and disable ability to arm */ /* flag failure and disable ability to arm */

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

@ -88,7 +88,6 @@ typedef struct {
volatile timCCR_t *ccr; volatile timCCR_t *ccr;
TIM_TypeDef *tim; TIM_TypeDef *tim;
uint16_t period; uint16_t period;
pwmWriteFuncPtr pwmWritePtr;
bool enabled; bool enabled;
IO_t io; IO_t io;
} pwmOutputPort_t; } pwmOutputPort_t;
@ -114,3 +113,4 @@ pwmOutputPort_t *pwmGetMotors(void);
bool pwmIsSynced(void); bool pwmIsSynced(void);
void pwmDisableMotors(void); void pwmDisableMotors(void);
void pwmEnableMotors(void); void pwmEnableMotors(void);
bool pwmAreMotorsEnabled(void);

292
src/main/drivers/pwm_output_hal.c
View file

@ -1,292 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#include "io.h"
#include "timer.h"
#include "pwm_output.h"
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
#ifdef USE_SERVOS
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
#endif
bool pwmMotorsEnabled = true;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
if(Handle == NULL) return;
TIM_OC_InitTypeDef TIM_OCInitStructure;
TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM2;
TIM_OCInitStructure.Pulse = value;
TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW;
TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH;
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
HAL_TIM_PWM_ConfigChannel(Handle, &TIM_OCInitStructure, channel);
//HAL_TIM_PWM_Start(Handle, channel);
}
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
{
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
if(Handle == NULL) return;
configTimeBase(timerHardware->tim, period, mhz);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
HAL_TIM_PWM_Start(Handle, timerHardware->channel);
HAL_TIM_Base_Start(Handle);
switch (timerHardware->channel) {
case TIM_CHANNEL_1:
port->ccr = &timerHardware->tim->CCR1;
break;
case TIM_CHANNEL_2:
port->ccr = &timerHardware->tim->CCR2;
break;
case TIM_CHANNEL_3:
port->ccr = &timerHardware->tim->CCR3;
break;
case TIM_CHANNEL_4:
port->ccr = &timerHardware->tim->CCR4;
break;
}
port->period = period;
port->tim = timerHardware->tim;
*port->ccr = 0;
}
static void pwmWriteBrushed(uint8_t index, uint16_t value)
{
*motors[index].ccr = (value - 1000) * motors[index].period / 1000;
}
static void pwmWriteStandard(uint8_t index, uint16_t value)
{
*motors[index].ccr = value;
}
static void pwmWriteOneShot125(uint8_t index, uint16_t value)
{
*motors[index].ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f));
}
static void pwmWriteOneShot42(uint8_t index, uint16_t value)
{
*motors[index].ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f));
}
static void pwmWriteMultiShot(uint8_t index, uint16_t value)
{
*motors[index].ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
}
void pwmWriteMotor(uint8_t index, uint16_t value)
{
if (index < MAX_SUPPORTED_MOTORS && pwmMotorsEnabled && motors[index].pwmWritePtr) {
motors[index].pwmWritePtr(index, value);
}
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
// Set the compare register to 0, which stops the output pulsing if the timer overflows
*motors[index].ccr = 0;
}
}
void pwmDisableMotors(void)
{
pwmMotorsEnabled = false;
}
void pwmEnableMotors(void)
{
pwmMotorsEnabled = true;
}
static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
bool overflowed = false;
// If we have not already overflowed this timer
for (int j = 0; j < index; j++) {
if (motors[j].tim == motors[index].tim) {
overflowed = true;
break;
}
}
if (!overflowed) {
timerForceOverflow(motors[index].tim);
}
// Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
// This compare register will be set to the output value on the next main loop.
*motors[index].ccr = 0;
}
}
void pwmCompleteMotorUpdate(uint8_t motorCount)
{
if (pwmCompleteWritePtr) {
pwmCompleteWritePtr(motorCount);
}
}
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{
uint32_t timerMhzCounter;
pwmWriteFuncPtr pwmWritePtr;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
bool isDigital = false;
switch (motorConfig->motorPwmProtocol) {
default:
case PWM_TYPE_ONESHOT125:
timerMhzCounter = ONESHOT125_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot125;
break;
case PWM_TYPE_ONESHOT42:
timerMhzCounter = ONESHOT42_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot42;
break;
case PWM_TYPE_MULTISHOT:
timerMhzCounter = MULTISHOT_TIMER_MHZ;
pwmWritePtr = pwmWriteMultiShot;
break;
case PWM_TYPE_BRUSHED:
timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
pwmWritePtr = pwmWriteBrushed;
useUnsyncedPwm = true;
idlePulse = 0;
break;
case PWM_TYPE_STANDARD:
timerMhzCounter = PWM_TIMER_MHZ;
pwmWritePtr = pwmWriteStandard;
useUnsyncedPwm = true;
idlePulse = 0;
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
isDigital = true;
break;
#endif
}
if (!useUnsyncedPwm && !isDigital) {
pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate;
}
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const ioTag_t tag = motorConfig->ioTags[motorIndex];
if (!tag) {
break;
}
const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY);
if (timerHardware == NULL) {
/* flag failure and disable ability to arm */
break;
}
#ifdef USE_DSHOT
if (isDigital) {
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
motors[motorIndex].pwmWritePtr = pwmWriteDigital;
motors[motorIndex].enabled = true;
continue;
}
#endif
motors[motorIndex].io = IOGetByTag(tag);
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
//IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
motors[motorIndex].pwmWritePtr = pwmWritePtr;
if (useUnsyncedPwm) {
const uint32_t hz = timerMhzCounter * 1000000;
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmProtocol, idlePulse);
} else {
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0);
}
motors[motorIndex].enabled = true;
}
}
pwmOutputPort_t *pwmGetMotors(void)
{
return motors;
}
#ifdef USE_SERVOS
void pwmWriteServo(uint8_t index, uint16_t value)
{
if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) {
*servos[index].ccr = value;
}
}
void servoInit(const servoConfig_t *servoConfig)
{
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
const ioTag_t tag = servoConfig->ioTags[servoIndex];
if (!tag) {
break;
}
servos[servoIndex].io = IOGetByTag(tag);
IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
//IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);
IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
if (timer == NULL) {
/* flag failure and disable ability to arm */
break;
}
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
servos[servoIndex].enabled = true;
}
}
#endif

3
src/main/drivers/resource.c
View file

@ -58,6 +58,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"RX_BIND", "RX_BIND",
"INVERTER", "INVERTER",
"LED_STRIP", "LED_STRIP",
"TRANSPONDER" "TRANSPONDER",
"VTX",
}; };

1
src/main/drivers/resource.h
View file

@ -59,6 +59,7 @@ typedef enum {
OWNER_INVERTER, OWNER_INVERTER,
OWNER_LED_STRIP, OWNER_LED_STRIP,
OWNER_TRANSPONDER, OWNER_TRANSPONDER,
OWNER_VTX,
OWNER_TOTAL_COUNT OWNER_TOTAL_COUNT
} resourceOwner_e; } resourceOwner_e;

27
src/main/drivers/rx_nrf24l01.c
View file

@ -28,13 +28,15 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "system.h" #include "bus_spi.h"
#include "io.h" #include "io.h"
#include "io_impl.h"
#include "rx_spi.h" #include "rx_spi.h"
#include "rx_nrf24l01.h" #include "rx_nrf24l01.h"
#include "system.h"
#define NRF24_CE_HI() {IOHi(IOGetByTag(IO_TAG(RX_CE_PIN)));} #define NRF24_CE_HI() {IOHi(DEFIO_IO(RX_CE_PIN));}
#define NRF24_CE_LO() {IOLo(IOGetByTag(IO_TAG(RX_CE_PIN)));} #define NRF24_CE_LO() {IOLo(DEFIO_IO(RX_CE_PIN));}
// Instruction Mnemonics // Instruction Mnemonics
// nRF24L01: Table 16. Command set for the nRF24L01 SPI. Product Specification, p46 // nRF24L01: Table 16. Command set for the nRF24L01 SPI. Product Specification, p46
@ -52,6 +54,15 @@
#define REUSE_TX_PL 0xE3 #define REUSE_TX_PL 0xE3
#define NOP 0xFF #define NOP 0xFF
static void NRF24L01_InitGpio(void)
{
// CE as OUTPUT
const SPIDevice rxSPIDevice = spiDeviceByInstance(RX_SPI_INSTANCE);
IOInit(DEFIO_IO(RX_CE_PIN), OWNER_RX_SPI_CS, rxSPIDevice + 1);
IOConfigGPIO(DEFIO_IO(RX_CE_PIN), SPI_IO_CS_CFG);
NRF24_CE_LO();
}
uint8_t NRF24L01_WriteReg(uint8_t reg, uint8_t data) uint8_t NRF24L01_WriteReg(uint8_t reg, uint8_t data)
{ {
return rxSpiWriteCommand(W_REGISTER | (REGISTER_MASK & reg), data); return rxSpiWriteCommand(W_REGISTER | (REGISTER_MASK & reg), data);
@ -98,7 +109,7 @@ uint8_t NRF24L01_ReadPayload(uint8_t *data, uint8_t length)
/* /*
* Empty the transmit FIFO buffer. * Empty the transmit FIFO buffer.
*/ */
void NRF24L01_FlushTx() void NRF24L01_FlushTx(void)
{ {
rxSpiWriteByte(FLUSH_TX); rxSpiWriteByte(FLUSH_TX);
} }
@ -106,7 +117,7 @@ void NRF24L01_FlushTx()
/* /*
* Empty the receive FIFO buffer. * Empty the receive FIFO buffer.
*/ */
void NRF24L01_FlushRx() void NRF24L01_FlushRx(void)
{ {
rxSpiWriteByte(FLUSH_RX); rxSpiWriteByte(FLUSH_RX);
} }
@ -122,7 +133,7 @@ static uint8_t standbyConfig;
void NRF24L01_Initialize(uint8_t baseConfig) void NRF24L01_Initialize(uint8_t baseConfig)
{ {
standbyConfig = BV(NRF24L01_00_CONFIG_PWR_UP) | baseConfig; standbyConfig = BV(NRF24L01_00_CONFIG_PWR_UP) | baseConfig;
NRF24_CE_LO(); NRF24L01_InitGpio();
// nRF24L01+ needs 100 milliseconds settling time from PowerOnReset to PowerDown mode // nRF24L01+ needs 100 milliseconds settling time from PowerOnReset to PowerDown mode
static const uint32_t settlingTimeUs = 100000; static const uint32_t settlingTimeUs = 100000;
const uint32_t currentTimeUs = micros(); const uint32_t currentTimeUs = micros();
@ -208,8 +219,8 @@ bool NRF24L01_ReadPayloadIfAvailable(uint8_t *data, uint8_t length)
} }
#ifndef UNIT_TEST #ifndef UNIT_TEST
#define DISABLE_RX() {IOHi(IOGetByTag(IO_TAG(RX_NSS_PIN)));} #define DISABLE_RX() {IOHi(DEFIO_IO(RX_NSS_PIN));}
#define ENABLE_RX() {IOLo(IOGetByTag(IO_TAG(RX_NSS_PIN)));} #define ENABLE_RX() {IOLo(DEFIO_IO(RX_NSS_PIN));}
/* /*
* Fast read of payload, for use in interrupt service routine * Fast read of payload, for use in interrupt service routine
*/ */

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

@ -34,7 +34,7 @@
#include "timer.h" #include "timer.h"
#include "pwm_output.h" #include "pwm_output.h"
#include "pwm_rx.h" #include "rx_pwm.h"
#define DEBUG_PPM_ISR #define DEBUG_PPM_ISR

2
src/main/drivers/pwm_rx.h → src/main/drivers/rx_pwm.h
View file

@ -17,7 +17,7 @@
#pragma once #pragma once
#include "drivers/io.h" #include "drivers/io_types.h"
typedef enum { typedef enum {
INPUT_FILTERING_DISABLED = 0, INPUT_FILTERING_DISABLED = 0,

32
src/main/drivers/rx_spi.c
View file

@ -28,22 +28,17 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "system.h" #include "bus_spi.h"
#include "bus_spi_soft.h"
#include "gpio.h" #include "gpio.h"
#include "io.h" #include "io.h"
#include "io_impl.h" #include "io_impl.h"
#include "rcc.h" #include "rcc.h"
#include "rx_spi.h" #include "rx_spi.h"
#include "system.h"
#include "bus_spi.h" #define DISABLE_RX() {IOHi(DEFIO_IO(RX_NSS_PIN));}
#include "bus_spi_soft.h" #define ENABLE_RX() {IOLo(DEFIO_IO(RX_NSS_PIN));}
#define DISABLE_RX() {IOHi(IOGetByTag(IO_TAG(RX_NSS_PIN)));}
#define ENABLE_RX() {IOLo(IOGetByTag(IO_TAG(RX_NSS_PIN)));}
#ifdef RX_CE_PIN
#define RX_CE_HI() {IOHi(IOGetByTag(IO_TAG(RX_CE_PIN)));}
#define RX_CE_LO() {IOLo(IOGetByTag(IO_TAG(RX_CE_PIN)));}
#endif
#ifdef USE_RX_SOFTSPI #ifdef USE_RX_SOFTSPI
static const softSPIDevice_t softSPIDevice = { static const softSPIDevice_t softSPIDevice = {
@ -73,24 +68,9 @@ void rxSpiDeviceInit(rx_spi_type_e spiType)
#else #else
UNUSED(spiType); UNUSED(spiType);
const SPIDevice rxSPIDevice = spiDeviceByInstance(RX_SPI_INSTANCE); const SPIDevice rxSPIDevice = spiDeviceByInstance(RX_SPI_INSTANCE);
IOInit(IOGetByTag(IO_TAG(RX_NSS_PIN)), OWNER_SPI_CS, rxSPIDevice + 1); IOInit(DEFIO_IO(RX_NSS_PIN), OWNER_SPI_CS, rxSPIDevice + 1);
#endif // USE_RX_SOFTSPI #endif // USE_RX_SOFTSPI
#if defined(STM32F10X)
RCC_AHBPeriphClockCmd(RX_NSS_GPIO_CLK_PERIPHERAL, ENABLE);
RCC_AHBPeriphClockCmd(RX_CE_GPIO_CLK_PERIPHERAL, ENABLE);
#endif
#ifdef RX_CE_PIN
// CE as OUTPUT
IOInit(IOGetByTag(IO_TAG(RX_CE_PIN)), OWNER_RX_SPI_CS, rxSPIDevice + 1);
#if defined(STM32F10X)
IOConfigGPIO(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG);
#elif defined(STM32F3) || defined(STM32F4)
IOConfigGPIOAF(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG, 0);
#endif
RX_CE_LO();
#endif // RX_CE_PIN
DISABLE_RX(); DISABLE_RX();
#ifdef RX_SPI_INSTANCE #ifdef RX_SPI_INSTANCE

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

@ -32,6 +32,7 @@
#include "io.h" #include "io.h"
#include "nvic.h" #include "nvic.h"
#include "inverter.h" #include "inverter.h"
#include "dma.h"
#include "serial.h" #include "serial.h"
#include "serial_uart.h" #include "serial_uart.h"
@ -265,6 +266,7 @@ void uartStartTxDMA(uartPort_t *s)
s->port.txBufferTail = 0; s->port.txBufferTail = 0;
} }
s->txDMAEmpty = false; s->txDMAEmpty = false;
HAL_CLEANCACHE((uint8_t *)&s->port.txBuffer[fromwhere],size);
HAL_UART_Transmit_DMA(&s->Handle, (uint8_t *)&s->port.txBuffer[fromwhere], size); HAL_UART_Transmit_DMA(&s->Handle, (uint8_t *)&s->port.txBuffer[fromwhere], size);
} }

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

@ -422,7 +422,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
} }
// DMA TX Interrupt // DMA TX Interrupt
dmaInit(uart->txIrq, OWNER_SERIAL_TX, (uint32_t)uart); dmaInit(uart->txIrq, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
dmaSetHandler(uart->txIrq, dmaIRQHandler, uart->txPriority, (uint32_t)uart); dmaSetHandler(uart->txIrq, dmaIRQHandler, uart->txPriority, (uint32_t)uart);

30
src/main/drivers/serial_usb_vcp.c
View file

@ -25,10 +25,14 @@
#include "common/utils.h" #include "common/utils.h"
#include "io.h" #include "io.h"
#if defined(STM32F4)
#include "usb_core.h" #include "usb_core.h"
#ifdef STM32F4
#include "usbd_cdc_vcp.h" #include "usbd_cdc_vcp.h"
#elif defined(STM32F7)
#include "vcp_hal/usbd_cdc_interface.h"
USBD_HandleTypeDef USBD_Device;
#else #else
#include "usb_core.h"
#include "usb_init.h" #include "usb_init.h"
#include "hw_config.h" #include "hw_config.h"
#endif #endif
@ -94,9 +98,8 @@ static void usbVcpWriteBuf(serialPort_t *instance, const void *data, int count)
uint32_t start = millis(); uint32_t start = millis();
const uint8_t *p = data; const uint8_t *p = data;
uint32_t txed = 0;
while (count > 0) { while (count > 0) {
txed = CDC_Send_DATA(p, count); uint32_t txed = CDC_Send_DATA(p, count);
count -= txed; count -= txed;
p += txed; p += txed;
@ -108,7 +111,7 @@ static void usbVcpWriteBuf(serialPort_t *instance, const void *data, int count)
static bool usbVcpFlush(vcpPort_t *port) static bool usbVcpFlush(vcpPort_t *port)
{ {
uint8_t count = port->txAt; uint32_t count = port->txAt;
port->txAt = 0; port->txAt = 0;
if (count == 0) { if (count == 0) {
@ -121,9 +124,8 @@ static bool usbVcpFlush(vcpPort_t *port)
uint32_t start = millis(); uint32_t start = millis();
uint8_t *p = port->txBuf; uint8_t *p = port->txBuf;
uint32_t txed = 0;
while (count > 0) { while (count > 0) {
txed = CDC_Send_DATA(p, count); uint32_t txed = CDC_Send_DATA(p, count);
count -= txed; count -= txed;
p += txed; p += txed;
@ -181,10 +183,24 @@ serialPort_t *usbVcpOpen(void)
{ {
vcpPort_t *s; vcpPort_t *s;
#ifdef STM32F4 #if defined(STM32F4)
IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0);
IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0);
USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb);
#elif defined(STM32F7)
IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0);
IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0);
/* Init Device Library */
USBD_Init(&USBD_Device, &VCP_Desc, 0);
/* Add Supported Class */
USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS);
/* Add CDC Interface Class */
USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_fops);
/* Start Device Process */
USBD_Start(&USBD_Device);
#else #else
Set_System(); Set_System();
Set_USBClock(); Set_USBClock();

193
src/main/drivers/serial_usb_vcp_hal.c
View file

@ -1,193 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <stdbool.h>
#include "platform.h"
#include "build/build_config.h"
#include "common/utils.h"
#include "io.h"
#include "vcp_hal/usbd_cdc_interface.h"
#include "system.h"
#include "serial.h"
#include "serial_usb_vcp.h"
#define USB_TIMEOUT 50
static vcpPort_t vcpPort;
USBD_HandleTypeDef USBD_Device;
static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
{
UNUSED(instance);
UNUSED(baudRate);
// TODO implement
}
static void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
{
UNUSED(instance);
UNUSED(mode);
// TODO implement
}
static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance)
{
UNUSED(instance);
return true;
}
static uint32_t usbVcpAvailable(const serialPort_t *instance)
{
UNUSED(instance);
uint32_t receiveLength = vcpAvailable();
return receiveLength;
}
static uint8_t usbVcpRead(serialPort_t *instance)
{
UNUSED(instance);
return vcpRead();
}
static void usbVcpWriteBuf(serialPort_t *instance, const void *data, int count)
{
UNUSED(instance);
if (!vcpIsConnected()) {
return;
}
uint32_t start = millis();
const uint8_t *p = data;
while (count > 0) {
uint32_t txed = vcpWrite(p, count);
count -= txed;
p += txed;
if (millis() - start > USB_TIMEOUT) {
break;
}
}
}
static bool usbVcpFlush(vcpPort_t *port)
{
uint8_t count = port->txAt;
port->txAt = 0;
if (count == 0) {
return true;
}
if (!vcpIsConnected()) {
return false;
}
uint32_t start = millis();
uint8_t *p = port->txBuf;
while (count > 0) {
uint32_t txed = vcpWrite(p, count);
count -= txed;
p += txed;
if (millis() - start > USB_TIMEOUT) {
break;
}
}
return count == 0;
}
static void usbVcpWrite(serialPort_t *instance, uint8_t c)
{
vcpPort_t *port = container_of(instance, vcpPort_t, port);
port->txBuf[port->txAt++] = c;
if (!port->buffering || port->txAt >= ARRAYLEN(port->txBuf)) {
usbVcpFlush(port);
}
}
static void usbVcpBeginWrite(serialPort_t *instance)
{
vcpPort_t *port = container_of(instance, vcpPort_t, port);
port->buffering = true;
}
uint32_t usbTxBytesFree()
{
return CDC_Send_FreeBytes();
}
static void usbVcpEndWrite(serialPort_t *instance)
{
vcpPort_t *port = container_of(instance, vcpPort_t, port);
port->buffering = false;
usbVcpFlush(port);
}
static const struct serialPortVTable usbVTable[] = {
{
.serialWrite = usbVcpWrite,
.serialTotalRxWaiting = usbVcpAvailable,
.serialTotalTxFree = usbTxBytesFree,
.serialRead = usbVcpRead,
.serialSetBaudRate = usbVcpSetBaudRate,
.isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty,
.setMode = usbVcpSetMode,
.writeBuf = usbVcpWriteBuf,
.beginWrite = usbVcpBeginWrite,
.endWrite = usbVcpEndWrite
}
};
serialPort_t *usbVcpOpen(void)
{
vcpPort_t *s;
/* Init Device Library */
USBD_Init(&USBD_Device, &VCP_Desc, 0);
/* Add Supported Class */
USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS);
/* Add CDC Interface Class */
USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_fops);
/* Start Device Process */
USBD_Start(&USBD_Device);
s = &vcpPort;
s->port.vTable = usbVTable;
return (serialPort_t *)s;
}
uint32_t usbVcpGetBaudRate(serialPort_t *instance)
{
UNUSED(instance);
return vcpBaudrate();
}

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

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

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

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

30
src/main/drivers/timer_stm32f10x.c
View file

@ -15,7 +15,7 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>. * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "platform.h" #include <platform.h>
#include "common/utils.h" #include "common/utils.h"
@ -24,23 +24,23 @@
#include "timer.h" #include "timer.h"
const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM1, .rcc = RCC_APB2(TIM1) }, { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn },
{ .TIMx = TIM2, .rcc = RCC_APB1(TIM2) }, { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), .inputIrq = TIM2_IRQn },
{ .TIMx = TIM3, .rcc = RCC_APB1(TIM3) }, { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), .inputIrq = TIM3_IRQn },
{ .TIMx = TIM4, .rcc = RCC_APB1(TIM4) }, { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), .inputIrq = TIM4_IRQn },
#if defined(STM32F10X_HD) || defined(STM32F10X_CL) || defined(STM32F10X_XL) || defined(STM32F10X_HD_VL) #if defined(STM32F10X_HD) || defined(STM32F10X_CL) || defined(STM32F10X_XL) || defined(STM32F10X_HD_VL)
{ .TIMx = TIM5, .rcc = RCC_APB1(TIM5) }, { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), .inputIrq = TIM5_IRQn },
{ .TIMx = TIM6, .rcc = RCC_APB1(TIM6) }, { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), .inputIrq = 0 },
{ .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), .inputIrq = 0 },
#endif #endif
#if defined(STM32F10X_XL) || defined(STM32F10X_HD_VL) #if defined(STM32F10X_XL) || defined(STM32F10X_HD_VL)
{ .TIMx = TIM8, .rcc = RCC_APB1(TIM8) }, { .TIMx = TIM8, .rcc = RCC_APB1(TIM8), .inputIrq = TIM8_CC_IRQn },
{ .TIMx = TIM9, .rcc = RCC_APB2(TIM9) }, { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), .inputIrq = TIM1_BRK_TIM9_IRQn },
{ .TIMx = TIM10, .rcc = RCC_APB2(TIM10) }, { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), .inputIrq = TIM1_UP_TIM10_IRQn },
{ .TIMx = TIM11, .rcc = RCC_APB2(TIM11) }, { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), .inputIrq = TIM1_TRG_COM_TIM11_IRQn },
{ .TIMx = TIM12, .rcc = RCC_APB1(TIM12) }, { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), .inputIrq = TIM12_IRQn },
{ .TIMx = TIM13, .rcc = RCC_APB1(TIM13) }, { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), .inputIrq = TIM13_IRQn },
{ .TIMx = TIM14, .rcc = RCC_APB1(TIM14) }, { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), .inputIrq = TIM14_IRQn },
#endif #endif
}; };

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

@ -32,6 +32,7 @@
#include "common/maths.h" #include "common/maths.h"
#include "vtx_rtc6705.h" #include "vtx_rtc6705.h"
#include "io.h"
#include "bus_spi.h" #include "bus_spi.h"
#include "system.h" #include "system.h"
@ -86,6 +87,12 @@
#define DISABLE_RTC6705 GPIO_SetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN) #define DISABLE_RTC6705 GPIO_SetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
#define ENABLE_RTC6705 GPIO_ResetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN) #define ENABLE_RTC6705 GPIO_ResetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
static IO_t vtxPowerPin = IO_NONE;
#define ENABLE_VTX_POWER IOLo(vtxPowerPin)
#define DISABLE_VTX_POWER IOHi(vtxPowerPin)
// Define variables // Define variables
static const uint32_t channelArray[RTC6705_BAND_MAX][RTC6705_CHANNEL_MAX] = { static const uint32_t channelArray[RTC6705_BAND_MAX][RTC6705_CHANNEL_MAX] = {
{ RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 }, { RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 },
@ -129,8 +136,17 @@ static uint32_t reverse32(uint32_t in)
/** /**
* Start chip if available * Start chip if available
*/ */
bool rtc6705Init(void) bool rtc6705Init(void)
{ {
#ifdef RTC6705_POWER_PIN
vtxPowerPin = IOGetByTag(IO_TAG(RTC6705_POWER_PIN));
IOInit(vtxPowerPin, OWNER_VTX, 0);
IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP);
ENABLE_VTX_POWER;
#endif
DISABLE_RTC6705; DISABLE_RTC6705;
spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
return rtc6705IsReady(); return rtc6705IsReady();

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

@ -36,18 +36,19 @@
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/compass.h" #include "drivers/compass.h"
#include "drivers/system.h" #include "drivers/io.h"
#include "drivers/timer.h" #include "drivers/light_ws2811strip.h"
#include "drivers/pwm_rx.h" #include "drivers/max7456.h"
#include "drivers/rx_spi.h"
#include "drivers/serial.h"
#include "drivers/pwm_esc_detect.h" #include "drivers/pwm_esc_detect.h"
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
#include "drivers/vcd.h" #include "drivers/rx_pwm.h"
#include "drivers/max7456.h" #include "drivers/rx_spi.h"
#include "drivers/sound_beeper.h"
#include "drivers/light_ws2811strip.h"
#include "drivers/sdcard.h" #include "drivers/sdcard.h"
#include "drivers/serial.h"
#include "drivers/sound_beeper.h"
#include "drivers/system.h"
#include "drivers/timer.h"
#include "drivers/vcd.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
@ -747,7 +748,7 @@ void createDefaultConfig(master_t *config)
resetProfile(&config->profile[0]); resetProfile(&config->profile[0]);
resetRollAndPitchTrims(&config->accelerometerTrims); resetRollAndPitchTrims(&config->accelerometerConfig.accelerometerTrims);
config->compassConfig.mag_declination = 0; config->compassConfig.mag_declination = 0;
config->accelerometerConfig.acc_lpf_hz = 10.0f; config->accelerometerConfig.acc_lpf_hz = 10.0f;
@ -783,13 +784,13 @@ void createDefaultConfig(master_t *config)
#ifdef USE_SERVOS #ifdef USE_SERVOS
// servos // servos
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
config->servoConf[i].min = DEFAULT_SERVO_MIN; config->servoProfile.servoConf[i].min = DEFAULT_SERVO_MIN;
config->servoConf[i].max = DEFAULT_SERVO_MAX; config->servoProfile.servoConf[i].max = DEFAULT_SERVO_MAX;
config->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; config->servoProfile.servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
config->servoConf[i].rate = 100; config->servoProfile.servoConf[i].rate = 100;
config->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; config->servoProfile.servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE;
config->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; config->servoProfile.servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE;
config->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; config->servoProfile.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
} }
// gimbal // gimbal
@ -882,7 +883,7 @@ void activateConfig(void)
resetAdjustmentStates(); resetAdjustmentStates();
useRcControlsConfig( useRcControlsConfig(
masterConfig.modeActivationConditions, modeActivationProfile()->modeActivationConditions,
&masterConfig.motorConfig, &masterConfig.motorConfig,
&currentProfile->pidProfile &currentProfile->pidProfile
); );
@ -909,7 +910,7 @@ void activateConfig(void)
); );
#ifdef USE_SERVOS #ifdef USE_SERVOS
servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig); servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoProfile.servoConf, &masterConfig.gimbalConfig);
#endif #endif
@ -1083,6 +1084,9 @@ void saveConfigAndNotify(void)
void changeProfile(uint8_t profileIndex) void changeProfile(uint8_t profileIndex)
{ {
if (profileIndex >= MAX_PROFILE_COUNT) {
profileIndex = MAX_PROFILE_COUNT - 1;
}
masterConfig.current_profile_index = profileIndex; masterConfig.current_profile_index = profileIndex;
writeEEPROM(); writeEEPROM();
readEEPROM(); readEEPROM();

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

@ -262,10 +262,8 @@ static void serializeNames(sbuf_t *dst, const char *s)
static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
{ {
uint8_t boxIndex; for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
const box_t *candidate; const box_t *candidate = &boxes[boxIndex];
for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
candidate = &boxes[boxIndex];
if (candidate->boxId == activeBoxId) { if (candidate->boxId == activeBoxId) {
return candidate; return candidate;
} }
@ -275,10 +273,8 @@ static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
static const box_t *findBoxByPermenantId(uint8_t permenantId) static const box_t *findBoxByPermenantId(uint8_t permenantId)
{ {
uint8_t boxIndex; for (uint8_t boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
const box_t *candidate; const box_t *candidate = &boxes[boxIndex];
for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
candidate = &boxes[boxIndex];
if (candidate->permanentId == permenantId) { if (candidate->permanentId == permenantId) {
return candidate; return candidate;
} }
@ -397,9 +393,7 @@ void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXCALIB; activeBoxIds[activeBoxIdCount++] = BOXCALIB;
} }
if (feature(FEATURE_OSD)) {
activeBoxIds[activeBoxIdCount++] = BOXOSD; activeBoxIds[activeBoxIdCount++] = BOXOSD;
}
#ifdef TELEMETRY #ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch) { if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch) {
@ -683,25 +677,25 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_SERVO_CONFIGURATIONS: case MSP_SERVO_CONFIGURATIONS:
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
sbufWriteU16(dst, masterConfig.servoConf[i].min); sbufWriteU16(dst, servoProfile()->servoConf[i].min);
sbufWriteU16(dst, masterConfig.servoConf[i].max); sbufWriteU16(dst, servoProfile()->servoConf[i].max);
sbufWriteU16(dst, masterConfig.servoConf[i].middle); sbufWriteU16(dst, servoProfile()->servoConf[i].middle);
sbufWriteU8(dst, masterConfig.servoConf[i].rate); sbufWriteU8(dst, servoProfile()->servoConf[i].rate);
sbufWriteU8(dst, masterConfig.servoConf[i].angleAtMin); sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMin);
sbufWriteU8(dst, masterConfig.servoConf[i].angleAtMax); sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMax);
sbufWriteU8(dst, masterConfig.servoConf[i].forwardFromChannel); sbufWriteU8(dst, servoProfile()->servoConf[i].forwardFromChannel);
sbufWriteU32(dst, masterConfig.servoConf[i].reversedSources); sbufWriteU32(dst, servoProfile()->servoConf[i].reversedSources);
} }
break; break;
case MSP_SERVO_MIX_RULES: case MSP_SERVO_MIX_RULES:
for (int i = 0; i < MAX_SERVO_RULES; i++) { for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, masterConfig.customServoMixer[i].targetChannel); sbufWriteU8(dst, customServoMixer(i)->targetChannel);
sbufWriteU8(dst, masterConfig.customServoMixer[i].inputSource); sbufWriteU8(dst, customServoMixer(i)->inputSource);
sbufWriteU8(dst, masterConfig.customServoMixer[i].rate); sbufWriteU8(dst, customServoMixer(i)->rate);
sbufWriteU8(dst, masterConfig.customServoMixer[i].speed); sbufWriteU8(dst, customServoMixer(i)->speed);
sbufWriteU8(dst, masterConfig.customServoMixer[i].min); sbufWriteU8(dst, customServoMixer(i)->min);
sbufWriteU8(dst, masterConfig.customServoMixer[i].max); sbufWriteU8(dst, customServoMixer(i)->max);
sbufWriteU8(dst, masterConfig.customServoMixer[i].box); sbufWriteU8(dst, customServoMixer(i)->box);
} }
break; break;
#endif #endif
@ -797,7 +791,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_MODE_RANGES: case MSP_MODE_RANGES:
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
const box_t *box = &boxes[mac->modeId]; const box_t *box = &boxes[mac->modeId];
sbufWriteU8(dst, box->permanentId); sbufWriteU8(dst, box->permanentId);
sbufWriteU8(dst, mac->auxChannelIndex); sbufWriteU8(dst, mac->auxChannelIndex);
@ -808,7 +802,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_ADJUSTMENT_RANGES: case MSP_ADJUSTMENT_RANGES:
for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i]; adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
sbufWriteU8(dst, adjRange->adjustmentIndex); sbufWriteU8(dst, adjRange->adjustmentIndex);
sbufWriteU8(dst, adjRange->auxChannelIndex); sbufWriteU8(dst, adjRange->auxChannelIndex);
sbufWriteU8(dst, adjRange->range.startStep); sbufWriteU8(dst, adjRange->range.startStep);
@ -908,8 +902,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
// Additional commands that are not compatible with MultiWii // Additional commands that are not compatible with MultiWii
case MSP_ACC_TRIM: case MSP_ACC_TRIM:
sbufWriteU16(dst, masterConfig.accelerometerTrims.values.pitch); sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch);
sbufWriteU16(dst, masterConfig.accelerometerTrims.values.roll); sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll);
break; break;
case MSP_UID: case MSP_UID:
@ -1289,8 +1283,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#endif #endif
break; break;
case MSP_SET_ACC_TRIM: case MSP_SET_ACC_TRIM:
masterConfig.accelerometerTrims.values.pitch = sbufReadU16(src); accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src);
masterConfig.accelerometerTrims.values.roll = sbufReadU16(src); accelerometerConfig()->accelerometerTrims.values.roll = sbufReadU16(src);
break; break;
case MSP_SET_ARMING_CONFIG: case MSP_SET_ARMING_CONFIG:
armingConfig()->auto_disarm_delay = sbufReadU8(src); armingConfig()->auto_disarm_delay = sbufReadU8(src);
@ -1316,7 +1310,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_MODE_RANGE: case MSP_SET_MODE_RANGE:
i = sbufReadU8(src); i = sbufReadU8(src);
if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
i = sbufReadU8(src); i = sbufReadU8(src);
const box_t *box = findBoxByPermenantId(i); const box_t *box = findBoxByPermenantId(i);
if (box) { if (box) {
@ -1325,7 +1319,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
mac->range.startStep = sbufReadU8(src); mac->range.startStep = sbufReadU8(src);
mac->range.endStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src);
useRcControlsConfig(masterConfig.modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile); useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile);
} else { } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} }
@ -1337,7 +1331,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_ADJUSTMENT_RANGE: case MSP_SET_ADJUSTMENT_RANGE:
i = sbufReadU8(src); i = sbufReadU8(src);
if (i < MAX_ADJUSTMENT_RANGE_COUNT) { if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i]; adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
i = sbufReadU8(src); i = sbufReadU8(src);
if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
adjRange->adjustmentIndex = i; adjRange->adjustmentIndex = i;
@ -1420,20 +1414,19 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef USE_SERVOS #ifdef USE_SERVOS
if (dataSize != 1 + sizeof(servoParam_t)) { if (dataSize != 1 + sizeof(servoParam_t)) {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break;
} }
i = sbufReadU8(src); i = sbufReadU8(src);
if (i >= MAX_SUPPORTED_SERVOS) { if (i >= MAX_SUPPORTED_SERVOS) {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} else { } else {
masterConfig.servoConf[i].min = sbufReadU16(src); servoProfile()->servoConf[i].min = sbufReadU16(src);
masterConfig.servoConf[i].max = sbufReadU16(src); servoProfile()->servoConf[i].max = sbufReadU16(src);
masterConfig.servoConf[i].middle = sbufReadU16(src); servoProfile()->servoConf[i].middle = sbufReadU16(src);
masterConfig.servoConf[i].rate = sbufReadU8(src); servoProfile()->servoConf[i].rate = sbufReadU8(src);
masterConfig.servoConf[i].angleAtMin = sbufReadU8(src); servoProfile()->servoConf[i].angleAtMin = sbufReadU8(src);
masterConfig.servoConf[i].angleAtMax = sbufReadU8(src); servoProfile()->servoConf[i].angleAtMax = sbufReadU8(src);
masterConfig.servoConf[i].forwardFromChannel = sbufReadU8(src); servoProfile()->servoConf[i].forwardFromChannel = sbufReadU8(src);
masterConfig.servoConf[i].reversedSources = sbufReadU32(src); servoProfile()->servoConf[i].reversedSources = sbufReadU32(src);
} }
#endif #endif
break; break;
@ -1444,13 +1437,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= MAX_SERVO_RULES) { if (i >= MAX_SERVO_RULES) {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} else { } else {
masterConfig.customServoMixer[i].targetChannel = sbufReadU8(src); customServoMixer(i)->targetChannel = sbufReadU8(src);
masterConfig.customServoMixer[i].inputSource = sbufReadU8(src); customServoMixer(i)->inputSource = sbufReadU8(src);
masterConfig.customServoMixer[i].rate = sbufReadU8(src); customServoMixer(i)->rate = sbufReadU8(src);
masterConfig.customServoMixer[i].speed = sbufReadU8(src); customServoMixer(i)->speed = sbufReadU8(src);
masterConfig.customServoMixer[i].min = sbufReadU8(src); customServoMixer(i)->min = sbufReadU8(src);
masterConfig.customServoMixer[i].max = sbufReadU8(src); customServoMixer(i)->max = sbufReadU8(src);
masterConfig.customServoMixer[i].box = sbufReadU8(src); customServoMixer(i)->box = sbufReadU8(src);
loadCustomServoMixer(); loadCustomServoMixer();
} }
#endif #endif
@ -1574,7 +1567,6 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_TRANSPONDER_CONFIG: case MSP_SET_TRANSPONDER_CONFIG:
if (dataSize != sizeof(masterConfig.transponderData)) { if (dataSize != sizeof(masterConfig.transponderData)) {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break;
} }
for (unsigned int i = 0; i < sizeof(masterConfig.transponderData); i++) { for (unsigned int i = 0; i < sizeof(masterConfig.transponderData); i++) {
masterConfig.transponderData[i] = sbufReadU8(src); masterConfig.transponderData[i] = sbufReadU8(src);

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

@ -92,7 +92,7 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
{ {
UNUSED(currentTimeUs); UNUSED(currentTimeUs);
imuUpdateAccelerometer(&masterConfig.accelerometerTrims); accUpdate(&accelerometerConfig()->accelerometerTrims);
} }
static void taskHandleSerial(timeUs_t currentTimeUs) static void taskHandleSerial(timeUs_t currentTimeUs)
@ -111,8 +111,8 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
static void taskUpdateBattery(timeUs_t currentTimeUs) static void taskUpdateBattery(timeUs_t currentTimeUs)
{ {
#if defined(USE_ADC) || defined(USE_ESC_SENSOR) #if defined(USE_ADC) || defined(USE_ESC_SENSOR)
static uint32_t vbatLastServiced = 0;
if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) { if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) {
static uint32_t vbatLastServiced = 0;
if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) { if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTimeUs; vbatLastServiced = currentTimeUs;
updateBattery(); updateBattery();
@ -120,8 +120,8 @@ static void taskUpdateBattery(timeUs_t currentTimeUs)
} }
#endif #endif
static uint32_t ibatLastServiced = 0;
if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) { if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) {
static uint32_t ibatLastServiced = 0;
const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced); const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) { if (ibatTimeSinceLastServiced >= IBATINTERVAL) {

12
src/main/fc/mw.c
View file

@ -103,8 +103,8 @@ float rcInput[3];
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{ {
masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
masterConfig.accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; accelerometerConfig()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
saveConfigAndNotify(); saveConfigAndNotify();
} }
@ -576,11 +576,11 @@ void processRx(timeUs_t currentTimeUs)
updateInflightCalibrationState(); updateInflightCalibrationState();
} }
updateActivatedModes(masterConfig.modeActivationConditions); updateActivatedModes(modeActivationProfile()->modeActivationConditions);
if (!cliMode) { if (!cliMode) {
updateAdjustmentStates(masterConfig.adjustmentRanges); updateAdjustmentStates(adjustmentProfile()->adjustmentRanges);
processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig); processRcAdjustments(currentControlRateProfile, rxConfig());
} }
bool canUseHorizonMode = true; bool canUseHorizonMode = true;
@ -679,7 +679,7 @@ void subTaskPidController(void)
pidController( pidController(
&currentProfile->pidProfile, &currentProfile->pidProfile,
pidConfig()->max_angle_inclination, pidConfig()->max_angle_inclination,
&masterConfig.accelerometerTrims, &accelerometerConfig()->accelerometerTrims,
rxConfig()->midrc rxConfig()->midrc
); );
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;} if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;}

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

@ -140,6 +140,10 @@ typedef struct modeActivationCondition_s {
channelRange_t range; channelRange_t range;
} modeActivationCondition_t; } modeActivationCondition_t;
typedef struct modeActivationProfile_s {
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
} modeActivationProfile_t;
#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
typedef struct controlRateConfig_s { typedef struct controlRateConfig_s {
@ -263,6 +267,10 @@ typedef struct adjustmentState_s {
#define MAX_ADJUSTMENT_RANGE_COUNT 15 #define MAX_ADJUSTMENT_RANGE_COUNT 15
typedef struct adjustmentProfile_s {
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
} adjustmentProfile_t;
bool isAirmodeActive(void); bool isAirmodeActive(void);
void resetAdjustmentStates(void); void resetAdjustmentStates(void);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);

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

@ -65,7 +65,6 @@ float fc_acc;
float smallAngleCosZ = 0; float smallAngleCosZ = 0;
float magneticDeclination = 0.0f; // calculated at startup from config float magneticDeclination = 0.0f; // calculated at startup from config
static bool isAccelUpdatedAtLeastOnce = false;
static imuRuntimeConfig_t imuRuntimeConfig; static imuRuntimeConfig_t imuRuntimeConfig;
static pidProfile_t *pidProfile; static pidProfile_t *pidProfile;
@ -406,17 +405,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
} }
void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
{
if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims);
isAccelUpdatedAtLeastOnce = true;
}
}
void imuUpdateAttitude(timeUs_t currentTimeUs) void imuUpdateAttitude(timeUs_t currentTimeUs)
{ {
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) { if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
imuCalculateEstimatedAttitude(currentTimeUs); imuCalculateEstimatedAttitude(currentTimeUs);
} else { } else {
acc.accSmooth[X] = 0; acc.accSmooth[X] = 0;

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

@ -98,8 +98,6 @@ void imuConfigure(
float getCosTiltAngle(void); float getCosTiltAngle(void);
void calculateEstimatedAltitude(timeUs_t currentTimeUs); void calculateEstimatedAltitude(timeUs_t currentTimeUs);
union rollAndPitchTrims_u;
void imuUpdateAccelerometer(union rollAndPitchTrims_u *accelerometerTrims);
void imuUpdateAttitude(timeUs_t currentTimeUs); void imuUpdateAttitude(timeUs_t currentTimeUs);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle); float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
@ -109,5 +107,4 @@ union u_fp_vector;
int16_t imuCalculateHeading(union u_fp_vector *vec); int16_t imuCalculateHeading(union u_fp_vector *vec);
void imuResetAccelerationSum(void); void imuResetAccelerationSum(void);
void imuUpdateAcc(union rollAndPitchTrims_u *accelerometerTrims);
void imuInit(void); void imuInit(void);

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

@ -55,7 +55,7 @@
#define EXTERNAL_CONVERSION_MIN_VALUE 1000 #define EXTERNAL_CONVERSION_MIN_VALUE 1000
#define EXTERNAL_CONVERSION_MAX_VALUE 2000 #define EXTERNAL_CONVERSION_MAX_VALUE 2000
uint8_t motorCount; static uint8_t motorCount;
int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
@ -239,7 +239,8 @@ static motorMixer_t *customMixers;
static uint16_t disarmMotorOutput, motorOutputHigh, motorOutputLow, deadbandMotor3dHigh, deadbandMotor3dLow; static uint16_t disarmMotorOutput, motorOutputHigh, motorOutputLow, deadbandMotor3dHigh, deadbandMotor3dLow;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
uint8_t getMotorCount() { uint8_t getMotorCount()
{
return motorCount; return motorCount;
} }
@ -306,13 +307,11 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
void mixerConfigureOutput(void) void mixerConfigureOutput(void)
{ {
int i;
motorCount = 0; motorCount = 0;
if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
// load custom mixer into currentMixer // load custom mixer into currentMixer
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done // check if done
if (customMixers[i].throttle == 0.0f) if (customMixers[i].throttle == 0.0f)
break; break;
@ -321,9 +320,12 @@ void mixerConfigureOutput(void)
} }
} else { } else {
motorCount = mixers[currentMixerMode].motorCount; motorCount = mixers[currentMixerMode].motorCount;
if (motorCount > MAX_SUPPORTED_MOTORS) {
motorCount = MAX_SUPPORTED_MOTORS;
}
// copy motor-based mixers // copy motor-based mixers
if (mixers[currentMixerMode].motor) { if (mixers[currentMixerMode].motor) {
for (i = 0; i < motorCount; i++) for (int i = 0; i < motorCount; i++)
currentMixer[i] = mixers[currentMixerMode].motor[i]; currentMixer[i] = mixers[currentMixerMode].motor[i];
} }
} }
@ -331,7 +333,7 @@ void mixerConfigureOutput(void)
// in 3D mode, mixer gain has to be halved // in 3D mode, mixer gain has to be halved
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
if (motorCount > 1) { if (motorCount > 1) {
for (i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
currentMixer[i].pitch *= 0.5f; currentMixer[i].pitch *= 0.5f;
currentMixer[i].roll *= 0.5f; currentMixer[i].roll *= 0.5f;
currentMixer[i].yaw *= 0.5f; currentMixer[i].yaw *= 0.5f;
@ -344,26 +346,26 @@ void mixerConfigureOutput(void)
void mixerLoadMix(int index, motorMixer_t *customMixers) void mixerLoadMix(int index, motorMixer_t *customMixers)
{ {
int i;
// we're 1-based // we're 1-based
index++; index++;
// clear existing // clear existing
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
customMixers[i].throttle = 0.0f; customMixers[i].throttle = 0.0f;
}
// do we have anything here to begin with? // do we have anything here to begin with?
if (mixers[index].motor != NULL) { if (mixers[index].motor != NULL) {
for (i = 0; i < mixers[index].motorCount; i++) for (int i = 0; i < mixers[index].motorCount; i++) {
customMixers[i] = mixers[index].motor[i]; customMixers[i] = mixers[index].motor[i];
} }
}
} }
#else #else
void mixerConfigureOutput(void) void mixerConfigureOutput(void)
{ {
motorCount = QUAD_MOTOR_COUNT; motorCount = QUAD_MOTOR_COUNT;
for (uint8_t i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
currentMixer[i] = mixerQuadX[i]; currentMixer[i] = mixerQuadX[i];
} }
@ -373,17 +375,19 @@ void mixerConfigureOutput(void)
void mixerResetDisarmedMotors(void) void mixerResetDisarmedMotors(void)
{ {
int i;
// set disarmed motor values // set disarmed motor values
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
motor_disarmed[i] = disarmMotorOutput; motor_disarmed[i] = disarmMotorOutput;
}
} }
void writeMotors(void) void writeMotors(void)
{ {
for (uint8_t i = 0; i < motorCount; i++) { if (pwmAreMotorsEnabled()) {
for (int i = 0; i < motorCount; i++) {
pwmWriteMotor(i, motor[i]); pwmWriteMotor(i, motor[i]);
} }
}
pwmCompleteMotorUpdate(motorCount); pwmCompleteMotorUpdate(motorCount);
} }
@ -391,7 +395,7 @@ void writeMotors(void)
static void writeAllMotors(int16_t mc) static void writeAllMotors(int16_t mc)
{ {
// Sends commands to all motors // Sends commands to all motors
for (uint8_t i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
motor[i] = mc; motor[i] = mc;
} }
@ -413,13 +417,9 @@ void stopPwmAllMotors(void)
void mixTable(pidProfile_t *pidProfile) void mixTable(pidProfile_t *pidProfile)
{ {
uint32_t i = 0;
float vbatCompensationFactor = 1;
// Scale roll/pitch/yaw uniformly to fit within throttle range // Scale roll/pitch/yaw uniformly to fit within throttle range
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
float motorMix[MAX_SUPPORTED_MOTORS]; float throttle = 0, currentThrottleInputRange = 0;
float motorOutputRange = 0, throttle = 0, currentThrottleInputRange = 0, motorMixRange = 0, motorMixMax = 0, motorMixMin = 0;
uint16_t motorOutputMin, motorOutputMax; uint16_t motorOutputMin, motorOutputMax;
static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
bool mixerInversion = false; bool mixerInversion = false;
@ -461,33 +461,41 @@ void mixTable(pidProfile_t *pidProfile)
} }
throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
motorOutputRange = motorOutputMax - motorOutputMin; const float motorOutputRange = motorOutputMax - motorOutputMin;
float scaledAxisPIDf[3]; float scaledAxisPIDf[3];
// Limit the PIDsum // Limit the PIDsum
for (int axis = 0; axis < 3; axis++) for (int axis = 0; axis < 3; axis++) {
scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
}
// Calculate voltage compensation // Calculate voltage compensation
if (batteryConfig && pidProfile->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f;
// Find roll/pitch/yaw desired output // Find roll/pitch/yaw desired output
for (i = 0; i < motorCount; i++) { float motorMix[MAX_SUPPORTED_MOTORS];
float motorMixMax = 0, motorMixMin = 0;
for (int i = 0; i < motorCount; i++) {
motorMix[i] = motorMix[i] =
scaledAxisPIDf[PITCH] * currentMixer[i].pitch + scaledAxisPIDf[PITCH] * currentMixer[i].pitch +
scaledAxisPIDf[ROLL] * currentMixer[i].roll + scaledAxisPIDf[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * scaledAxisPIDf[YAW] * currentMixer[i].yaw; scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig->yaw_motor_direction);
if (vbatCompensationFactor > 1.0f) motorMix[i] *= vbatCompensationFactor; // Add voltage compensation if (vbatCompensationFactor > 1.0f) {
motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
if (motorMix[i] > motorMixMax) motorMixMax = motorMix[i];
if (motorMix[i] < motorMixMin) motorMixMin = motorMix[i];
} }
motorMixRange = motorMixMax - motorMixMin; if (motorMix[i] > motorMixMax) {
motorMixMax = motorMix[i];
} else if (motorMix[i] < motorMixMin) {
motorMixMin = motorMix[i];
}
}
const float motorMixRange = motorMixMax - motorMixMin;
if (motorMixRange > 1.0f) { if (motorMixRange > 1.0f) {
for (i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
motorMix[i] /= motorMixRange; motorMix[i] /= motorMixRange;
} }
// Get the maximum correction by setting offset to center // Get the maximum correction by setting offset to center
@ -499,6 +507,7 @@ void mixTable(pidProfile_t *pidProfile)
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
uint32_t i = 0;
for (i = 0; i < motorCount; i++) { for (i = 0; i < motorCount; i++) {
motor[i] = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))); motor[i] = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)));

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

@ -110,7 +110,6 @@ typedef struct airplaneConfig_s {
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
extern uint8_t motorCount;
extern const mixer_t mixers[]; extern const mixer_t mixers[];
extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];

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

@ -110,6 +110,10 @@ typedef struct servoMixerConfig_s{
int8_t servo_lowpass_enable; // enable/disable lowpass filter int8_t servo_lowpass_enable; // enable/disable lowpass filter
} servoMixerConfig_t; } servoMixerConfig_t;
typedef struct servoProfile_s {
servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
} servoProfile_t;
extern int16_t servo[MAX_SUPPORTED_SERVOS]; extern int16_t servo[MAX_SUPPORTED_SERVOS];
void servoTable(void); void servoTable(void);

12
src/main/io/ledstrip.c
View file

@ -38,7 +38,7 @@
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/gpio.h" #include "drivers/gpio.h"
#include "drivers/timer.h" #include "drivers/timer.h"
#include "drivers/pwm_rx.h" #include "drivers/rx_pwm.h"
#include "common/printf.h" #include "common/printf.h"
#include "common/axis.h" #include "common/axis.h"
@ -568,13 +568,11 @@ static void applyLedBatteryLayer(bool updateNow, timeUs_t *timer)
{ {
static bool flash = false; static bool flash = false;
int state;
int timerDelayUs = HZ_TO_US(1); int timerDelayUs = HZ_TO_US(1);
if (updateNow) { if (updateNow) {
state = getBatteryState();
switch (state) { switch (getBatteryState()) {
case BATTERY_OK: case BATTERY_OK:
flash = true; flash = true;
timerDelayUs = HZ_TO_US(1); timerDelayUs = HZ_TO_US(1);
@ -605,11 +603,10 @@ static void applyLedRssiLayer(bool updateNow, timeUs_t *timer)
{ {
static bool flash = false; static bool flash = false;
int state;
int timerDelay = HZ_TO_US(1); int timerDelay = HZ_TO_US(1);
if (updateNow) { if (updateNow) {
state = (rssi * 100) / 1023; int state = (rssi * 100) / 1023;
if (state > 50) { if (state > 50) {
flash = true; flash = true;
@ -634,11 +631,12 @@ static void applyLedRssiLayer(bool updateNow, timeUs_t *timer)
#ifdef GPS #ifdef GPS
static void applyLedGpsLayer(bool updateNow, timeUs_t *timer) static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
{ {
static uint8_t gpsFlashCounter = 0;
static uint8_t gpsPauseCounter = 0; static uint8_t gpsPauseCounter = 0;
const uint8_t blinkPauseLength = 4; const uint8_t blinkPauseLength = 4;
if (updateNow) { if (updateNow) {
static uint8_t gpsFlashCounter = 0;
if (gpsPauseCounter > 0) { if (gpsPauseCounter > 0) {
gpsPauseCounter--; gpsPauseCounter--;
} else if (gpsFlashCounter >= GPS_numSat) { } else if (gpsFlashCounter >= GPS_numSat) {

8
src/main/io/osd.c
View file

@ -40,6 +40,9 @@
#include "drivers/max7456_symbols.h" #include "drivers/max7456_symbols.h"
#include "drivers/display.h" #include "drivers/display.h"
#include "drivers/system.h" #include "drivers/system.h"
#ifdef USE_RTC6705
#include "drivers/vtx_soft_spi_rtc6705.h"
#endif
#include "cms/cms.h" #include "cms/cms.h"
#include "cms/cms_types.h" #include "cms/cms_types.h"
@ -48,6 +51,9 @@
#include "io/flashfs.h" #include "io/flashfs.h"
#include "io/osd.h" #include "io/osd.h"
#include "io/vtx.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
@ -251,7 +257,7 @@ static void osdDrawSingleElement(uint8_t item)
break; break;
} }
#ifdef VTX #ifdef USE_RTC6705
case OSD_VTX_CHANNEL: case OSD_VTX_CHANNEL:
{ {
sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1); sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1);

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

@ -17,6 +17,8 @@
#pragma once #pragma once
#include "common/time.h"
#define VISIBLE_FLAG 0x0800 #define VISIBLE_FLAG 0x0800
#define BLINK_FLAG 0x0400 #define BLINK_FLAG 0x0400
#define VISIBLE(x) (x & VISIBLE_FLAG) #define VISIBLE(x) (x & VISIBLE_FLAG)

484
src/main/io/serial_cli.c
View file

File diff suppressed because it is too large Load diff

1
src/main/io/vtx.c
View file

@ -23,6 +23,7 @@
// Own interfaces // Own interfaces
#include "io/vtx.h" #include "io/vtx.h"
#include "io/osd.h"
//External dependencies //External dependencies
#include "config/config_master.h" #include "config/config_master.h"

48
src/main/main.c
View file

@ -46,7 +46,7 @@
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/compass.h" #include "drivers/compass.h"
#include "drivers/pwm_esc_detect.h" #include "drivers/pwm_esc_detect.h"
#include "drivers/pwm_rx.h" #include "drivers/rx_pwm.h"
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
#include "drivers/adc.h" #include "drivers/adc.h"
#include "drivers/bus_i2c.h" #include "drivers/bus_i2c.h"
@ -198,18 +198,24 @@ void init(void)
#endif #endif
#if defined(BUTTONS) #if defined(BUTTONS)
#ifdef BUTTON_A_PIN
IO_t buttonAPin = IOGetByTag(IO_TAG(BUTTON_A_PIN)); IO_t buttonAPin = IOGetByTag(IO_TAG(BUTTON_A_PIN));
IOInit(buttonAPin, OWNER_SYSTEM, 0); IOInit(buttonAPin, OWNER_SYSTEM, 0);
IOConfigGPIO(buttonAPin, IOCFG_IPU); IOConfigGPIO(buttonAPin, IOCFG_IPU);
#endif
#ifdef BUTTON_B_PIN
IO_t buttonBPin = IOGetByTag(IO_TAG(BUTTON_B_PIN)); IO_t buttonBPin = IOGetByTag(IO_TAG(BUTTON_B_PIN));
IOInit(buttonBPin, OWNER_SYSTEM, 0); IOInit(buttonBPin, OWNER_SYSTEM, 0);
IOConfigGPIO(buttonBPin, IOCFG_IPU); IOConfigGPIO(buttonBPin, IOCFG_IPU);
#endif
// Check status of bind plug and exit if not active // Check status of bind plug and exit if not active
delayMicroseconds(10); // allow configuration to settle delayMicroseconds(10); // allow configuration to settle
if (!isMPUSoftReset()) { if (!isMPUSoftReset()) {
#if defined(BUTTON_A_PIN) && defined(BUTTON_B_PIN)
// two buttons required
uint8_t secondsRemaining = 5; uint8_t secondsRemaining = 5;
bool bothButtonsHeld; bool bothButtonsHeld;
do { do {
@ -223,6 +229,7 @@ void init(void)
LED0_TOGGLE; LED0_TOGGLE;
} }
} while (bothButtonsHeld); } while (bothButtonsHeld);
#endif
} }
#endif #endif
@ -234,7 +241,7 @@ void init(void)
// Spektrum satellite binding if enabled on startup. // 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. // 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() // The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig); spektrumBind(rxConfig());
break; break;
} }
} }
@ -245,16 +252,16 @@ void init(void)
timerInit(); // timer must be initialized before any channel is allocated timerInit(); // timer must be initialized before any channel is allocated
#if defined(AVOID_UART1_FOR_PWM_PPM) #if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM) #elif defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM) #elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else #else
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif #endif
mixerInit(mixerConfig()->mixerMode, masterConfig.customMotorMixer); mixerInit(mixerConfig()->mixerMode, masterConfig.customMotorMixer);
@ -273,20 +280,13 @@ void init(void)
} }
mixerConfigureOutput(); mixerConfigureOutput();
motorInit(motorConfig(), idlePulse, getMotorCount());
#ifdef USE_SERVOS #ifdef USE_SERVOS
servoConfigureOutput(); servoConfigureOutput();
#endif
#ifdef USE_QUAD_MIXER_ONLY
motorInit(motorConfig(), idlePulse, QUAD_MOTOR_COUNT);
#else
motorInit(motorConfig(), idlePulse, motorCount);
#endif
#ifdef USE_SERVOS
if (isMixerUsingServos()) { if (isMixerUsingServos()) {
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
servoInit(&masterConfig.servoConfig); servoInit(servoConfig());
} }
#endif #endif
@ -388,7 +388,7 @@ void init(void)
if (feature(FEATURE_OSD)) { if (feature(FEATURE_OSD)) {
#ifdef USE_MAX7456 #ifdef USE_MAX7456
// if there is a max7456 chip for the OSD then use it, otherwise use MSP // if there is a max7456 chip for the OSD then use it, otherwise use MSP
displayPort_t *osdDisplayPort = max7456DisplayPortInit(&masterConfig.vcdProfile); displayPort_t *osdDisplayPort = max7456DisplayPortInit(vcdProfile());
#else #else
displayPort_t *osdDisplayPort = displayPortMspInit(); displayPort_t *osdDisplayPort = displayPortMspInit();
#endif #endif
@ -397,7 +397,7 @@ void init(void)
#endif #endif
#ifdef SONAR #ifdef SONAR
const sonarConfig_t *sonarConfig = &masterConfig.sonarConfig; const sonarConfig_t *sonarConfig = sonarConfig();
#else #else
const void *sonarConfig = NULL; const void *sonarConfig = NULL;
#endif #endif
@ -443,16 +443,16 @@ void init(void)
failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle); failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle);
rxInit(rxConfig(), masterConfig.modeActivationConditions); rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions);
#ifdef GPS #ifdef GPS
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {
gpsInit( gpsInit(
&masterConfig.serialConfig, serialConfig(),
&masterConfig.gpsConfig gpsConfig()
); );
navigationInit( navigationInit(
&masterConfig.gpsProfile, gpsProfile(),
&currentProfile->pidProfile &currentProfile->pidProfile
); );
} }
@ -493,7 +493,7 @@ void init(void)
#ifdef USE_FLASHFS #ifdef USE_FLASHFS
#if defined(USE_FLASH_M25P16) #if defined(USE_FLASH_M25P16)
m25p16_init(&masterConfig.flashConfig); m25p16_init(flashConfig());
#endif #endif
flashfsInit(); flashfsInit();
@ -542,7 +542,7 @@ void init(void)
// Now that everything has powered up the voltage and cell count be determined. // Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig); batteryInit(batteryConfig());
#ifdef USE_DASHBOARD #ifdef USE_DASHBOARD
if (feature(FEATURE_DASHBOARD)) { if (feature(FEATURE_DASHBOARD)) {

6
src/main/rx/nrf24_cx10.c
View file

@ -129,7 +129,7 @@ STATIC_UNIT_TESTED uint16_t cx10ConvertToPwmUnsigned(const uint8_t *pVal)
void cx10Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload) void cx10Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
{ {
const uint8_t offset = (cx10Protocol == NRF24RX_CX10) ? 0 : 4; const uint8_t offset = (cx10Protocol == RX_SPI_NRF24_CX10) ? 0 : 4;
rcData[RC_SPI_ROLL] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[5 + offset]); // aileron rcData[RC_SPI_ROLL] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[5 + offset]); // aileron
rcData[RC_SPI_PITCH] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[7 + offset]); // elevator rcData[RC_SPI_PITCH] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[7 + offset]); // elevator
rcData[RC_SPI_THROTTLE] = cx10ConvertToPwmUnsigned(&payload[9 + offset]); // throttle rcData[RC_SPI_THROTTLE] = cx10ConvertToPwmUnsigned(&payload[9 + offset]); // throttle
@ -274,8 +274,8 @@ static void cx10Nrf24Setup(rx_spi_protocol_e protocol)
{ {
cx10Protocol = protocol; cx10Protocol = protocol;
protocolState = STATE_BIND; protocolState = STATE_BIND;
payloadSize = (protocol == NRF24RX_CX10) ? CX10_PROTOCOL_PAYLOAD_SIZE : CX10A_PROTOCOL_PAYLOAD_SIZE; payloadSize = (protocol == RX_SPI_NRF24_CX10) ? CX10_PROTOCOL_PAYLOAD_SIZE : CX10A_PROTOCOL_PAYLOAD_SIZE;
hopTimeout = (protocol == NRF24RX_CX10) ? CX10_PROTOCOL_HOP_TIMEOUT : CX10A_PROTOCOL_HOP_TIMEOUT; hopTimeout = (protocol == RX_SPI_NRF24_CX10) ? CX10_PROTOCOL_HOP_TIMEOUT : CX10A_PROTOCOL_HOP_TIMEOUT;
NRF24L01_Initialize(0); // sets PWR_UP, no CRC NRF24L01_Initialize(0); // sets PWR_UP, no CRC
NRF24L01_SetupBasic(); NRF24L01_SetupBasic();

6
src/main/rx/nrf24_syma.c
View file

@ -124,7 +124,7 @@ static uint32_t hopTimeout = 10000; // 10ms
STATIC_UNIT_TESTED bool symaCheckBindPacket(const uint8_t *packet) STATIC_UNIT_TESTED bool symaCheckBindPacket(const uint8_t *packet)
{ {
bool bindPacket = false; bool bindPacket = false;
if (symaProtocol == NRF24RX_SYMA_X) { if (symaProtocol == RX_SPI_NRF24_SYMA_X) {
if ((packet[5] == 0xaa) && (packet[6] == 0xaa) && (packet[7] == 0xaa)) { if ((packet[5] == 0xaa) && (packet[6] == 0xaa) && (packet[7] == 0xaa)) {
bindPacket = true; bindPacket = true;
rxTxAddr[4] = packet[0]; rxTxAddr[4] = packet[0];
@ -162,7 +162,7 @@ void symaNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
{ {
rcData[RC_SPI_THROTTLE] = symaConvertToPwmUnsigned(packet[0]); // throttle rcData[RC_SPI_THROTTLE] = symaConvertToPwmUnsigned(packet[0]); // throttle
rcData[RC_SPI_ROLL] = symaConvertToPwmSigned(packet[3]); // aileron rcData[RC_SPI_ROLL] = symaConvertToPwmSigned(packet[3]); // aileron
if (symaProtocol == NRF24RX_SYMA_X) { if (symaProtocol == RX_SPI_NRF24_SYMA_X) {
rcData[RC_SPI_PITCH] = symaConvertToPwmSigned(packet[1]); // elevator rcData[RC_SPI_PITCH] = symaConvertToPwmSigned(packet[1]); // elevator
rcData[RC_SPI_YAW] = symaConvertToPwmSigned(packet[2]); // rudder rcData[RC_SPI_YAW] = symaConvertToPwmSigned(packet[2]); // rudder
const uint8_t rate = (packet[5] & 0xc0) >> 6; const uint8_t rate = (packet[5] & 0xc0) >> 6;
@ -271,7 +271,7 @@ static void symaNrf24Setup(rx_spi_protocol_e protocol)
NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV( NRF24L01_00_CONFIG_CRCO)); // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV( NRF24L01_00_CONFIG_CRCO)); // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC
NRF24L01_SetupBasic(); NRF24L01_SetupBasic();
if (symaProtocol == NRF24RX_SYMA_X) { if (symaProtocol == RX_SPI_NRF24_SYMA_X) {
payloadSize = SYMA_X_PROTOCOL_PAYLOAD_SIZE; payloadSize = SYMA_X_PROTOCOL_PAYLOAD_SIZE;
NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm); NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
protocolState = STATE_BIND; protocolState = STATE_BIND;

2
src/main/rx/nrf24_v202.c
View file

@ -230,7 +230,7 @@ static void v202Nrf24Setup(rx_spi_protocol_e protocol)
NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0)); // Enable data pipe 0 NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0)); // Enable data pipe 0
NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xFF); // 4ms retransmit t/o, 15 tries NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xFF); // 4ms retransmit t/o, 15 tries
if (protocol == NRF24RX_V202_250K) { if (protocol == RX_SPI_NRF24_V202_250K) {
NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm); NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
} else { } else {
NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm); NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);

8
src/main/rx/pwm.c
View file

@ -25,14 +25,14 @@
#if defined(USE_PWM) || defined(USE_PPM) #if defined(USE_PWM) || defined(USE_PPM)
#include "build/build_config.h" #include "drivers/rx_pwm.h"
#include "drivers/pwm_rx.h" #include "common/utils.h"
#include "fc/config.h"
#include "config/feature.h" #include "config/feature.h"
#include "fc/config.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/pwm.h" #include "rx/pwm.h"

3
src/main/rx/rx.c
View file

@ -27,11 +27,12 @@
#include "build/debug.h" #include "build/debug.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h"
#include "config/feature.h" #include "config/feature.h"
#include "drivers/adc.h" #include "drivers/adc.h"
#include "drivers/pwm_rx.h" #include "drivers/rx_pwm.h"
#include "drivers/rx_spi.h" #include "drivers/rx_spi.h"
#include "drivers/system.h" #include "drivers/system.h"

28
src/main/rx/rx_spi.c
View file

@ -43,13 +43,13 @@ uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE]; STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE];
STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packet is received STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packet is received
typedef void (*protocolInitPtr)(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); typedef void (*protocolInitFnPtr)(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
typedef rx_spi_received_e (*protocolDataReceivedPtr)(uint8_t *payload); typedef rx_spi_received_e (*protocolDataReceivedFnPtr)(uint8_t *payload);
typedef void (*protocolSetRcDataFromPayloadPtr)(uint16_t *rcData, const uint8_t *payload); typedef void (*protocolSetRcDataFromPayloadFnPtr)(uint16_t *rcData, const uint8_t *payload);
static protocolInitPtr protocolInit; static protocolInitFnPtr protocolInit;
static protocolDataReceivedPtr protocolDataReceived; static protocolDataReceivedFnPtr protocolDataReceived;
static protocolSetRcDataFromPayloadPtr protocolSetRcDataFromPayload; static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
{ {
@ -69,38 +69,38 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
switch (protocol) { switch (protocol) {
default: default:
#ifdef USE_RX_V202 #ifdef USE_RX_V202
case NRF24RX_V202_250K: case RX_SPI_NRF24_V202_250K:
case NRF24RX_V202_1M: case RX_SPI_NRF24_V202_1M:
protocolInit = v202Nrf24Init; protocolInit = v202Nrf24Init;
protocolDataReceived = v202Nrf24DataReceived; protocolDataReceived = v202Nrf24DataReceived;
protocolSetRcDataFromPayload = v202Nrf24SetRcDataFromPayload; protocolSetRcDataFromPayload = v202Nrf24SetRcDataFromPayload;
break; break;
#endif #endif
#ifdef USE_RX_SYMA #ifdef USE_RX_SYMA
case NRF24RX_SYMA_X: case RX_SPI_NRF24_SYMA_X:
case NRF24RX_SYMA_X5C: case RX_SPI_NRF24_SYMA_X5C:
protocolInit = symaNrf24Init; protocolInit = symaNrf24Init;
protocolDataReceived = symaNrf24DataReceived; protocolDataReceived = symaNrf24DataReceived;
protocolSetRcDataFromPayload = symaNrf24SetRcDataFromPayload; protocolSetRcDataFromPayload = symaNrf24SetRcDataFromPayload;
break; break;
#endif #endif
#ifdef USE_RX_CX10 #ifdef USE_RX_CX10
case NRF24RX_CX10: case RX_SPI_NRF24_CX10:
case NRF24RX_CX10A: case RX_SPI_NRF24_CX10A:
protocolInit = cx10Nrf24Init; protocolInit = cx10Nrf24Init;
protocolDataReceived = cx10Nrf24DataReceived; protocolDataReceived = cx10Nrf24DataReceived;
protocolSetRcDataFromPayload = cx10Nrf24SetRcDataFromPayload; protocolSetRcDataFromPayload = cx10Nrf24SetRcDataFromPayload;
break; break;
#endif #endif
#ifdef USE_RX_H8_3D #ifdef USE_RX_H8_3D
case NRF24RX_H8_3D: case RX_SPI_NRF24_H8_3D:
protocolInit = h8_3dNrf24Init; protocolInit = h8_3dNrf24Init;
protocolDataReceived = h8_3dNrf24DataReceived; protocolDataReceived = h8_3dNrf24DataReceived;
protocolSetRcDataFromPayload = h8_3dNrf24SetRcDataFromPayload; protocolSetRcDataFromPayload = h8_3dNrf24SetRcDataFromPayload;
break; break;
#endif #endif
#ifdef USE_RX_INAV #ifdef USE_RX_INAV
case NRF24RX_INAV: case RX_SPI_NRF24_INAV:
protocolInit = inavNrf24Init; protocolInit = inavNrf24Init;
protocolDataReceived = inavNrf24DataReceived; protocolDataReceived = inavNrf24DataReceived;
protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload; protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload;

18
src/main/rx/rx_spi.h
View file

@ -21,15 +21,15 @@
#include <stdint.h> #include <stdint.h>
typedef enum { typedef enum {
NRF24RX_V202_250K = 0, RX_SPI_NRF24_V202_250K = 0,
NRF24RX_V202_1M, RX_SPI_NRF24_V202_1M,
NRF24RX_SYMA_X, RX_SPI_NRF24_SYMA_X,
NRF24RX_SYMA_X5C, RX_SPI_NRF24_SYMA_X5C,
NRF24RX_CX10, RX_SPI_NRF24_CX10,
NRF24RX_CX10A, RX_SPI_NRF24_CX10A,
NRF24RX_H8_3D, RX_SPI_NRF24_H8_3D,
NRF24RX_INAV, RX_SPI_NRF24_INAV,
NRF24RX_PROTOCOL_COUNT RX_SPI_PROTOCOL_COUNT
} rx_spi_protocol_e; } rx_spi_protocol_e;
typedef enum { typedef enum {

49
src/main/rx/spektrum.c
View file

@ -18,8 +18,9 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>
#include "string.h"
#include "platform.h" #include "platform.h"
#include "common/maths.h"
#ifdef SERIAL_RX #ifdef SERIAL_RX
@ -41,13 +42,14 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/spektrum.h" #include "rx/spektrum.h"
#include "config/feature.h"
// driver for spektrum satellite receiver / sbus // driver for spektrum satellite receiver / sbus
#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12 #define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
#define SPEKTRUM_2048_CHANNEL_COUNT 12 #define SPEKTRUM_2048_CHANNEL_COUNT 12
#define SPEKTRUM_1024_CHANNEL_COUNT 7 #define SPEKTRUM_1024_CHANNEL_COUNT 7
#define SPEK_FRAME_SIZE 16
#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000 #define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
#define SPEKTRUM_BAUDRATE 115200 #define SPEKTRUM_BAUDRATE 115200
@ -70,6 +72,7 @@ static uint8_t rssi_channel; // Stores the RX RSSI channel.
static volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
static rxRuntimeConfig_t *rxRuntimeConfigPtr; static rxRuntimeConfig_t *rxRuntimeConfigPtr;
static serialPort_t *serialPort;
#ifdef SPEKTRUM_BIND #ifdef SPEKTRUM_BIND
static IO_t BindPin = DEFIO_IO(NONE); static IO_t BindPin = DEFIO_IO(NONE);
@ -78,6 +81,10 @@ static IO_t BindPin = DEFIO_IO(NONE);
static IO_t BindPlug = DEFIO_IO(NONE); static IO_t BindPlug = DEFIO_IO(NONE);
#endif #endif
static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
static uint8_t telemetryBufLen = 0;
void srxlRxSendTelemetryData(void);
// Receive ISR callback // Receive ISR callback
static void spektrumDataReceive(uint16_t c) static void spektrumDataReceive(uint16_t c)
@ -146,6 +153,9 @@ static uint8_t spektrumFrameStatus(void)
} }
} }
if (telemetryBufLen) {
srxlRxSendTelemetryData();
}
return RX_FRAME_COMPLETE; return RX_FRAME_COMPLETE;
} }
@ -275,15 +285,22 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
#ifdef TELEMETRY #ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig); bool portShared = telemetryCheckRxPortShared(portConfig);
bool srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared && rxConfig->serialrx_provider == SERIALRX_SPEKTRUM2048);
#else #else
bool srxlEnabled = false;
bool portShared = false; bool portShared = false;
#endif #endif
serialPort_t *spektrumPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, spektrumDataReceive, SPEKTRUM_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED); serialPort = openSerialPort(portConfig->identifier,
FUNCTION_RX_SERIAL,
spektrumDataReceive,
SPEKTRUM_BAUDRATE,
portShared || srxlEnabled ? MODE_RXTX : MODE_RX,
SERIAL_NOT_INVERTED | (srxlEnabled ? SERIAL_BIDIR : 0));
#ifdef TELEMETRY #ifdef TELEMETRY
if (portShared) { if (portShared) {
telemetrySharedPort = spektrumPort; telemetrySharedPort = serialPort;
} }
#endif #endif
@ -292,7 +309,29 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
rssi_channel = 0; rssi_channel = 0;
} }
return spektrumPort != NULL; return serialPort != NULL;
} }
void srxlRxWriteTelemetryData(const void *data, int len)
{
len = MIN(len, (int)sizeof(telemetryBuf));
memcpy(telemetryBuf, data, len);
telemetryBufLen = len;
}
void srxlRxSendTelemetryData(void)
{
// if there is telemetry data to write
if (telemetryBufLen > 0) {
serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
telemetryBufLen = 0; // reset telemetry buffer
}
}
bool srxlRxIsActive(void)
{
return serialPort != NULL;
}
#endif // SERIAL_RX #endif // SERIAL_RX

7
src/main/rx/spektrum.h
View file

@ -20,5 +20,12 @@
#define SPEKTRUM_SAT_BIND_DISABLED 0 #define SPEKTRUM_SAT_BIND_DISABLED 0
#define SPEKTRUM_SAT_BIND_MAX 10 #define SPEKTRUM_SAT_BIND_MAX 10
#define SPEK_FRAME_SIZE 16
#define SRXL_FRAME_OVERHEAD 5
#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD)
void spektrumBind(rxConfig_t *rxConfig); void spektrumBind(rxConfig_t *rxConfig);
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
void srxlRxWriteTelemetryData(const void *data, int len);
bool srxlRxIsActive(void);

27
src/main/rx/xbus.c
View file

@ -42,7 +42,9 @@
#define XBUS_RJ01_CHANNEL_COUNT 12 #define XBUS_RJ01_CHANNEL_COUNT 12
// Frame is: ID(1 byte) + 12*channel(2 bytes) + CRC(2 bytes) = 27 // Frame is: ID(1 byte) + 12*channel(2 bytes) + CRC(2 bytes) = 27
#define XBUS_FRAME_SIZE 27 #define XBUS_FRAME_SIZE_A1 27
#define XBUS_FRAME_SIZE_A2 35
#define XBUS_RJ01_FRAME_SIZE 33 #define XBUS_RJ01_FRAME_SIZE 33
#define XBUS_RJ01_MESSAGE_LENGTH 30 #define XBUS_RJ01_MESSAGE_LENGTH 30
@ -63,7 +65,8 @@
// However, the JR XG14 that is used for test at the moment // However, the JR XG14 that is used for test at the moment
// does only use 0xA1 as its output. This is why the implementation // does only use 0xA1 as its output. This is why the implementation
// is based on these numbers only. Maybe update this in the future? // is based on these numbers only. Maybe update this in the future?
#define XBUS_START_OF_FRAME_BYTE (0xA1) #define XBUS_START_OF_FRAME_BYTE_A1 (0xA1) //12 channels
#define XBUS_START_OF_FRAME_BYTE_A2 (0xA2) //16 channels transfare, but only 12 channels use for
// Pulse length convertion from [0...4095] to µs: // Pulse length convertion from [0...4095] to µs:
// 800µs -> 0x000 // 800µs -> 0x000
@ -82,7 +85,7 @@ static uint8_t xBusProvider;
// Use max values for ram areas // Use max values for ram areas
static volatile uint8_t xBusFrame[XBUS_RJ01_FRAME_SIZE]; static volatile uint8_t xBusFrame[XBUS_FRAME_SIZE_A2]; //siz 35 for 16 channels in xbus_Mode_B
static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT]; static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT];
// The xbus mode B CRC calculations // The xbus mode B CRC calculations
@ -136,16 +139,16 @@ static void xBusUnpackModeBFrame(uint8_t offsetBytes)
uint8_t frameAddr; uint8_t frameAddr;
// Calculate on all bytes except the final two CRC bytes // Calculate on all bytes except the final two CRC bytes
for (i = 0; i < XBUS_FRAME_SIZE - 2; i++) { for (i = 0; i < xBusFrameLength - 2; i++) {
inCrc = xBusCRC16(inCrc, xBusFrame[i+offsetBytes]); inCrc = xBusCRC16(inCrc, xBusFrame[i+offsetBytes]);
} }
// Get the received CRC // Get the received CRC
crc = ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 2]) << 8; crc = ((uint16_t)xBusFrame[offsetBytes + xBusFrameLength - 2]) << 8;
crc = crc + ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 1]); crc = crc + ((uint16_t)xBusFrame[offsetBytes + xBusFrameLength - 1]);
if (crc == inCrc) { if (crc == inCrc) {
// Unpack the data, we have a valid frame // Unpack the data, we have a valid frame, only 12 channel unpack also when receive 16 channel
for (i = 0; i < xBusChannelCount; i++) { for (i = 0; i < xBusChannelCount; i++) {
frameAddr = offsetBytes + 1 + i * 2; frameAddr = offsetBytes + 1 + i * 2;
@ -224,8 +227,14 @@ static void xBusDataReceive(uint16_t c)
} }
// Check if we shall start a frame? // Check if we shall start a frame?
if ((xBusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) { if (xBusFramePosition == 0) {
if (c == XBUS_START_OF_FRAME_BYTE_A1) {
xBusDataIncoming = true; xBusDataIncoming = true;
xBusFrameLength = XBUS_FRAME_SIZE_A1; //decrease framesize (when receiver change, otherwise board must reboot)
} else if (c == XBUS_START_OF_FRAME_BYTE_A2) {//16channel packet
xBusDataIncoming = true;
xBusFrameLength = XBUS_FRAME_SIZE_A2; //increase framesize
}
} }
// Only do this if we are receiving to a frame // Only do this if we are receiving to a frame
@ -285,7 +294,7 @@ bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
xBusDataIncoming = false; xBusDataIncoming = false;
xBusFramePosition = 0; xBusFramePosition = 0;
baudRate = XBUS_BAUDRATE; baudRate = XBUS_BAUDRATE;
xBusFrameLength = XBUS_FRAME_SIZE; xBusFrameLength = XBUS_FRAME_SIZE_A1;
xBusChannelCount = XBUS_CHANNEL_COUNT; xBusChannelCount = XBUS_CHANNEL_COUNT;
xBusProvider = SERIALRX_XBUS_MODE_B; xBusProvider = SERIALRX_XBUS_MODE_B;
break; break;

9
src/main/sensors/acceleration.c
View file

@ -51,9 +51,10 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "config/feature.h" #include "config/feature.h"
@ -230,6 +231,9 @@ retry:
bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval) bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval)
{ {
memset(&acc, 0, sizeof(acc)); 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)) { if (!accDetect(&acc.dev, accelerometerConfig->acc_hardware)) {
return false; return false;
} }
@ -377,11 +381,12 @@ static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrim
acc.accSmooth[Z] -= accelerationTrims->raw[Z]; acc.accSmooth[Z] -= accelerationTrims->raw[Z];
} }
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims)
{ {
if (!acc.dev.read(&acc.dev)) { if (!acc.dev.read(&acc.dev)) {
return; return;
} }
acc.isAccelUpdatedAtLeastOnce = true;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
DEBUG_SET(DEBUG_ACCELEROMETER, axis, acc.dev.ADCRaw[axis]); DEBUG_SET(DEBUG_ACCELEROMETER, axis, acc.dev.ADCRaw[axis]);

4
src/main/sensors/acceleration.h
View file

@ -39,6 +39,7 @@ typedef struct acc_s {
accDev_t dev; accDev_t dev;
uint32_t accSamplingInterval; uint32_t accSamplingInterval;
int32_t accSmooth[XYZ_AXIS_COUNT]; int32_t accSmooth[XYZ_AXIS_COUNT];
bool isAccelUpdatedAtLeastOnce;
} acc_t; } acc_t;
extern acc_t acc; extern acc_t acc;
@ -59,13 +60,14 @@ typedef struct accelerometerConfig_s {
sensor_align_e acc_align; // acc alignment sensor_align_e acc_align; // acc alignment
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
flightDynamicsTrims_t accZero; flightDynamicsTrims_t accZero;
rollAndPitchTrims_t accelerometerTrims;
} accelerometerConfig_t; } accelerometerConfig_t;
bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime); bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime);
bool isAccelerationCalibrationComplete(void); bool isAccelerationCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims); void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims);
union flightDynamicsTrims_u; union flightDynamicsTrims_u;
void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse); void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse);
void setAccelerationFilter(uint16_t initialAccLpfCutHz); void setAccelerationFilter(uint16_t initialAccLpfCutHz);

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

@ -85,7 +85,8 @@ static void updateBatteryVoltage(void)
#ifdef USE_ESC_SENSOR #ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) {
vbatLatest = getEscSensorVbat(); escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
vbatLatest = escData.stale ? 0 : escData.voltage / 10;
if (debugMode == DEBUG_BATTERY) { if (debugMode == DEBUG_BATTERY) {
debug[0] = -1; debug[0] = -1;
} }
@ -293,9 +294,15 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
case CURRENT_SENSOR_ESC: case CURRENT_SENSOR_ESC:
#ifdef USE_ESC_SENSOR #ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) { if (feature(FEATURE_ESC_SENSOR)) {
amperageLatest = getEscSensorCurrent(); escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (!escData.stale) {
amperageLatest = escData.current;
mAhDrawn = escData.consumption;
} else {
amperageLatest = 0;
mAhDrawn = 0;
}
amperage = amperageLatest; amperage = amperageLatest;
mAhDrawn = getEscSensorConsumption();
updateConsumptionWarning(); updateConsumptionWarning();
} }

132
src/main/sensors/esc_sensor.c
View file

@ -1,3 +1,20 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>
@ -21,10 +38,10 @@
#include "sensors/battery.h" #include "sensors/battery.h"
#include "esc_sensor.h"
#include "build/debug.h" #include "build/debug.h"
#include "esc_sensor.h"
/* /*
KISS ESC TELEMETRY PROTOCOL KISS ESC TELEMETRY PROTOCOL
--------------------------- ---------------------------
@ -50,22 +67,15 @@ DEBUG INFORMATION
set debug_mode = DEBUG_ESC_TELEMETRY in cli set debug_mode = DEBUG_ESC_TELEMETRY in cli
0: current motor index requested
1: number of timeouts
2: voltage
3: current
*/ */
#ifdef USE_DSHOT #ifdef USE_DSHOT
enum {
typedef struct { DEBUG_ESC_MOTOR_INDEX = 1,
bool skipped; DEBUG_ESC_NUM_TIMEOUTS = 2,
int16_t temperature; DEBUG_ESC_TEMPERATURE = 3,
int16_t voltage; DEBUG_ESC_RPM = 3
int16_t current; };
int16_t consumption;
int16_t rpm;
} esc_telemetry_t;
typedef enum { typedef enum {
ESC_SENSOR_FRAME_PENDING = 1 << 0, // 1 ESC_SENSOR_FRAME_PENDING = 1 << 0, // 1
@ -87,7 +97,7 @@ static bool tlmFrameDone = false;
static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, }; static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, };
static uint8_t tlmFramePosition = 0; static uint8_t tlmFramePosition = 0;
static serialPort_t *escSensorPort = NULL; static serialPort_t *escSensorPort = NULL;
static esc_telemetry_t escSensorData[MAX_SUPPORTED_MOTORS]; static escSensorData_t escSensorData[MAX_SUPPORTED_MOTORS];
static uint32_t escTriggerTimestamp = -1; static uint32_t escTriggerTimestamp = -1;
static uint32_t escLastResponseTimestamp; static uint32_t escLastResponseTimestamp;
static uint8_t timeoutRetryCount = 0; static uint8_t timeoutRetryCount = 0;
@ -97,10 +107,6 @@ static uint8_t escSensorMotor = 0; // motor index
static bool escSensorEnabled = false; static bool escSensorEnabled = false;
static escSensorTriggerState_t escSensorTriggerState = ESC_SENSOR_TRIGGER_WAIT; static escSensorTriggerState_t escSensorTriggerState = ESC_SENSOR_TRIGGER_WAIT;
static int16_t escVbat = 0;
static int16_t escCurrent = 0;
static int16_t escConsumption = 0;
static void escSensorDataReceive(uint16_t c); static void escSensorDataReceive(uint16_t c);
static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed); static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed);
static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen); static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen);
@ -111,19 +117,51 @@ bool isEscSensorActive(void)
return escSensorEnabled; return escSensorEnabled;
} }
int16_t getEscSensorVbat(void) escSensorData_t getEscSensorData(uint8_t motorNumber)
{ {
return escVbat / 10; if (motorNumber < getMotorCount()) {
return escSensorData[motorNumber];
}
escSensorData_t combinedEscSensorData = {
.stale = true,
.temperature = 0,
.voltage = 0,
.current = 0,
.consumption = 0,
.rpm = 0
};
if (motorNumber == ESC_SENSOR_COMBINED) {
unsigned int activeSensors = 0;
for (int i = 0; i < getMotorCount(); i = i + 1) {
if (!escSensorData[i].stale) {
combinedEscSensorData.temperature = MAX(combinedEscSensorData.temperature, escSensorData[i].temperature);
combinedEscSensorData.voltage += escSensorData[i].voltage;
combinedEscSensorData.current += escSensorData[i].current;
combinedEscSensorData.consumption += escSensorData[i].consumption;
combinedEscSensorData.rpm += escSensorData[i].rpm;
activeSensors = activeSensors + 1;
}
}
if (activeSensors > 0) {
combinedEscSensorData.stale = false;
combinedEscSensorData.voltage = combinedEscSensorData.voltage / activeSensors;
combinedEscSensorData.rpm = combinedEscSensorData.rpm / activeSensors;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_TEMPERATURE, combinedEscSensorData.temperature);
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_RPM, combinedEscSensorData.rpm);
}
}
return combinedEscSensorData;
} }
int16_t getEscSensorCurrent(void) static void resetEscSensorData(void)
{ {
return escCurrent; for (int i; i < MAX_SUPPORTED_MOTORS; i = i + 1) {
} escSensorData[i].stale = true;
}
int16_t getEscSensorConsumption(void)
{
return escConsumption;
} }
bool escSensorInit(void) bool escSensorInit(void)
@ -142,10 +180,12 @@ bool escSensorInit(void)
escSensorEnabled = true; escSensorEnabled = true;
} }
resetEscSensorData();
return escSensorPort != NULL; return escSensorPort != NULL;
} }
void freeEscSensorPort(void) static void freeEscSensorPort(void)
{ {
closeSerialPort(escSensorPort); closeSerialPort(escSensorPort);
escSensorPort = NULL; escSensorPort = NULL;
@ -170,7 +210,7 @@ static void escSensorDataReceive(uint16_t c)
} }
} }
uint8_t escSensorFrameStatus(void) static uint8_t escSensorFrameStatus(void)
{ {
uint8_t frameStatus = ESC_SENSOR_FRAME_PENDING; uint8_t frameStatus = ESC_SENSOR_FRAME_PENDING;
uint16_t chksum, tlmsum; uint16_t chksum, tlmsum;
@ -186,7 +226,7 @@ uint8_t escSensorFrameStatus(void)
tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1]; // last byte contains CRC value tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1]; // last byte contains CRC value
if (chksum == tlmsum) { if (chksum == tlmsum) {
escSensorData[escSensorMotor].skipped = false; escSensorData[escSensorMotor].stale = false;
escSensorData[escSensorMotor].temperature = tlm[0]; escSensorData[escSensorMotor].temperature = tlm[0];
escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2]; escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2];
escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4]; escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4];
@ -219,7 +259,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
escLastResponseTimestamp = escTriggerTimestamp; escLastResponseTimestamp = escTriggerTimestamp;
} }
else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) { else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) {
DEBUG_SET(DEBUG_ESC_SENSOR, 0, escSensorMotor+1); DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor); motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
motor->requestTelemetry = true; motor->requestTelemetry = true;
@ -235,35 +275,17 @@ void escSensorProcess(timeUs_t currentTimeUs)
if (timeoutRetryCount == 4) { if (timeoutRetryCount == 4) {
// Not responding after 3 times, skip motor // Not responding after 3 times, skip motor
escSensorData[escSensorMotor].skipped = true; escSensorData[escSensorMotor].stale = true;
selectNextMotor(); selectNextMotor();
} }
DEBUG_SET(DEBUG_ESC_SENSOR, 1, ++totalRetryCount); DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_TIMEOUTS, ++totalRetryCount);
} }
// Get received frame status // Get received frame status
uint8_t state = escSensorFrameStatus(); uint8_t state = escSensorFrameStatus();
if (state == ESC_SENSOR_FRAME_COMPLETE) { if (state == ESC_SENSOR_FRAME_COMPLETE) {
// Wait until all ESCs are processed
if (escSensorMotor == getMotorCount()-1) {
escCurrent = 0;
escConsumption = 0;
escVbat = 0;
for (int i = 0; i < getMotorCount(); i++) {
if (!escSensorData[i].skipped) {
escVbat = i > 0 ? ((escVbat + escSensorData[i].voltage) / 2) : escSensorData[i].voltage;
escCurrent = escCurrent + escSensorData[i].current;
escConsumption = escConsumption + escSensorData[i].consumption;
}
}
}
DEBUG_SET(DEBUG_ESC_SENSOR, 2, escVbat);
DEBUG_SET(DEBUG_ESC_SENSOR, 3, escCurrent);
selectNextMotor(); selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
escLastResponseTimestamp = currentTimeMs; escLastResponseTimestamp = currentTimeMs;
@ -274,8 +296,8 @@ void escSensorProcess(timeUs_t currentTimeUs)
// ESCs did not respond for 10 seconds // ESCs did not respond for 10 seconds
// Disable ESC telemetry and reset voltage and current to let the use know something is wrong // Disable ESC telemetry and reset voltage and current to let the use know something is wrong
freeEscSensorPort(); freeEscSensorPort();
escVbat = 0;
escCurrent = 0; resetEscSensorData();
} }
} }

41
src/main/sensors/esc_sensor.h
View file

@ -1,10 +1,37 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once #pragma once
uint8_t escSensorFrameStatus(void); #include "common/time.h"
bool escSensorInit(void);
bool isEscSensorActive(void); typedef struct {
int16_t getEscSensorVbat(void); bool stale;
int16_t getEscSensorCurrent(void); int8_t temperature;
int16_t getEscSensorConsumption(void); int16_t voltage;
int16_t current;
int16_t consumption;
int16_t rpm;
} escSensorData_t;
bool escSensorInit(void);
void escSensorProcess(timeUs_t currentTime);
#define ESC_SENSOR_COMBINED 255
escSensorData_t getEscSensorData(uint8_t motorNumber);
void escSensorProcess(uint32_t currentTime);

5
src/main/sensors/gyro.c
View file

@ -231,8 +231,9 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
gyroConfig = gyroConfigToUse; gyroConfig = gyroConfigToUse;
memset(&gyro, 0, sizeof(gyro)); 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) #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)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig(); gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig();
mpuDetect(extiConfig); mpuDetect(&gyro.dev);
mpuReset = gyro.dev.mpuConfiguration.reset;
#endif #endif
if (!gyroDetect(&gyro.dev)) { if (!gyroDetect(&gyro.dev)) {

3
src/main/target/ALIENFLIGHTF3/config.c
View file

@ -22,10 +22,11 @@
#include "common/axis.h" #include "common/axis.h"
#include "drivers/sensor.h"
#include "drivers/compass.h" #include "drivers/compass.h"
#include "drivers/io.h"
#include "drivers/pwm_esc_detect.h" #include "drivers/pwm_esc_detect.h"
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
#include "drivers/sensor.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"

4
src/main/target/ALIENFLIGHTF4/target.h
View file

@ -69,14 +69,14 @@
//#define SDCARD_DETECT_INVERTED //#define SDCARD_DETECT_INVERTED
#define SDCARD_DETECT_PIN PB10 #define SDCARD_DETECT_PIN PB11
#define SDCARD_DETECT_EXTI_LINE EXTI_Line10 #define SDCARD_DETECT_EXTI_LINE EXTI_Line10
#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10 #define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10
#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB #define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB
#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn #define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn
#define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_INSTANCE SPI2
#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN #define SDCARD_SPI_CS_PIN PB10
// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz

4
src/main/target/BLUEJAYF4/config.c
View file

@ -20,10 +20,12 @@
#include <platform.h> #include <platform.h>
#include "blackbox/blackbox_io.h"
#include "config/config_master.h" #include "config/config_master.h"
#include "config/feature.h" #include "config/feature.h"
#include "blackbox/blackbox_io.h" #include "drivers/io.h"
#include "hardware_revision.h" #include "hardware_revision.h"

15
src/main/target/CJMCU/target.h
View file

@ -60,9 +60,6 @@
#define RX_SPI_INSTANCE SPI1 #define RX_SPI_INSTANCE SPI1
// Nordic Semiconductor uses 'CSN', STM uses 'NSS' // Nordic Semiconductor uses 'CSN', STM uses 'NSS'
#define RX_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
#define RX_IRQ_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
#define RX_CE_PIN PA4 #define RX_CE_PIN PA4
#define RX_NSS_PIN PA11 #define RX_NSS_PIN PA11
#define RX_SCK_PIN PA5 #define RX_SCK_PIN PA5
@ -81,12 +78,12 @@
#define USE_RX_INAV #define USE_RX_INAV
#define USE_RX_SYMA #define USE_RX_SYMA
#define USE_RX_V202 #define USE_RX_V202
//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_SYMA_X5 //#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_SYMA_X5
//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C //#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_SYMA_X5C
//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_INAV //#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_INAV
#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_H8_3D #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_H8_3D
//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_CX10A //#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_CX10A
//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_V202_1M //#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_V202_1M
#define DEFAULT_RX_FEATURE FEATURE_RX_SPI #define DEFAULT_RX_FEATURE FEATURE_RX_SPI
//#define TELEMETRY //#define TELEMETRY

80
src/main/target/COLIBRI_RACE/i2c_bst.c
View file

@ -26,7 +26,7 @@
#include "drivers/bus_i2c.h" #include "drivers/bus_i2c.h"
#include "drivers/gpio.h" #include "drivers/gpio.h"
#include "drivers/timer.h" #include "drivers/timer.h"
#include "drivers/pwm_rx.h" #include "drivers/rx_pwm.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/mw.h" #include "fc/mw.h"
@ -627,25 +627,25 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
break; break;
case BST_SERVO_CONFIGURATIONS: case BST_SERVO_CONFIGURATIONS:
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
bstWrite16(masterConfig.servoConf[i].min); bstWrite16(servoProfile()->servoConf[i].min);
bstWrite16(masterConfig.servoConf[i].max); bstWrite16(servoProfile()->servoConf[i].max);
bstWrite16(masterConfig.servoConf[i].middle); bstWrite16(servoProfile()->servoConf[i].middle);
bstWrite8(masterConfig.servoConf[i].rate); bstWrite8(servoProfile()->servoConf[i].rate);
bstWrite8(masterConfig.servoConf[i].angleAtMin); bstWrite8(servoProfile()->servoConf[i].angleAtMin);
bstWrite8(masterConfig.servoConf[i].angleAtMax); bstWrite8(servoProfile()->servoConf[i].angleAtMax);
bstWrite8(masterConfig.servoConf[i].forwardFromChannel); bstWrite8(servoProfile()->servoConf[i].forwardFromChannel);
bstWrite32(masterConfig.servoConf[i].reversedSources); bstWrite32(servoProfile()->servoConf[i].reversedSources);
} }
break; break;
case BST_SERVO_MIX_RULES: case BST_SERVO_MIX_RULES:
for (i = 0; i < MAX_SERVO_RULES; i++) { for (i = 0; i < MAX_SERVO_RULES; i++) {
bstWrite8(masterConfig.customServoMixer[i].targetChannel); bstWrite8(customServoMixer(i)->targetChannel);
bstWrite8(masterConfig.customServoMixer[i].inputSource); bstWrite8(customServoMixer(i)->inputSource);
bstWrite8(masterConfig.customServoMixer[i].rate); bstWrite8(customServoMixer(i)->rate);
bstWrite8(masterConfig.customServoMixer[i].speed); bstWrite8(customServoMixer(i)->speed);
bstWrite8(masterConfig.customServoMixer[i].min); bstWrite8(customServoMixer(i)->min);
bstWrite8(masterConfig.customServoMixer[i].max); bstWrite8(customServoMixer(i)->max);
bstWrite8(masterConfig.customServoMixer[i].box); bstWrite8(customServoMixer(i)->box);
} }
break; break;
#endif #endif
@ -720,7 +720,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
break; break;
case BST_MODE_RANGES: case BST_MODE_RANGES:
for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
const box_t *box = &boxes[mac->modeId]; const box_t *box = &boxes[mac->modeId];
bstWrite8(box->permanentId); bstWrite8(box->permanentId);
bstWrite8(mac->auxChannelIndex); bstWrite8(mac->auxChannelIndex);
@ -730,7 +730,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
break; break;
case BST_ADJUSTMENT_RANGES: case BST_ADJUSTMENT_RANGES:
for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i]; adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
bstWrite8(adjRange->adjustmentIndex); bstWrite8(adjRange->adjustmentIndex);
bstWrite8(adjRange->auxChannelIndex); bstWrite8(adjRange->auxChannelIndex);
bstWrite8(adjRange->range.startStep); bstWrite8(adjRange->range.startStep);
@ -837,8 +837,8 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
// Additional commands that are not compatible with MultiWii // Additional commands that are not compatible with MultiWii
case BST_ACC_TRIM: case BST_ACC_TRIM:
bstWrite16(masterConfig.accelerometerTrims.values.pitch); bstWrite16(accelerometerConfig()->accelerometerTrims.values.pitch);
bstWrite16(masterConfig.accelerometerTrims.values.roll); bstWrite16(accelerometerConfig()->accelerometerTrims.values.roll);
break; break;
case BST_UID: case BST_UID:
@ -1033,8 +1033,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
} }
} }
case BST_SET_ACC_TRIM: case BST_SET_ACC_TRIM:
masterConfig.accelerometerTrims.values.pitch = bstRead16(); accelerometerConfig()->accelerometerTrims.values.pitch = bstRead16();
masterConfig.accelerometerTrims.values.roll = bstRead16(); accelerometerConfig()->accelerometerTrims.values.roll = bstRead16();
break; break;
case BST_SET_ARMING_CONFIG: case BST_SET_ARMING_CONFIG:
armingConfig()->auto_disarm_delay = bstRead8(); armingConfig()->auto_disarm_delay = bstRead8();
@ -1056,7 +1056,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
case BST_SET_MODE_RANGE: case BST_SET_MODE_RANGE:
i = bstRead8(); i = bstRead8();
if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
i = bstRead8(); i = bstRead8();
const box_t *box = findBoxByPermenantId(i); const box_t *box = findBoxByPermenantId(i);
if (box) { if (box) {
@ -1065,7 +1065,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
mac->range.startStep = bstRead8(); mac->range.startStep = bstRead8();
mac->range.endStep = bstRead8(); mac->range.endStep = bstRead8();
useRcControlsConfig(masterConfig.modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile); useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile);
} else { } else {
ret = BST_FAILED; ret = BST_FAILED;
} }
@ -1076,7 +1076,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
case BST_SET_ADJUSTMENT_RANGE: case BST_SET_ADJUSTMENT_RANGE:
i = bstRead8(); i = bstRead8();
if (i < MAX_ADJUSTMENT_RANGE_COUNT) { if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
adjustmentRange_t *adjRange = &masterConfig.adjustmentRanges[i]; adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
i = bstRead8(); i = bstRead8();
if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
adjRange->adjustmentIndex = i; adjRange->adjustmentIndex = i;
@ -1157,14 +1157,14 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
if (i >= MAX_SUPPORTED_SERVOS) { if (i >= MAX_SUPPORTED_SERVOS) {
ret = BST_FAILED; ret = BST_FAILED;
} else { } else {
masterConfig.servoConf[i].min = bstRead16(); servoProfile()->servoConf[i].min = bstRead16();
masterConfig.servoConf[i].max = bstRead16(); servoProfile()->servoConf[i].max = bstRead16();
masterConfig.servoConf[i].middle = bstRead16(); servoProfile()->servoConf[i].middle = bstRead16();
masterConfig.servoConf[i].rate = bstRead8(); servoProfile()->servoConf[i].rate = bstRead8();
masterConfig.servoConf[i].angleAtMin = bstRead8(); servoProfile()->servoConf[i].angleAtMin = bstRead8();
masterConfig.servoConf[i].angleAtMax = bstRead8(); servoProfile()->servoConf[i].angleAtMax = bstRead8();
masterConfig.servoConf[i].forwardFromChannel = bstRead8(); servoProfile()->servoConf[i].forwardFromChannel = bstRead8();
masterConfig.servoConf[i].reversedSources = bstRead32(); servoProfile()->servoConf[i].reversedSources = bstRead32();
} }
#endif #endif
break; break;
@ -1174,13 +1174,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
if (i >= MAX_SERVO_RULES) { if (i >= MAX_SERVO_RULES) {
ret = BST_FAILED; ret = BST_FAILED;
} else { } else {
masterConfig.customServoMixer[i].targetChannel = bstRead8(); customServoMixer(i)->targetChannel = bstRead8();
masterConfig.customServoMixer[i].inputSource = bstRead8(); customServoMixer(i)->inputSource = bstRead8();
masterConfig.customServoMixer[i].rate = bstRead8(); customServoMixer(i)->rate = bstRead8();
masterConfig.customServoMixer[i].speed = bstRead8(); customServoMixer(i)->speed = bstRead8();
masterConfig.customServoMixer[i].min = bstRead8(); customServoMixer(i)->min = bstRead8();
masterConfig.customServoMixer[i].max = bstRead8(); customServoMixer(i)->max = bstRead8();
masterConfig.customServoMixer[i].box = bstRead8(); customServoMixer(i)->box = bstRead8();
loadCustomServoMixer(); loadCustomServoMixer();
} }
#endif #endif

16
src/main/target/KIWIF4/README.md Normal file
View file

@ -0,0 +1,16 @@
##KIWI F4
Available at: flyinglemon.eu
###Board information:
- CPU - STM32F405RGT6
- MPU6000 SPI gyro
- SPI Flash
- MAX7456 (NO DMA)
- Build in 5V BEC + 12V BEC + LC filter - board can be powered from main battery
- RC input: S.BUS with HW inverter, Spektrum
- Other connectors: RSSI, Current sensor, LED strip, Buzzer, Video IN/Out + LC filter
- dedicated PDB with current sensor
- Size: 36mm x 36mm

33
src/main/target/KIWIF4/target.c Normal file
View file

@ -0,0 +1,33 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/dma.h"
#include "drivers/timer.h"
#include "drivers/timer_def.h"
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 1, 1),
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1, 0),
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1, 0),
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1, 0),
DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0), // LED
};

144
src/main/target/KIWIF4/target.h Normal file
View file

@ -0,0 +1,144 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "KIWI"
#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
#define USBD_PRODUCT_STRING "KIWIF4"
#define LED0 PB5
#define LED1 PB4
#define BEEPER PA8
#define INVERTER PC0 // PC0 used as inverter select GPIO
#define INVERTER_USART USART1
// MPU6000 interrupts
#define USE_EXTI
#define MPU_INT_EXTI PC4
#define USE_MPU_DATA_READY_SIGNAL
#define MPU6000_CS_PIN PA4
#define MPU6000_SPI_INSTANCE SPI1
#define GYRO
#define USE_GYRO_SPI_MPU6000
#define GYRO_MPU6000_ALIGN CW180_DEG
#define USE_ACC_SPI_MPU6000
#define ACC_MPU6000_ALIGN CW180_DEG
#define USE_ACC_SPI_MPU6000
#define ACC_MPU6000_ALIGN CW180_DEG
#define OSD
#define USE_MAX7456
#define MAX7456_SPI_INSTANCE SPI2
#define MAX7456_SPI_CS_PIN PB12
//#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5
//#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0
//#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_CS_PIN SPI3_NSS_PIN
#define M25P16_SPI_INSTANCE SPI3
#define USE_VCP
#define VBUS_SENSING_PIN PC5
#define VBUS_SENSING_ENABLED
#define USE_UART1
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2
#define USE_UART3
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define USE_DSHOT
#define USE_SPI
#define USE_SPI_DEVICE_1
#define USE_SPI_DEVICE_2
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_NSS_PIN PA15
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PC12
/* #define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA
#define USE_I2C_PULLUP
#define I2C1_SCL PB6
#define I2C1_SDA PB7 */
#define USE_ADC
#define BOARD_HAS_VOLTAGE_DIVIDER
#define VBAT_ADC_PIN PC1
#define RSSI_ADC_PIN PC2
#define CURRENT_METER_ADC_PIN PC3
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define LED_STRIP
#define SPEKTRUM_BIND
// USART3 Rx, PB11
#define BIND_PIN PB11
#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 (BIT(2))
#define USABLE_TIMER_CHANNEL_COUNT 12
#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) )
#define CMS
#define USE_MSP_DISPLAYPORT

10
src/main/target/KIWIF4/target.mk Normal file
View file

@ -0,0 +1,10 @@
F405_TARGETS += $(TARGET)
FEATURES += VCP ONBOARDFLASH
TARGET_SRC = \
drivers/accgyro_spi_mpu6000.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_stm32f4xx.c\
drivers/max7456.c \
io/osd.c

16
src/main/target/MULTIFLITEPICO/config.c
View file

@ -57,14 +57,14 @@ void targetConfiguration(master_t *config)
config->rcControlsConfig.yaw_deadband = 2; config->rcControlsConfig.yaw_deadband = 2;
config->rcControlsConfig.deadband = 2; config->rcControlsConfig.deadband = 2;
config->modeActivationConditions[0].modeId = BOXANGLE; config->modeActivationProfile.modeActivationConditions[0].modeId = BOXANGLE;
config->modeActivationConditions[0].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; config->modeActivationProfile.modeActivationConditions[0].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT;
config->modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(900); config->modeActivationProfile.modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(900);
config->modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(1400); config->modeActivationProfile.modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(1400);
config->modeActivationConditions[1].modeId = BOXHORIZON; config->modeActivationProfile.modeActivationConditions[1].modeId = BOXHORIZON;
config->modeActivationConditions[1].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; config->modeActivationProfile.modeActivationConditions[1].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT;
config->modeActivationConditions[1].range.startStep = CHANNEL_VALUE_TO_STEP(1425); config->modeActivationProfile.modeActivationConditions[1].range.startStep = CHANNEL_VALUE_TO_STEP(1425);
config->modeActivationConditions[1].range.endStep = CHANNEL_VALUE_TO_STEP(1575); config->modeActivationProfile.modeActivationConditions[1].range.endStep = CHANNEL_VALUE_TO_STEP(1575);
config->failsafeConfig.failsafe_delay = 2; config->failsafeConfig.failsafe_delay = 2;
config->failsafeConfig.failsafe_off_delay = 0; config->failsafeConfig.failsafe_off_delay = 0;

4
src/main/target/NAZE/config.c
View file

@ -20,6 +20,10 @@
#include <platform.h> #include <platform.h>
#include "common/utils.h"
#include "drivers/io.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"

10
src/main/target/OMNIBUSF4/target.h
View file

@ -43,11 +43,17 @@
#define ACC #define ACC
#define USE_ACC_SPI_MPU6000 #define USE_ACC_SPI_MPU6000
#define GYRO_MPU6000_ALIGN CW180_DEG
#define GYRO #define GYRO
#define USE_GYRO_SPI_MPU6000 #define USE_GYRO_SPI_MPU6000
#define ACC_MPU6000_ALIGN CW180_DEG
#ifdef OMNIBUSF4SD
#define GYRO_MPU6000_ALIGN CW270_DEG
#define ACC_MPU6000_ALIGN CW270_DEG
#else
#define GYRO_MPU6000_ALIGN CW180_DEG
#define ACC_MPU6000_ALIGN CW180_DEG
#endif
// MPU6000 interrupts // MPU6000 interrupts
#define USE_EXTI #define USE_EXTI

46
src/main/target/SPRACINGF3NEO/target.c Executable file
View file

@ -0,0 +1,46 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <platform.h>
#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] = {
DEF_TIM(TIM15, CH2, PA3, TIM_USE_PPM, 1 ), // PWM1 / PPM / UART2 RX
DEF_TIM(TIM15, CH1, PA2, TIM_USE_PWM, 1 ), // PWM2
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1 ), // ESC1
DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 1 ), // ESC2
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1 ), // ESC3
DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 1 ), // ESC4
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 1 ), // ESC5
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 1 ), // ESC6
DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 1 ), // PWM3 - PB10 - *TIM2_CH3, UART3_TX (AF7)
DEF_TIM(TIM2, CH4, PB11, TIM_USE_MOTOR, 1 ), // PWM4 - PB11 - *TIM2_CH4, UART3_RX (AF7)
DEF_TIM(TIM16, CH1, PB8, TIM_USE_TRANSPONDER, 0 ),
DEF_TIM(TIM8, CH1, PA15, TIM_USE_LED, 0 ),
};

194
src/main/target/SPRACINGF3NEO/target.h Executable file
View file

@ -0,0 +1,194 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "SP3N"
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
#define LED0 PB9
#define LED1 PB2
#define BEEPER PC15
#define BEEPER_INVERTED
#define USE_EXTI
#define MPU_INT_EXTI PC13
#define EXTI15_10_CALLBACK_HANDLER_COUNT 4 // MPU_INT, SDCardDetect, OSD
#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 USE_GYRO_SPI_MPU6500
#define ACC
#define USE_ACC_SPI_MPU6500
#define ACC_MPU6500_ALIGN CW0_DEG
#define GYRO_MPU6500_ALIGN CW0_DEG
#define BARO
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define MAG
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define USB_IO
#define USE_VCP
#define USE_UART1
#define USE_UART2
#define USE_UART3
#define USE_UART4
#define USE_UART5
#define SERIAL_PORT_COUNT 6
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
#define USE_SPI
#define USE_SPI_DEVICE_1 // MPU
#define USE_SPI_DEVICE_2 // SDCard
#define USE_SPI_DEVICE_3 // External (MAX7456 & RTC6705)
#define SPI1_NSS_PIN PA4
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define SPI3_NSS_PIN PA15
#define SPI3_SCK_PIN PB3
#define SPI3_MISO_PIN PB4
#define SPI3_MOSI_PIN PB5
#define VTX
#define RTC6705_CS_GPIO GPIOF
#define RTC6705_CS_PIN GPIO_Pin_4
#define RTC6705_SPI_INSTANCE SPI3
#define RTC6705_POWER_PIN PC3
#define USE_MAX7456
#define MAX7456_SPI_INSTANCE SPI3
#define MAX7456_SPI_CS_PIN PA15
#define MAX7456_DMA_CHANNEL_TX DMA2_Channel2
#define MAX7456_DMA_CHANNEL_RX DMA2_Channel1
#define MAX7456_DMA_IRQ_HANDLER_ID DMA2_CH1_HANDLER
#define USE_SDCARD
#define USE_SDCARD_SPI2
#define SDCARD_DETECT_INVERTED
#define SDCARD_DETECT_PIN PC14
#define SDCARD_SPI_INSTANCE SPI2
#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN
// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init:
#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128
// Divide to under 25MHz for normal operation:
#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2
// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx.
#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5
#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5
#define MPU6500_CS_PIN SPI1_NSS_PIN
#define MPU6500_SPI_INSTANCE SPI1
#define BOARD_HAS_VOLTAGE_DIVIDER
#define USE_ADC
#define ADC_INSTANCE ADC1
#define VBAT_ADC_PIN PC1
#define CURRENT_METER_ADC_PIN PC2
#define RSSI_ADC_PIN PC0
#define LED_STRIP
#define USE_LED_STRIP_ON_DMA1_CHANNEL2
#define WS2811_PIN PA8
#define WS2811_TIMER TIM1
#define WS2811_DMA_CHANNEL DMA1_Channel2
#define WS2811_IRQ DMA1_Channel2_IRQn
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define WS2811_TIMER_GPIO_AF GPIO_AF_6
#define TRANSPONDER
#define TRANSPONDER_GPIO GPIOB
#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB
#define TRANSPONDER_GPIO_AF GPIO_AF_1
#define TRANSPONDER_PIN GPIO_Pin_8 // TIM16_CH1
#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8
#define TRANSPONDER_TIMER TIM16
#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16
#define TRANSPONDER_DMA_CHANNEL DMA1_Channel3
#define TRANSPONDER_IRQ DMA1_Channel3_IRQn
#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC3
#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define OSD
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_OSD)
#define BUTTONS
#define BUTTON_A_PIN PD2
#define SPEKTRUM_BIND
// USART3
#define BIND_PIN PA3
#define HARDWARE_BIND_PLUG
#define BINDPLUG_PIN PD2
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
// IO - assuming 303 in 64pin package, TODO
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
#define USABLE_TIMER_CHANNEL_COUNT 12 // 2xPPM, 6xPWM, UART3 RX/TX, LED Strip, IR.
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16))

21
src/main/target/SPRACINGF3NEO/target.mk Executable file
View file

@ -0,0 +1,21 @@
F3_TARGETS += $(TARGET)
FEATURES = VCP SDCARD
TARGET_SRC = \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6500.c \
drivers/accgyro_spi_mpu6500.c \
drivers/barometer_bmp280.c \
drivers/barometer_ms5611.c \
drivers/compass_ak8975.c \
drivers/compass_hmc5883l.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_stm32f30x.c \
drivers/serial_usb_vcp.c \
drivers/transponder_ir.c \
drivers/transponder_ir_stm32f30x.c \
drivers/max7456.c \
drivers/vtx_rtc6705.c \
io/osd.c \
io/transponder_ir.c \
io/vtx.c

10
src/main/target/common.h
View file

@ -17,6 +17,13 @@
#pragma once #pragma once
// type conversion warnings.
// -Wconversion can be turned on to enable the process of eliminating these warnings
//#pragma GCC diagnostic warning "-Wconversion"
#pragma GCC diagnostic ignored "-Wsign-conversion"
// -Wpadded can be turned on to check padding of structs
//#pragma GCC diagnostic warning "-Wpadded"
//#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons //#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons
#define DEBUG_MODE DEBUG_NONE // change this to change initial debug mode #define DEBUG_MODE DEBUG_NONE // change this to change initial debug mode
@ -25,6 +32,8 @@
#ifdef STM32F7 #ifdef STM32F7
#define STM_FAST_TARGET #define STM_FAST_TARGET
#define I2C3_OVERCLOCK true
#define I2C4_OVERCLOCK true
#endif #endif
/**************************** /****************************
@ -87,6 +96,7 @@
#define USE_DASHBOARD #define USE_DASHBOARD
#define USE_MSP_DISPLAYPORT #define USE_MSP_DISPLAYPORT
#define TELEMETRY_CRSF #define TELEMETRY_CRSF
#define TELEMETRY_SRXL
#define TELEMETRY_JETIEXBUS #define TELEMETRY_JETIEXBUS
#define TELEMETRY_MAVLINK #define TELEMETRY_MAVLINK
#define USE_RX_MSP #define USE_RX_MSP

7
src/main/target/system_stm32f7xx.c
View file

@ -197,6 +197,7 @@
|RCC_PERIPHCLK_USART3|RCC_PERIPHCLK_USART6 |RCC_PERIPHCLK_USART3|RCC_PERIPHCLK_USART6
|RCC_PERIPHCLK_UART4|RCC_PERIPHCLK_UART5 |RCC_PERIPHCLK_UART4|RCC_PERIPHCLK_UART5
|RCC_PERIPHCLK_UART7|RCC_PERIPHCLK_UART8 |RCC_PERIPHCLK_UART7|RCC_PERIPHCLK_UART8
|RCC_PERIPHCLK_I2C1|RCC_PERIPHCLK_I2C3
|RCC_PERIPHCLK_I2C2|RCC_PERIPHCLK_I2C4; |RCC_PERIPHCLK_I2C2|RCC_PERIPHCLK_I2C4;
PeriphClkInitStruct.Usart1ClockSelection = RCC_USART1CLKSOURCE_PCLK2; PeriphClkInitStruct.Usart1ClockSelection = RCC_USART1CLKSOURCE_PCLK2;
PeriphClkInitStruct.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1; PeriphClkInitStruct.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1;
@ -206,7 +207,9 @@
PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2; PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2;
PeriphClkInitStruct.Uart7ClockSelection = RCC_UART7CLKSOURCE_PCLK1; PeriphClkInitStruct.Uart7ClockSelection = RCC_UART7CLKSOURCE_PCLK1;
PeriphClkInitStruct.Uart8ClockSelection = RCC_UART8CLKSOURCE_PCLK1; PeriphClkInitStruct.Uart8ClockSelection = RCC_UART8CLKSOURCE_PCLK1;
PeriphClkInitStruct.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
PeriphClkInitStruct.I2c2ClockSelection = RCC_I2C2CLKSOURCE_PCLK1; PeriphClkInitStruct.I2c2ClockSelection = RCC_I2C2CLKSOURCE_PCLK1;
PeriphClkInitStruct.I2c3ClockSelection = RCC_I2C3CLKSOURCE_PCLK1;
PeriphClkInitStruct.I2c4ClockSelection = RCC_I2C4CLKSOURCE_PCLK1; PeriphClkInitStruct.I2c4ClockSelection = RCC_I2C4CLKSOURCE_PCLK1;
ret = HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct); ret = HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct);
if (ret != HAL_OK) if (ret != HAL_OK)
@ -268,10 +271,10 @@ void SystemInit(void)
#endif #endif
/* Enable I-Cache */ /* Enable I-Cache */
//SCB_EnableICache(); SCB_EnableICache();
/* Enable D-Cache */ /* Enable D-Cache */
//SCB_EnableDCache(); SCB_EnableDCache();
/* Configure the system clock to 216 MHz */ /* Configure the system clock to 216 MHz */
SystemClock_Config(); SystemClock_Config();

21
src/main/telemetry/frsky.c
View file

@ -30,6 +30,7 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/utils.h"
#include "config/feature.h" #include "config/feature.h"
@ -61,6 +62,10 @@
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "telemetry/frsky.h" #include "telemetry/frsky.h"
#ifdef USE_ESC_SENSOR
#include "sensors/esc_sensor.h"
#endif
static serialPort_t *frskyPort = NULL; static serialPort_t *frskyPort = NULL;
static serialPortConfig_t *portConfig; static serialPortConfig_t *portConfig;
@ -195,23 +200,33 @@ static void sendGpsAltitude(void)
static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{ {
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
sendDataHead(ID_RPM); sendDataHead(ID_RPM);
#ifdef USE_ESC_SENSOR
UNUSED(rxConfig);
UNUSED(deadband3d_throttle);
escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
serialize16(escData.stale ? 0 : escData.rpm);
#else
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
throttleForRPM = 0; throttleForRPM = 0;
serialize16(throttleForRPM); serialize16(throttleForRPM);
} else { } else {
serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER)); serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER));
} }
#endif
} }
static void sendTemperature1(void) static void sendTemperature1(void)
{ {
sendDataHead(ID_TEMPRATURE1); sendDataHead(ID_TEMPRATURE1);
#ifdef BARO #if defined(USE_ESC_SENSOR)
escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
serialize16(escData.stale ? 0 : escData.temperature);
#elif defined(BARO)
serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf
#else #else
serialize16(telemTemperature1 / 10); serialize16(telemTemperature1 / 10);

9
src/main/telemetry/hott.c
View file

@ -64,6 +64,7 @@
#include "build/debug.h" #include "build/debug.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/time.h"
#include "drivers/system.h" #include "drivers/system.h"
@ -73,6 +74,7 @@
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "sensors/barometer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/navigation.h" #include "flight/navigation.h"
@ -201,7 +203,12 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF; hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8; hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
const uint16_t hottGpsAltitude = (GPS_altitude) + HOTT_GPS_ALTITUDE_OFFSET; // GPS_altitude in m ; offset = 500 -> O m uint16_t altitude = GPS_altitude;
if (!STATE(GPS_FIX)) {
altitude = baro.BaroAlt / 100;
}
const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // GPS_altitude in m ; offset = 500 -> O m
hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF; hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
hottGPSMessage->altitude_H = hottGpsAltitude >> 8; hottGPSMessage->altitude_H = hottGpsAltitude >> 8;

270
src/main/telemetry/srxl.c Normal file
View file

@ -0,0 +1,270 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#ifdef TELEMETRY
#include "config/feature.h"
#include "build/version.h"
#include "common/streambuf.h"
#include "common/utils.h"
#include "sensors/battery.h"
#include "io/gps.h"
#include "io/serial.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "io/gps.h"
#include "flight/imu.h"
#include "rx/rx.h"
#include "rx/spektrum.h"
#include "telemetry/telemetry.h"
#include "telemetry/srxl.h"
#include "fc/config.h"
#define SRXL_CYCLETIME_US 100000 // 100ms, 10 Hz
#define SRXL_ADDRESS_FIRST 0xA5
#define SRXL_ADDRESS_SECOND 0x80
#define SRXL_PACKET_LENGTH 0x15
#define SRXL_FRAMETYPE_TELE_QOS 0x7F
#define SRXL_FRAMETYPE_TELE_RPM 0x7E
#define SRXL_FRAMETYPE_POWERBOX 0x0A
#define SRXL_FRAMETYPE_SID 0x00
static bool srxlTelemetryEnabled;
static uint16_t srxlCrc;
static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX];
#define SRXL_POLY 0x1021
static uint16_t srxlCrc16(uint16_t crc, uint8_t data)
{
crc = crc ^ data << 8;
for (int i = 0; i < 8; i++) {
if (crc & 0x8000) {
crc = crc << 1 ^ SRXL_POLY;
} else {
crc = crc << 1;
}
}
return crc;
}
static void srxlInitializeFrame(sbuf_t *dst)
{
srxlCrc = 0;
dst->ptr = srxlFrame;
dst->end = ARRAYEND(srxlFrame);
sbufWriteU8(dst, SRXL_ADDRESS_FIRST);
sbufWriteU8(dst, SRXL_ADDRESS_SECOND);
sbufWriteU8(dst, SRXL_PACKET_LENGTH);
}
static void srxlSerialize8(sbuf_t *dst, uint8_t v)
{
sbufWriteU8(dst, v);
srxlCrc = srxlCrc16(srxlCrc, v);
}
static void srxlSerialize16(sbuf_t *dst, uint16_t v)
{
// Use BigEndian format
srxlSerialize8(dst, (v >> 8));
srxlSerialize8(dst, (uint8_t)v);
}
static void srxlFinalize(sbuf_t *dst)
{
sbufWriteU16(dst, srxlCrc);
sbufSwitchToReader(dst, srxlFrame);
// write the telemetry frame to the receiver.
srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
}
/*
SRXL frame has the structure:
<0xA5><0x80><Length><16-byte telemetry packet><2 Byte CRC of payload>
The <Length> shall be 0x15 (length of the 16-byte telemetry packet + overhead).
*/
/*
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;
*/
void srxlFrameQos(sbuf_t *dst)
{
srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_QOS);
srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
srxlSerialize16(dst, 0xFFFF); // A
srxlSerialize16(dst, 0xFFFF); // B
srxlSerialize16(dst, 0xFFFF); // L
srxlSerialize16(dst, 0xFFFF); // R
srxlSerialize16(dst, 0xFFFF); // F
srxlSerialize16(dst, 0xFFFF); // H
srxlSerialize16(dst, 0xFFFF); // rxVoltage
}
/*
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;
*/
void srxlFrameRpm(sbuf_t *dst)
{
srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_RPM);
srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
srxlSerialize16(dst, 0xFFFF); // pulse leading edges
srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V
srxlSerialize16(dst, 0x7FFF); // temperature
srxlSerialize8(dst, 0xFF); // dbmA
srxlSerialize8(dst, 0xFF); // dbmB
/* unused */
srxlSerialize16(dst, 0xFFFF);
srxlSerialize16(dst, 0xFFFF);
srxlSerialize16(dst, 0xFFFF);
}
/*
typedef struct
{
UINT8 identifier; // Source device = 0x0A
UINT8 sID; // Secondary ID
UINT16 volt1; // Volts, 0.01v
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)
void srxlFramePowerBox(sbuf_t *dst)
{
srxlSerialize8(dst, SRXL_FRAMETYPE_POWERBOX);
srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat1
srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat2
srxlSerialize16(dst, amperage / 10);
srxlSerialize16(dst, 0xFFFF);
srxlSerialize16(dst, 0xFFFF); // spare
srxlSerialize16(dst, 0xFFFF); // spare
srxlSerialize8(dst, 0xFF); // spare
srxlSerialize8(dst, 0x00); // ALARMS
}
// schedule array to decide how often each type of frame is sent
#define SRXL_SCHEDULE_COUNT_MAX 3
typedef void (*srxlSchedulePtr)(sbuf_t *dst);
const srxlSchedulePtr srxlScheduleFuncs[SRXL_SCHEDULE_COUNT_MAX] = {
/* must send srxlFrameQos, Rpm and then alternating items of our own */
srxlFrameQos,
srxlFrameRpm,
srxlFramePowerBox
};
static void processSrxl(void)
{
static uint8_t srxlScheduleIndex = 0;
sbuf_t srxlPayloadBuf;
sbuf_t *dst = &srxlPayloadBuf;
srxlSchedulePtr srxlPtr = srxlScheduleFuncs[srxlScheduleIndex];
if (srxlPtr) {
srxlInitializeFrame(dst);
srxlPtr(dst);
srxlFinalize(dst);
}
srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX;
}
void initSrxlTelemetry(void)
{
// check if there is a serial port open for SRXL telemetry (ie opened by the SRXL RX)
// and feature is enabled, if so, set SRXL telemetry enabled
srxlTelemetryEnabled = srxlRxIsActive();
}
bool checkSrxlTelemetryState(void)
{
return srxlTelemetryEnabled;
}
/*
* Called periodically by the scheduler
*/
void handleSrxlTelemetry(timeUs_t currentTimeUs)
{
static uint32_t srxlLastCycleTime;
if (!srxlTelemetryEnabled) {
return;
}
// Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
if (currentTimeUs >= srxlLastCycleTime + SRXL_CYCLETIME_US) {
srxlLastCycleTime = currentTimeUs;
processSrxl();
}
}
#endif

24
src/main/telemetry/srxl.h Normal file
View file

@ -0,0 +1,24 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "common/time.h"
void initSrxlTelemetry(void);
bool checkSrxlTelemetryState(void);
void handleSrxlTelemetry(timeUs_t currentTimeUs);

10
src/main/telemetry/telemetry.c
View file

@ -47,6 +47,7 @@
#include "telemetry/jetiexbus.h" #include "telemetry/jetiexbus.h"
#include "telemetry/mavlink.h" #include "telemetry/mavlink.h"
#include "telemetry/crsf.h" #include "telemetry/crsf.h"
#include "telemetry/srxl.h"
static telemetryConfig_t *telemetryConfig; static telemetryConfig_t *telemetryConfig;
@ -78,6 +79,9 @@ void telemetryInit(void)
#ifdef TELEMETRY_CRSF #ifdef TELEMETRY_CRSF
initCrsfTelemetry(); initCrsfTelemetry();
#endif #endif
#ifdef TELEMETRY_SRXL
initSrxlTelemetry();
#endif
telemetryCheckState(); telemetryCheckState();
} }
@ -126,6 +130,9 @@ void telemetryCheckState(void)
#ifdef TELEMETRY_CRSF #ifdef TELEMETRY_CRSF
checkCrsfTelemetryState(); checkCrsfTelemetryState();
#endif #endif
#ifdef TELEMETRY_SRXL
checkSrxlTelemetryState();
#endif
} }
void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
@ -156,6 +163,9 @@ void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadb
#ifdef TELEMETRY_CRSF #ifdef TELEMETRY_CRSF
handleCrsfTelemetry(currentTime); handleCrsfTelemetry(currentTime);
#endif #endif
#ifdef TELEMETRY_SRXL
handleSrxlTelemetry(currentTime);
#endif
} }
#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT) #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)

2
src/main/vcp/hw_config.c
View file

@ -282,7 +282,7 @@ static void IntToUnicode(uint32_t value, uint8_t *pbuf, uint8_t len)
* Output : None. * Output : None.
* Return : None. * Return : None.
*******************************************************************************/ *******************************************************************************/
uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength) uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength)
{ {
/* Last transmission hasn't finished, abort */ /* Last transmission hasn't finished, abort */
if (packetSent) { if (packetSent) {

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