From fda5e0f962d920f5ad1b40082032dce21cc3850f Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 15 Jun 2017 15:14:50 +0900 Subject: [PATCH 01/40] Configurable transponder --- src/main/drivers/transponder_ir.c | 10 +--------- src/main/drivers/transponder_ir.h | 2 +- src/main/fc/.cli.c.swo | Bin 0 -> 16384 bytes src/main/fc/cli.c | 5 +++++ src/main/io/transponder_ir.c | 25 ++++++++++++++++++++++++- src/main/io/transponder_ir.h | 1 + 6 files changed, 32 insertions(+), 11 deletions(-) create mode 100644 src/main/fc/.cli.c.swo diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 9c4a4fcb3e..1b26e691a7 100644 --- a/src/main/drivers/transponder_ir.c +++ b/src/main/drivers/transponder_ir.c @@ -152,16 +152,8 @@ void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder) DMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE); } -bool transponderIrInit(const transponderProvider_e provider) +bool transponderIrInit(const ioTag_t ioTag, const transponderProvider_e provider) { - ioTag_t ioTag = IO_TAG_NONE; - for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if (timerHardware[i].usageFlags & TIM_USE_TRANSPONDER) { - ioTag = timerHardware[i].tag; - break; - } - } - if (!ioTag) { return false; } diff --git a/src/main/drivers/transponder_ir.h b/src/main/drivers/transponder_ir.h index 7f4979e9fc..fe7416a7c4 100644 --- a/src/main/drivers/transponder_ir.h +++ b/src/main/drivers/transponder_ir.h @@ -113,7 +113,7 @@ struct transponderVTable { void (*updateTransponderDMABuffer)(transponder_t *transponder, const uint8_t* transponderData); }; -bool transponderIrInit(const transponderProvider_e provider); +bool transponderIrInit(const ioTag_t ioTag, const transponderProvider_e provider); void transponderIrDisable(void); void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder); diff --git a/src/main/fc/.cli.c.swo b/src/main/fc/.cli.c.swo new file mode 100644 index 0000000000000000000000000000000000000000..df03206f51fc2498ffea6682a54e7d24e8311f7f GIT binary patch literal 16384 zcmeI2ZEPGz8OOIt32jOsY5)m==p?D)bK-jQo=`ghCvlRKY{&9TLP)}OZg=)>;=SEv zX4Wq)NdQqq5NbgJ38;ibTd5xkd;nVb7C~u003T3+wgQQww1q;$OKDTylEQy>XMJ~j z=Scks>`6c0dH*xdJoC)#?7VDs^U?As+10|v&Nu(K!i~>oVNvwLfi2f4LqOM>dKbFu^W^Jw&&nF?{}88 zRR~1{iU_>X2&}TUZQZh2=UTsREm?E(^cyu$q%R^+M4*U35rHBCMFffn6cH#QP(zAj7GW&lwx@YX)o74Z6(=X@r*NlGJW?yLH<{>Jn>Yrj7jJxDKG z_viHA8$FBP>Xq&JUo`r0lm8<*{dY#s`p1uQ`frV%_3zC$wcCHe=vn-}nbV&)dSuFf z*IU~2KWFrlMn9j^FB-jS^cQlv>8{pVqaRw;Zr@xkzKRGG5hx;1M4*U35rHBCMFffn z6cP9zjsSHn>piI7VZC0)`G2;4zxp=IdItOg+yf4R>%cGHYFST%FM>&MGq{Mg#IxWl zU;>PRQLr1_3RZ(FSdY92o&t}7AAqlehru9tH&_9_fpyX6z`fvO;2gLERKWYe25=Kt z2`*y|_A2-l_&#_Dd=_wU1dM?`px1hrupay&cno|MoCOpNf?L1}@DkRXKLd|~FM*GO zJHP=@0?WW}u&%uT9suV-0H(nd7yvzBHMoTJ^UuK};LG4n;D9O^1|_f>yoPo9)8GPl z5Ig|R01u3TjbJ5s1^Wfhf+xVYz+_!PJsoCal}`^F(~&5u-6afie_P}{av6sg9Z$3!`B**w`pHV;@P zfm5e~;N6aWiGE_|_EQ6R)1;YsH}XP0Cb(iIjpTQ-&?{}sE{k5WiEP=r8#zrz6qaNV z$lOe}C`%?d^~Xb@jIMv8SZh5G96xp$S*MuK8cc}=d+}F)eO=cCo{%#OT5M4z3uGvy zZNA&HW@2qYgxwVtwRv=xx_rQXVrYg2?HfUzdgOtgmQN7 z+PtGfOPjFcawy%7Yd&+O5X%mXzPp zV6Nb^4CBJAlWUet(6!Fpm#tEJIhi6Cq++@IZBbLynW^Azb!L{x90pWW>W&nfw=a<* z87?bKI^FKN#e$MlOIgsv;S!qbU_9%tiTO%2w?xHwfJk->VwpEQtnfyZyOEn>_u`T} zw7cXs=n_IYGISTsIF8QMWjxQksm8Lxw>sS|;7L}Q)6c#oqKCm+KSpfxt&OyFxAgY0 zBPwVz;8$uQjH3#7bN5B}bnPCQG#;ZW&fh;qOTQ`@&+vfhaHY`*13Ok2Ch~?8MJ+@O z2K4P=9z4xa3GvgJBZ1k3*EM&8s(N?3PUz>mo-s$~dH()%8qx+i->WhAUAwDispPV1 zoExQ5A^F=finX*&)yIvg-+Ik{m_j|5d1QKWxUzqwY*}m`O&=uFC_NLZ7+Vz`OnNR?wG;7s;gde&wpUZWwV zL&<~<#v~>k)F-M=k(c!dMtGoSXQY8t77`pRe{gh|2-XN^8Io~HyigFhQ)5b|`P?w* zQ6yk+h!WJXpOA4T>NJuhmJHW|@U}1_4eE$6bq5FS39mDQXSW-p`Oj`wJfsH288+{!aK)R zo$PPE-HeKtJzb+dA{irtb)}-)ZF_ERuD=$<{h_GYK6fyYWYXT-uk9QUVp^O3kr=_h(p z?&$Eu{=-9KQ$zd8Bju^%1WkIdJT*2vIY|y4nIJ=Cd}v~-ynlLRXo8GSPmCX#97g3P zQ7O2i3#`V994k*9J~BN;hQ^MQV?z@Yuy=eQ2}%f+&7vo2=k=(~QKdO51Pzp~l(x_k zH{A~~-$Jv&EK;Hi>WuX;(&eFNa&%7a`=rWrkMd%_&(ymFaUqNa2_rqZ?Iko=Am}={ zA+noWm!+*86!3mB^c4KSe_aY*Jn5QgKiFC5>OV>)H^92IvXRi0Gne;M!;<+?L$aX0)ZLPDGP<)CA6cH#QP(+}J zKoNl=0!0Lh2ow?ce~Q3*Y<+M~Z*63oBxfvab&%;X{C}X7Zc*+}_KkZ7th=n)kh^-1 zq8xBl!fuFESZ?d{pEQj=0c!of7w2t>z2UK;eIvt_p^4G**rCe7k)cDS@rmK1!(&tC z8muP?VS60=U6Cr`O6pznI<^N!c)&{QPKuKOc01N3X?%%gF+NGwwj{#=){1&F25E`j zd+8&36NB`yd9Heh3bE9~{#&4znWU%A{HP}}$vrGKJpy0LqeCO<-jOb0@tkIosr}TC z^=fyD%9%-h!X|M-Crjz>aT<+M?`?Y(G>Qg z*K%st+eb=Swu7gsAlb1T%1oSRYCPHUn_N^%I%GJ7F)7S{aH*L&%?rhB zU2SRY1xb8OC&>-~n^_kdKkWkAhvqN+Usk7*V;s;I^#{Vf^aK-?%nmv?Z8`ioTag = timerHardware[i].tag; + return; + } + } + transponderConfig->ioTag = IO_TAG_NONE; +} static bool transponderInitialised = false; static bool transponderRepeat = false; @@ -95,7 +118,7 @@ void transponderUpdate(timeUs_t currentTimeUs) void transponderInit(void) { - transponderInitialised = transponderIrInit(transponderConfig()->provider); + transponderInitialised = transponderIrInit(transponderConfig()->ioTag, transponderConfig()->provider); if (!transponderInitialised) { return; } diff --git a/src/main/io/transponder_ir.h b/src/main/io/transponder_ir.h index 155d78eb93..2b67ac0783 100644 --- a/src/main/io/transponder_ir.h +++ b/src/main/io/transponder_ir.h @@ -24,6 +24,7 @@ typedef struct transponderConfig_s { transponderProvider_e provider; uint8_t reserved; uint8_t data[9]; + ioTag_t ioTag; } transponderConfig_t; typedef struct transponderRequirement_s { From 4ab48c3351ac5c8183dce2cc8d9d7d9de6459040 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 15 Jun 2017 23:18:56 +0900 Subject: [PATCH 02/40] Use RESET_CONFIG macro --- src/main/io/transponder_ir.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/io/transponder_ir.c b/src/main/io/transponder_ir.c index f0cb0e4c8f..2a52ea6bb8 100644 --- a/src/main/io/transponder_ir.c +++ b/src/main/io/transponder_ir.c @@ -26,6 +26,7 @@ #ifdef TRANSPONDER #include "build/build_config.h" +#include "config/config_reset.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -51,21 +52,19 @@ PG_REGISTER_WITH_RESET_FN(transponderConfig_t, transponderConfig, PG_TRANSPONDER void pgResetFn_transponderConfig(transponderConfig_t *transponderConfig) { - transponderConfig_t configTemplate = { + RESET_CONFIG_2(transponderConfig_t, transponderConfig, .provider = TRANSPONDER_ILAP, .reserved = 0, .data = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0x0, 0x0, 0x0 }, // Note, this is NOT a valid transponder code, it's just for testing production hardware - }; - - memcpy(transponderConfig, &configTemplate, sizeof(*transponderConfig)); + .ioTag = IO_TAG_NONE + ); for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].usageFlags & TIM_USE_TRANSPONDER) { transponderConfig->ioTag = timerHardware[i].tag; - return; + break; } } - transponderConfig->ioTag = IO_TAG_NONE; } static bool transponderInitialised = false; From bb1ca6d57d64045666584cf83f766635c22544b9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 21 Jun 2017 02:24:51 +0900 Subject: [PATCH 03/40] Conflict resolution --- src/main/fc/cli.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 90e8476b72..97a6d5f706 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2753,6 +2753,11 @@ const cliResourceValue_t resourceTable[] = { #ifdef USE_INVERTER { OWNER_INVERTER, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagInverter[0]), SERIAL_PORT_MAX_INDEX }, #endif +#ifdef USE_I2C + { OWNER_I2C_SCL, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagScl[0]), I2CDEV_COUNT }, + { OWNER_I2C_SDA, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagSda[0]), I2CDEV_COUNT }, +#endif + { OWNER_LED, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, ioTags[0]), STATUS_LED_NUMBER }, #ifdef TRANSPONDER { OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 }, #endif From 06ad65a2f6c4f6859eeffe40d7aaf7813aa5f26b Mon Sep 17 00:00:00 2001 From: blckmn Date: Thu, 29 Jun 2017 11:19:04 +1000 Subject: [PATCH 04/40] Minor cleanup --- src/main/drivers/pwm_output.c | 8 ++++---- src/main/drivers/pwm_output.h | 2 +- src/main/drivers/rx_pwm.c | 4 ++-- src/main/drivers/timer.h | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 7d7ed4fc06..2def243011 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -42,7 +42,7 @@ static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; #ifdef BEEPER static pwmOutputPort_t beeperPwm; -static uint16_t freqBeep=0; +static uint16_t freqBeep = 0; #endif bool pwmMotorsEnabled = false; @@ -456,7 +456,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig) /* flag failure and disable ability to arm */ break; } - pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_HZ, PWM_TIMER_HZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0); + pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0); servos[servoIndex].enabled = true; } } @@ -469,7 +469,7 @@ void pwmWriteBeeper(bool onoffBeep) if(!beeperPwm.io) return; if(onoffBeep == true) { - *beeperPwm.ccr = (PWM_TIMER_HZ / freqBeep) / 2; + *beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2; beeperPwm.enabled = true; } else { *beeperPwm.ccr = 0; @@ -495,7 +495,7 @@ void beeperPwmInit(IO_t io, uint16_t frequency) IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP); #endif freqBeep = frequency; - pwmOutConfig(&beeperPwm, timer, PWM_TIMER_HZ, PWM_TIMER_HZ / freqBeep, (PWM_TIMER_HZ / freqBeep) / 2, 0); + pwmOutConfig(&beeperPwm, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0); } *beeperPwm.ccr = 0; beeperPwm.enabled = false; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 34c56c69d9..f0ef307c2a 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -77,7 +77,7 @@ typedef enum { PWM_TYPE_MAX } motorPwmProtocolTypes_e; -#define PWM_TIMER_HZ MHZ_TO_HZ(1) +#define PWM_TIMER_1MHZ MHZ_TO_HZ(1) #ifdef USE_DSHOT #define MAX_DMA_TIMERS 8 diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index 863e21e8e1..f922c83501 100644 --- a/src/main/drivers/rx_pwm.c +++ b/src/main/drivers/rx_pwm.c @@ -391,7 +391,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) IOConfigGPIO(io, IOCFG_AF_PP); #endif - timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_HZ); + timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ); timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback); timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback); timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb); @@ -448,7 +448,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig) IOConfigGPIO(io, IOCFG_AF_PP); #endif - timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_HZ); + timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ); timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback); timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback); timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb); diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 00cf4d8bc1..1127a1c45e 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -134,7 +134,7 @@ typedef enum { #define HARDWARE_TIMER_DEFINITION_COUNT 14 #endif -#define MHZ_TO_HZ(x) (x * 1000000) +#define MHZ_TO_HZ(x) ((x) * 1000000) extern const timerHardware_t timerHardware[]; extern const timerDef_t timerDefinitions[]; From e9b044274709ac7b10adad75403399b59b49a2b8 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 28 Jun 2017 16:38:07 +0900 Subject: [PATCH 05/40] Enable RSSI ADC on PA0 --- src/main/target/OMNIBUSF4/target.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 4be0de9714..0b81c29597 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -178,10 +178,9 @@ #define OLED_I2C_INSTANCE (I2CDEV_3) #define USE_ADC -#define CURRENT_METER_ADC_PIN PC1 -#define VBAT_ADC_PIN PC2 - -//#define RSSI_ADC_PIN PA0 +#define CURRENT_METER_ADC_PIN PC1 // Direct from CRNT pad (part of onboard sensor for Pro) +#define VBAT_ADC_PIN PC2 // 11:1 (10K + 1K) divider +#define RSSI_ADC_PIN PA0 // Direct from RSSI pad #define TRANSPONDER From d2c6f47afad91aee56a763019ee99cfb51ee962b Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 29 Jun 2017 16:13:23 +1200 Subject: [PATCH 06/40] Enabled 'escprog' on SPRACINGF4EVO. --- src/main/target/SPRACINGF4EVO/target.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index 0065aa4d36..14fc279bdb 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -84,9 +84,8 @@ #define UART5_TX_PIN PC12 #define UART5_RX_PIN PD2 -// TODO -// #define USE_ESCSERIAL -// #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define USE_ESCSERIAL +#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define USE_I2C #define USE_I2C_DEVICE_1 From 871cc5b2225571e41279be5990c5787357a3f68c Mon Sep 17 00:00:00 2001 From: jamming Date: Thu, 29 Jun 2017 13:34:49 +0800 Subject: [PATCH 07/40] blackbox issue changes log --- src/main/target/KAKUTEF4/target.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 8edd69b2fc..759594ea58 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -62,16 +62,16 @@ #define MAX7456_SPI_INSTANCE SPI3 #define MAX7456_SPI_CS_PIN PB14 -#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5 -#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 -#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER +//#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5 +//#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 +//#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 #define USE_FLASHFS #define USE_FLASH_M25P16 -#define USE_FLASH_TOOLS +//#define USE_FLASH_TOOLS #define USE_VCP #define VBUS_SENSING_PIN PA8 From f3b00269462a6ee92fbbea21b651001dfa9628ee Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 29 Jun 2017 15:04:05 +0900 Subject: [PATCH 08/40] Disable USE_RX_INAV --- src/main/target/CJMCU/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index e75e2a7b9b..338dc5022f 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -76,7 +76,7 @@ #define USE_RX_NRF24 #define USE_RX_CX10 #define USE_RX_H8_3D -#define USE_RX_INAV +//#define USE_RX_INAV // Temporary disabled to make some room in flash #define USE_RX_SYMA #define USE_RX_V202 //#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_SYMA_X5 From c0ce62d601cbb18134092416eade842dea848c99 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Jun 2017 13:17:58 +0100 Subject: [PATCH 09/40] Added USE_NAV build flag --- src/main/flight/navigation.c | 38 ++++++++++++++++++++++++--------- src/main/target/common_fc_pre.h | 7 +++--- 2 files changed, 31 insertions(+), 14 deletions(-) diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index c34b668195..d107939d0e 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -107,19 +107,22 @@ void navigationInit(void) #define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency #define GPS_LOW_SPEED_D_FILTER 1 // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise -static bool check_missed_wp(void); static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing); //static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing); static void GPS_calc_longitude_scaling(int32_t lat); static void GPS_calc_velocity(void); static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng); + +#ifdef USE_NAV +static bool check_missed_wp(void); static void GPS_calc_poshold(void); static void GPS_calc_nav_rate(uint16_t max_speed); static void GPS_update_crosstrack(void); static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow); +static int32_t wrap_36000(int32_t angle); +#endif static int32_t wrap_18000(int32_t error); -static int32_t wrap_36000(int32_t angle); typedef struct { int16_t last_velocity; @@ -134,7 +137,6 @@ typedef struct { static PID_PARAM posholdPID_PARAM; static PID_PARAM poshold_ratePID_PARAM; -static PID_PARAM navPID_PARAM; typedef struct { float integrator; // integrator value @@ -146,6 +148,9 @@ typedef struct { static PID posholdPID[2]; static PID poshold_ratePID[2]; + +#ifdef USE_NAV +static PID_PARAM navPID_PARAM; static PID navPID[2]; static int32_t get_P(int32_t error, PID_PARAM *pid) @@ -176,6 +181,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param) // add in derivative component return pid_param->kD * pid->derivative; } +#endif static void reset_PID(PID *pid) { @@ -197,11 +203,15 @@ static void reset_PID(PID *pid) static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read static int16_t actual_speed[2] = { 0, 0 }; static float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles +static int32_t error[2]; +#ifdef USE_NAV // The difference between the desired rate of travel and the actual rate of travel // updated after GPS read - 5-10hz static int16_t rate_error[2]; -static int32_t error[2]; +// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path +static int16_t crosstrack_error; +#endif // Currently used WP static int32_t GPS_WP[2]; @@ -217,8 +227,6 @@ static int32_t target_bearing; // deg * 100, The original angle to the next_WP when the next_WP was set // Also used to check when we pass a WP static int32_t original_target_bearing; -// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path -static int16_t crosstrack_error; //////////////////////////////////////////////////////////////////////////////// // The location of the copter in relation to home, updated every GPS read (1deg - 100) //static int32_t home_to_copter_bearing; @@ -265,10 +273,7 @@ void GPS_calculateDistanceAndDirectionToHome(void) void onGpsNewData(void) { - int axis; static uint32_t nav_loopTimer; - uint16_t speed; - if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) { return; @@ -283,7 +288,7 @@ void onGpsNewData(void) // Apply moving average filter to GPS data #if defined(GPS_FILTERING) GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH; - for (axis = 0; axis < 2; axis++) { + for (int axis = 0; axis < 2; axis++) { GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t @@ -321,6 +326,7 @@ void onGpsNewData(void) // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating GPS_calc_velocity(); +#ifdef USE_NAV if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) { // we are navigating @@ -328,6 +334,7 @@ void onGpsNewData(void) GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon); + uint16_t speed; switch (nav_mode) { case NAV_MODE_POSHOLD: // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 @@ -360,6 +367,7 @@ void onGpsNewData(void) break; } } //end of gps calcs +#endif } void GPS_reset_home_position(void) @@ -385,7 +393,9 @@ void GPS_reset_nav(void) nav[i] = 0; reset_PID(&posholdPID[i]); reset_PID(&poshold_ratePID[i]); +#ifdef USE_NAV reset_PID(&navPID[i]); +#endif } } @@ -401,10 +411,12 @@ void gpsUsePIDs(pidProfile_t *pidProfile) poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f; poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; +#ifdef USE_NAV navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f; navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.0f; navPID_PARAM.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f; navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; +#endif } // OK here is the onboard GPS code @@ -442,6 +454,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon) waypoint_speed_gov = navigationConfig()->nav_speed_min; } +#ifdef USE_NAV //////////////////////////////////////////////////////////////////////////////////// // Check if we missed the destination somehow // @@ -452,6 +465,7 @@ static bool check_missed_wp(void) temp = wrap_18000(temp); return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees } +#endif #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f #define TAN_89_99_DEGREES 5729.57795f @@ -522,6 +536,7 @@ static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, in error[LAT] = *target_lat - *gps_lat; // Y Error } +#ifdef USE_NAV //////////////////////////////////////////////////////////////////////////////////// // Calculate nav_lat and nav_lon from the x and y error and the speed // @@ -627,6 +642,7 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow) } return max_speed; } +#endif //////////////////////////////////////////////////////////////////////////////////// // Utilities @@ -640,6 +656,7 @@ static int32_t wrap_18000(int32_t error) return error; } +#ifdef USE_NAV static int32_t wrap_36000(int32_t angle) { if (angle > 36000) @@ -648,6 +665,7 @@ static int32_t wrap_36000(int32_t angle) angle += 36000; return angle; } +#endif void updateGpsStateForHomeAndHoldMode(void) { diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index a99bbb0e34..5ce914c9ea 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -115,6 +115,7 @@ #define TELEMETRY_SRXL #define USE_DASHBOARD #define USE_MSP_DISPLAYPORT +#define USE_RCSPLIT #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS #define USE_SENSOR_NAMES @@ -131,10 +132,8 @@ #endif #if (FLASH_SIZE > 256) -// Temporarily moved this here because of overflowing flash size on F3 +// Temporarily moved GPS here because of overflowing flash size on F3 #define GPS - +#define USE_NAV #define USE_UNCOMMON_MIXERS #endif - -#define USE_RCSPLIT From 31df82db2dc9bdfbc8edbc774b51cfd346fa69ce Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 19 Jun 2017 00:40:59 +1200 Subject: [PATCH 10/40] Reworked arming conditions. --- src/main/cms/cms.c | 4 +- src/main/fc/cli.c | 2 +- src/main/fc/fc_core.c | 107 ++++++++++++---------- src/main/fc/fc_core.h | 8 +- src/main/fc/fc_init.c | 1 - src/main/fc/fc_tasks.c | 2 +- src/main/fc/rc_controls.c | 15 ++- src/main/fc/runtime_config.c | 17 ++++ src/main/fc/runtime_config.h | 23 ++++- src/main/flight/failsafe.c | 7 +- src/main/io/ledstrip.c | 2 +- src/main/io/osd.c | 4 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 9 +- src/main/telemetry/smartport.c | 5 +- src/test/Makefile | 1 + src/test/unit/cms_unittest.cc | 3 + src/test/unit/flight_failsafe_unittest.cc | 37 +++----- src/test/unit/ledstrip_unittest.cc | 2 + src/test/unit/osd_unittest.cc | 3 + src/test/unit/rc_controls_unittest.cc | 4 +- 20 files changed, 149 insertions(+), 107 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 0964acca9b..fc8eb1249e 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -574,7 +574,7 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void) return; cmsInMenu = true; currentCtx = (cmsCtx_t){ &menuMain, 0, 0 }; - DISABLE_ARMING_FLAG(OK_TO_ARM); + setArmingDisabled(ARMING_DISABLED_CMS_MENU); } else { // Switch display displayPort_t *pNextDisplay = cmsDisplayPortSelectNext(); @@ -642,7 +642,7 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) systemReset(); } - ENABLE_ARMING_FLAG(OK_TO_ARM); + unsetArmingDisabled(ARMING_DISABLED_CMS_MENU); return 0; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 68fb5f4042..ea06814106 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3494,7 +3494,7 @@ void cliEnter(serialPort_t *serialPort) #endif cliPrompt(); - ENABLE_ARMING_FLAG(PREVENT_ARMING); + setArmingDisabled(ARMING_DISABLED_CLI); } void cliInit(const serialConfig_t *serialConfig) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 50e126a398..3aa78b71dd 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -128,7 +128,7 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD saveConfigAndNotify(); } -bool isCalibrating() +static bool isCalibrating() { #ifdef BARO if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) { @@ -141,35 +141,52 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } -void updateLEDs(void) +void updateArmingStatus(void) { if (ARMING_FLAG(ARMED)) { LED0_ON; } else { - if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) { - ENABLE_ARMING_FLAG(OK_TO_ARM); + if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { + setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE); + } else { + unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE); + } + + if (calculateThrottleStatus() != THROTTLE_LOW) { + setArmingDisabled(ARMING_DISABLED_THROTTLE); + } else { + unsetArmingDisabled(ARMING_DISABLED_THROTTLE); } if (!STATE(SMALL_ANGLE)) { - DISABLE_ARMING_FLAG(OK_TO_ARM); + setArmingDisabled(ARMING_DISABLED_ANGLE); + } else { + unsetArmingDisabled(ARMING_DISABLED_ANGLE); } - if (isCalibrating() || (averageSystemLoadPercent > 100)) { - warningLedFlash(); - DISABLE_ARMING_FLAG(OK_TO_ARM); + if (averageSystemLoadPercent > 100) { + setArmingDisabled(ARMING_DISABLED_LOAD); } else { - if (ARMING_FLAG(OK_TO_ARM)) { - warningLedDisable(); - } else { - warningLedFlash(); - } + unsetArmingDisabled(ARMING_DISABLED_LOAD); + } + + if (isCalibrating()) { + setArmingDisabled(ARMING_DISABLED_CALIBRATING); + } else { + unsetArmingDisabled(ARMING_DISABLED_CALIBRATING); + } + + if (isArmingDisabled()) { + warningLedFlash(); + } else { + warningLedDisable(); } warningLedUpdate(); } } -void mwDisarm(void) +void disarm(void) { armingCalibrationWasInitialised = false; @@ -186,7 +203,7 @@ void mwDisarm(void) } } -void mwArm(void) +void tryArm(void) { static bool firstArmingCalibrationWasCompleted; @@ -196,51 +213,47 @@ void mwArm(void) firstArmingCalibrationWasCompleted = true; } - if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated + updateArmingStatus(); - if (ARMING_FLAG(OK_TO_ARM)) { + if (!isArmingDisabled()) { if (ARMING_FLAG(ARMED)) { return; } - if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { - return; - } - if (!ARMING_FLAG(PREVENT_ARMING)) { #ifdef USE_DSHOT - if (!feature(FEATURE_3D)) { - //TODO: Use BOXDSHOTREVERSE here - if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { - reverseMotors = false; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); - } - } else { - reverseMotors = true; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); - } + if (!feature(FEATURE_3D)) { + //TODO: Use BOXDSHOTREVERSE here + if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { + reverseMotors = false; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); + } + } else { + reverseMotors = true; + for (unsigned index = 0; index < getMotorCount(); index++) { + pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); } } + } #endif - ENABLE_ARMING_FLAG(ARMED); - ENABLE_ARMING_FLAG(WAS_EVER_ARMED); - headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); + ENABLE_ARMING_FLAG(ARMED); + ENABLE_ARMING_FLAG(WAS_EVER_ARMED); + headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); - disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero + disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero - //beep to indicate arming + //beep to indicate arming #ifdef GPS - if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) - beeper(BEEPER_ARMING_GPS_FIX); - else - beeper(BEEPER_ARMING); -#else + if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { + beeper(BEEPER_ARMING_GPS_FIX); + } else { beeper(BEEPER_ARMING); + } +#else + beeper(BEEPER_ARMING); #endif - return; - } + return; } if (!ARMING_FLAG(ARMED)) { @@ -315,7 +328,7 @@ void processRx(timeUs_t currentTimeUs) // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) - mwDisarm(); + disarm(); } updateRSSI(currentTimeUs); @@ -364,7 +377,7 @@ void processRx(timeUs_t currentTimeUs) && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over - mwDisarm(); + disarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index baba693d5c..8bc8b42531 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -40,12 +40,12 @@ union rollAndPitchTrims_u; void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); void handleInflightCalibrationStickPosition(); -void mwDisarm(void); -void mwArm(void); +void disarm(void); +void tryArm(void); void processRx(timeUs_t currentTimeUs); -void updateLEDs(void); +void updateArmingStatus(void); void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); -bool isMotorsReversed(void); \ No newline at end of file +bool isMotorsReversed(void); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 783c0f956d..780ba0e413 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -663,7 +663,6 @@ void init(void) timerStart(); ENABLE_STATE(SMALL_ANGLE); - DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 0131892928..209a746650 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -151,7 +151,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); #endif - updateLEDs(); + updateArmingStatus(); #ifdef BARO if (sensors(SENSOR_BARO)) { diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 259cab256f..f904457039 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -144,11 +144,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) if (IS_RC_MODE_ACTIVE(BOXARM)) { rcDisarmTicks = 0; // Arming via ARM BOX - if (throttleStatus == THROTTLE_LOW) { - if (ARMING_FLAG(OK_TO_ARM)) { - mwArm(); - } - } + tryArm(); } else { // Disarming via ARM BOX @@ -156,9 +152,9 @@ void processRcStickPositions(throttleStatus_e throttleStatus) rcDisarmTicks++; if (rcDisarmTicks > 3) { if (armingConfig()->disarm_kill_switch) { - mwDisarm(); + disarm(); } else if (throttleStatus == THROTTLE_LOW) { - mwDisarm(); + disarm(); } } } @@ -173,7 +169,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) // Disarm on throttle down + yaw if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { if (ARMING_FLAG(ARMED)) - mwDisarm(); + disarm(); else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held rcDelayCommand = 0; // reset so disarm tone will repeat @@ -233,7 +229,8 @@ void processRcStickPositions(throttleStatus_e throttleStatus) if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { // Arm via YAW - mwArm(); + tryArm(); + return; } } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 889f612e15..40a92eb034 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -29,6 +29,23 @@ uint16_t flightModeFlags = 0; static uint32_t enabledSensors = 0; +static armingDisableFlags_e armingDisableFlags = 0; + +void setArmingDisabled(armingDisableFlags_e flag) +{ + armingDisableFlags = armingDisableFlags | flag; +} + +void unsetArmingDisabled(armingDisableFlags_e flag) +{ + armingDisableFlags = armingDisableFlags & ~flag; +} + +bool isArmingDisabled() +{ + return armingDisableFlags; +} + /** * Enables the given flight mode. A beep is sounded if the flight mode * has changed. Returns the new 'flightModeFlags' value. diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 6311050c92..ffa3604ffd 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -19,10 +19,8 @@ // FIXME some of these are flight modes, some of these are general status indicators typedef enum { - OK_TO_ARM = (1 << 0), - PREVENT_ARMING = (1 << 1), - ARMED = (1 << 2), - WAS_EVER_ARMED = (1 << 3) + ARMED = (1 << 0), + WAS_EVER_ARMED = (1 << 1) } armingFlag_e; extern uint8_t armingFlags; @@ -31,6 +29,23 @@ extern uint8_t armingFlags; #define ENABLE_ARMING_FLAG(mask) (armingFlags |= (mask)) #define ARMING_FLAG(mask) (armingFlags & (mask)) +typedef enum { + ARMING_DISABLED_FAILSAFE = (1 << 0), + ARMING_DISABLED_BOXFAILSAFE = (1 << 1), + ARMING_DISABLED_THROTTLE = (1 << 2), + ARMING_DISABLED_ANGLE = (1 << 3), + ARMING_DISABLED_LOAD = (1 << 4), + ARMING_DISABLED_CALIBRATING = (1 << 5), + ARMING_DISABLED_CLI = (1 << 6), + ARMING_DISABLED_CMS_MENU = (1 << 7), + ARMING_DISABLED_OSD_MENU = (1 << 8), + ARMING_DISABLED_BST = (1 << 9), +} armingDisableFlags_e; + +void setArmingDisabled(armingDisableFlags_e flag); +void unsetArmingDisabled(armingDisableFlags_e flag); +bool isArmingDisabled(void); + typedef enum { ANGLE_MODE = (1 << 0), HORIZON_MODE = (1 << 1), diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index a9c9e9c877..445d260c49 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -30,6 +30,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -261,8 +262,8 @@ void failsafeUpdateState(void) break; case FAILSAFE_LANDED: - ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link - mwDisarm(); + setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link + disarm(); failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING; reprocessState = true; @@ -274,7 +275,7 @@ void failsafeUpdateState(void) if (millis() > failsafeState.receivingRxDataPeriod) { // rx link is good now, when arming via ARM switch, it must be OFF first if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) { - DISABLE_ARMING_FLAG(PREVENT_ARMING); + unsetArmingDisabled(ARMING_DISABLED_FAILSAFE); failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 6baa47362d..4bd3b4eb6c 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -541,7 +541,7 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer) warningFlags |= 1 << WARNING_LOW_BATTERY; if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) warningFlags |= 1 << WARNING_FAILSAFE; - if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) + if (!ARMING_FLAG(ARMED) && isArmingDisabled()) warningFlags |= 1 << WARNING_ARMING_DISABLED; } *timer += HZ_TO_US(10); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 60c572b4c9..afb3ea9d7d 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1143,7 +1143,9 @@ void osdUpdate(timeUs_t currentTimeUs) #ifdef CMS // do not allow ARM if we are in menu if (displayIsGrabbed(osdDisplayPort)) { - DISABLE_ARMING_FLAG(OK_TO_ARM); + setArmingDisabled(ARMING_DISABLED_OSD_MENU); + } else { + unsetArmingDisabled(ARMING_DISABLED_OSD_MENU); } #endif } diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index ca870492eb..9c93858bd8 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -596,12 +596,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) isRebootScheduled = true; break; case BST_DISARM: - if (ARMING_FLAG(ARMED)) - mwDisarm(); - ENABLE_ARMING_FLAG(PREVENT_ARMING); + if (ARMING_FLAG(ARMED)) { + disarm(); + } + setArmingDisabled(ARMING_DISABLED_BST); break; case BST_ENABLE_ARM: - DISABLE_ARMING_FLAG(PREVENT_ARMING); + unsetArmingDisabled(ARMING_DISABLED_BST); break; case BST_SET_DEADBAND: rcControlsConfigMutable()->alt_hold_deadband = bstRead8(); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 08f85752c2..3f06e00397 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -698,10 +698,11 @@ void handleSmartPortTelemetry(void) // the Taranis seems to be able to fit 5 digits on the screen // the Taranis seems to consider this number a signed 16 bit integer - if (ARMING_FLAG(OK_TO_ARM)) + if (!isArmingDisabled()) { tmpi += 1; - if (ARMING_FLAG(PREVENT_ARMING)) + } else { tmpi += 2; + } if (ARMING_FLAG(ARMED)) tmpi += 4; diff --git a/src/test/Makefile b/src/test/Makefile index 4776ac9dfd..a30ab5cfda 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -70,6 +70,7 @@ encoding_unittest_SRC := \ flight_failsafe_unittest_SRC := \ $(USER_DIR)/common/bitarray.c \ $(USER_DIR)/fc/rc_modes.c \ + $(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/flight/failsafe.c diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc index a61f2a4dfd..c133cde63c 100644 --- a/src/test/unit/cms_unittest.cc +++ b/src/test/unit/cms_unittest.cc @@ -30,6 +30,7 @@ extern "C" { #include "target.h" #include "cms/cms.h" #include "cms/cms_types.h" + #include "fc/runtime_config.h" void cmsMenuOpen(void); long cmsMenuBack(displayPort_t *pDisplay); uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key); @@ -141,4 +142,6 @@ void saveConfigAndNotify(void) {} void stopMotors(void) {} void stopPwmAllMotors(void) {} void systemReset(void) {} +void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } +void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } } diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 5edb8c45de..219db6cbc1 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -49,7 +49,6 @@ extern "C" { #include "gtest/gtest.h" uint32_t testFeatureMask = 0; -uint16_t flightModeFlags = 0; uint16_t testMinThrottle = 0; throttleStatus_e throttleStatus = THROTTLE_HIGH; @@ -203,7 +202,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); // given failsafeOnValidDataFailed(); // set last invalid sample at current time @@ -217,7 +216,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); // given sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time @@ -230,7 +229,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly. - EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_FALSE(isArmingDisabled()); } /****************************************************************************************/ @@ -269,7 +268,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms) EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); // given failsafeOnValidDataFailed(); // set last invalid sample at current time @@ -283,7 +282,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms) EXPECT_EQ(true, failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); // given sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time @@ -296,7 +295,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms) EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly. - EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_FALSE(isArmingDisabled()); } /****************************************************************************************/ @@ -325,7 +324,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) // then EXPECT_EQ(true, failsafeIsActive()); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); @@ -342,7 +341,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) // then EXPECT_EQ(true, failsafeIsActive()); - EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_TRUE(isArmingDisabled()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); @@ -357,7 +356,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly. - EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_FALSE(isArmingDisabled()); } /****************************************************************************************/ @@ -406,14 +405,13 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected EXPECT_EQ(false, failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING)); + EXPECT_FALSE(isArmingDisabled()); } // STUBS extern "C" { int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; -uint8_t armingFlags; float rcCommand[4]; int16_t debug[DEBUG16_VALUE_COUNT]; bool isUsingSticksToArm = true; @@ -437,7 +435,7 @@ bool feature(uint32_t mask) { return (mask & testFeatureMask); } -void mwDisarm(void) { +void disarm(void) { callCounts[COUNTER_MW_DISARM]++; } @@ -445,18 +443,6 @@ void beeper(beeperMode_e mode) { UNUSED(mode); } -uint16_t enableFlightMode(flightModeFlags_e mask) -{ - flightModeFlags |= (mask); - return flightModeFlags; -} - -uint16_t disableFlightMode(flightModeFlags_e mask) -{ - flightModeFlags &= ~(mask); - return flightModeFlags; -} - uint16_t getCurrentMinthrottle(void) { return testMinThrottle; @@ -467,4 +453,5 @@ bool isUsingSticksForArming(void) return isUsingSticksToArm; } +void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); } } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 058cf2131e..7bb59caaf4 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -382,4 +382,6 @@ bool sensors(uint32_t mask) const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {}; +bool isArmingDisabled(void) { return false; } + } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 57c94cab66..de1524b111 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -574,4 +574,7 @@ extern "C" { UNUSED(pDisplay); return false; } + + void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } + void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 5cf1c57e6b..75f8e8358d 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -679,8 +679,8 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {} void handleInflightCalibrationStickPosition(void) {} bool feature(uint32_t) { return false;} bool sensors(uint32_t) { return false;} -void mwArm(void) {} -void mwDisarm(void) {} +void tryArm(void) {} +void disarm(void) {} void dashboardDisablePageCycling() {} void dashboardEnablePageCycling() {} From adee15a8064156f71e33719d198a97abe9ed68c0 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 08:51:06 +0900 Subject: [PATCH 11/40] Allow gyro-less booting --- src/main/fc/cli.c | 2 +- src/main/fc/fc_init.c | 4 ++-- src/main/fc/fc_tasks.c | 7 +++++-- src/main/fc/runtime_config.c | 5 +++++ src/main/fc/runtime_config.h | 2 ++ 5 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ea06814106..8c67fb9367 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2711,7 +2711,7 @@ static void cliStatus(char *cmdline) const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM))); cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d", constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate); - + cliPrintLinef("Arming disable flags: 0x%x", getArmingDisableFlags() & ~ARMING_DISABLED_CLI); } #ifndef SKIP_TASK_STATISTICS diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 780ba0e413..06e74a50be 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -478,8 +478,8 @@ void init(void) initBoardAlignment(boardAlignment()); if (!sensorsAutodetect()) { - // if gyro was not detected due to whatever reason, we give up now. - failureMode(FAILURE_MISSING_ACC); + // if gyro was not detected due to whatever reason, don't arm. + setArmingDisabled(ARMING_DISABLED_NO_GYRO); } systemState |= SYSTEM_STATE_SENSORS_READY; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 209a746650..5e1bb3b3b7 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -259,8 +259,11 @@ void osdSlaveTasksInit(void) void fcTasksInit(void) { schedulerInit(); - rescheduleTask(TASK_GYROPID, gyro.targetLooptime); - setTaskEnabled(TASK_GYROPID, true); + + if (sensors(SENSOR_GYRO)) { + rescheduleTask(TASK_GYROPID, gyro.targetLooptime); + setTaskEnabled(TASK_GYROPID, true); + } if (sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 40a92eb034..17bd29105b 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -46,6 +46,11 @@ bool isArmingDisabled() return armingDisableFlags; } +armingDisableFlags_e getArmingDisableFlags(void) +{ + return armingDisableFlags; +} + /** * Enables the given flight mode. A beep is sounded if the flight mode * has changed. Returns the new 'flightModeFlags' value. diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index ffa3604ffd..de2623a315 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -40,11 +40,13 @@ typedef enum { ARMING_DISABLED_CMS_MENU = (1 << 7), ARMING_DISABLED_OSD_MENU = (1 << 8), ARMING_DISABLED_BST = (1 << 9), + ARMING_DISABLED_NO_GYRO = (1 << 10), } armingDisableFlags_e; void setArmingDisabled(armingDisableFlags_e flag); void unsetArmingDisabled(armingDisableFlags_e flag); bool isArmingDisabled(void); +armingDisableFlags_e getArmingDisableFlags(void); typedef enum { ANGLE_MODE = (1 << 0), From 02e0d2e1c456312da44ec1266e8fc73b12e436bc Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 09:04:47 +0900 Subject: [PATCH 12/40] Bring no gyro flag to the top --- src/main/fc/runtime_config.h | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index de2623a315..a54f287bd3 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -29,18 +29,22 @@ extern uint8_t armingFlags; #define ENABLE_ARMING_FLAG(mask) (armingFlags |= (mask)) #define ARMING_FLAG(mask) (armingFlags & (mask)) +/* + * Arming disable flags are listed in the order of criticalness. + * (Beeper code can notify the most critical reason.) + */ typedef enum { - ARMING_DISABLED_FAILSAFE = (1 << 0), - ARMING_DISABLED_BOXFAILSAFE = (1 << 1), - ARMING_DISABLED_THROTTLE = (1 << 2), - ARMING_DISABLED_ANGLE = (1 << 3), - ARMING_DISABLED_LOAD = (1 << 4), - ARMING_DISABLED_CALIBRATING = (1 << 5), - ARMING_DISABLED_CLI = (1 << 6), - ARMING_DISABLED_CMS_MENU = (1 << 7), - ARMING_DISABLED_OSD_MENU = (1 << 8), - ARMING_DISABLED_BST = (1 << 9), - ARMING_DISABLED_NO_GYRO = (1 << 10), + ARMING_DISABLED_NO_GYRO = (1 << 0), + ARMING_DISABLED_FAILSAFE = (1 << 1), + ARMING_DISABLED_BOXFAILSAFE = (1 << 2), + ARMING_DISABLED_THROTTLE = (1 << 3), + ARMING_DISABLED_ANGLE = (1 << 4), + ARMING_DISABLED_LOAD = (1 << 5), + ARMING_DISABLED_CALIBRATING = (1 << 6), + ARMING_DISABLED_CLI = (1 << 7), + ARMING_DISABLED_CMS_MENU = (1 << 8), + ARMING_DISABLED_OSD_MENU = (1 << 9), + ARMING_DISABLED_BST = (1 << 10), } armingDisableFlags_e; void setArmingDisabled(armingDisableFlags_e flag); From bdeb004bd0861e2336352b84e467c68ebd489dca Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 14:08:40 +0900 Subject: [PATCH 13/40] Add gyro flag to MSP_STATUS_EX In detected sensor field (16-bits), lower 5-bits (bits 0-4) represents traditional sensors (ACC, BARO, MAG, GPS and SONAR). Bit 5 is an extension bit, and if on, bit 6 indicates gyro detection status. --- src/main/fc/fc_msp.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 53774dc225..7fceff3f36 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -912,7 +912,14 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #else sbufWriteU16(dst, 0); #endif - sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + + // Lower 5-bits (bits 0-4) are traditional sensor flags. + // If bit 5 is on, then bit 6 indicates gyro detection status. + uint16_t sensorBits = sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4; + sensorBits |= (1 << 5); // Extention indicator bit + sensorBits |= sensors(SENSOR_GYRO) << 6; + sbufWriteU16(dst, sensorBits); + sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits sbufWriteU8(dst, getCurrentPidProfileIndex()); sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); From 2c7970a9ed68792c343837524517dd1cf50363de Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 14:33:35 +0900 Subject: [PATCH 14/40] Revive visual indication of gyro detection failure --- src/main/drivers/system.c | 9 +++++++-- src/main/drivers/system.h | 1 + src/main/fc/fc_init.c | 3 ++- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index eee2069d21..b27a917d9d 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -164,9 +164,8 @@ void delay(uint32_t ms) #define SHORT_FLASH_DURATION 50 #define CODE_FLASH_DURATION 250 -void failureMode(failureMode_e mode) +void failureLedCode(failureMode_e mode, int codeRepeatsRemaining) { - int codeRepeatsRemaining = 10; int codeFlashesRemaining; int shortFlashesRemaining; @@ -201,6 +200,12 @@ void failureMode(failureMode_e mode) delay(1000); } +} + +void failureMode(failureMode_e mode) +{ + failureLedCode(mode, 10); + #ifdef DEBUG systemReset(); #else diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index b898c99010..4a5b83535d 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -33,6 +33,7 @@ typedef enum { } failureMode_e; // failure +void failureLedCode(failureMode_e mode, int repeatCount); void failureMode(failureMode_e mode); // bootloader/IAP diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 06e74a50be..7a4bb09c38 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -478,7 +478,8 @@ void init(void) initBoardAlignment(boardAlignment()); if (!sensorsAutodetect()) { - // if gyro was not detected due to whatever reason, don't arm. + // if gyro was not detected due to whatever reason, notify and don't arm. + failureLedCode(FAILURE_MISSING_ACC, 2); setArmingDisabled(ARMING_DISABLED_NO_GYRO); } From 1c2573c50f0626008b33f8a90e786d6fc4f0b5c3 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 14:35:22 +0900 Subject: [PATCH 15/40] MINIMAL_CLI only print the value. --- src/main/fc/cli.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 8c67fb9367..fc6851daff 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2711,7 +2711,11 @@ static void cliStatus(char *cmdline) const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM))); cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d", constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate); +#ifdef MINIMAL_CLI + cliPrintLinef("0x%x", getArmingDisableFlags() & ~ARMING_DISABLED_CLI); +#else cliPrintLinef("Arming disable flags: 0x%x", getArmingDisableFlags() & ~ARMING_DISABLED_CLI); +#endif } #ifndef SKIP_TASK_STATISTICS From 740b4a91bda2fd9d0933a95901158c697bf12048 Mon Sep 17 00:00:00 2001 From: J Blackman Date: Fri, 30 Jun 2017 20:41:17 +1000 Subject: [PATCH 16/40] Update README.md --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 06b2741d05..2fa791e7e7 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,6 @@ -![Betaflight](https://camo.githubusercontent.com/8178215d6cb90842dc95c9d437b1bdf09b2d57a7/687474703a2f2f7374617469632e726367726f7570732e6e65742f666f72756d732f6174746163686d656e74732f362f312f302f332f372f362f61393038383930302d3232382d62665f6c6f676f2e6a7067) +![BetaFlight Notice, version 3.2 will be the last version of Betaflight to support STM32F1 based flight controllers, this includes NAZE, CC3D (original) and CJMCU like flight controllers](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf3_2_notice.png) + +![BetaFlight](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf_logo.png) Betaflight is flight controller software (firmware) used to fly multi-rotor craft and fixed wing craft. From 40167bc13488540f5d646c1313ba54d5e1ca74f0 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 30 Jun 2017 20:58:19 +1000 Subject: [PATCH 17/40] Adding some padding for the synced option. --- src/main/drivers/pwm_output.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 2def243011..d065eefcee 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -313,7 +313,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 #endif /* standard PWM outputs */ - const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen)); + const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen * 2)); const uint32_t clock = timerClock(timerHardware->tim); /* used to find the desired timer frequency for max resolution */ From 42ba069a69c143c7e6d04b30500e6684d1787f85 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 22:02:40 +0900 Subject: [PATCH 18/40] Print names instead of hex value --- src/main/fc/cli.c | 11 +++++++++-- src/main/fc/runtime_config.c | 2 ++ src/main/fc/runtime_config.h | 15 +++++++++++++++ 3 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index fc6851daff..cd055811b2 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2712,9 +2712,16 @@ static void cliStatus(char *cmdline) cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d", constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate); #ifdef MINIMAL_CLI - cliPrintLinef("0x%x", getArmingDisableFlags() & ~ARMING_DISABLED_CLI); + cliPrintLinef("Arming disable flags: 0x%x", getArmingDisableFlags()); #else - cliPrintLinef("Arming disable flags: 0x%x", getArmingDisableFlags() & ~ARMING_DISABLED_CLI); + cliPrint("Arming disable flags:"); + uint16_t flags = getArmingDisableFlags(); + while (flags) { + int bitpos = ffs(flags) - 1; + flags &= ~(1 << bitpos); + cliPrintf(" %s", armingDisableFlagNames[bitpos]); + } + cliPrintLinefeed(); #endif } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 17bd29105b..3d49ed4884 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -23,6 +23,8 @@ #include "fc/runtime_config.h" #include "io/beeper.h" +const char *armingDisableFlagNames[] = { ARMING_DISBALED_FLAG_NAMES }; + uint8_t armingFlags = 0; uint8_t stateFlags = 0; uint16_t flightModeFlags = 0; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index a54f287bd3..59470af1a5 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -47,6 +47,21 @@ typedef enum { ARMING_DISABLED_BST = (1 << 10), } armingDisableFlags_e; +extern const char *armingDisableFlagNames[]; + +#define ARMING_DISBALED_FLAG_NAMES \ + "NOGYRO", \ + "FAILSAFE", \ + "BOXFAILSAFE", \ + "THROTTLE", \ + "ANGLE", \ + "LOAD", \ + "CALIB", \ + "CLI", \ + "CMS", \ + "OSD", \ + "BST", + void setArmingDisabled(armingDisableFlags_e flag); void unsetArmingDisabled(armingDisableFlags_e flag); bool isArmingDisabled(void); From ac0df42a05ef7226aad62ee4d60794d285af7851 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 22:28:04 +0900 Subject: [PATCH 19/40] Abondon the extension bit, rely on API version for switching --- src/main/fc/fc_msp.c | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7fceff3f36..97a562c854 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -912,14 +912,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #else sbufWriteU16(dst, 0); #endif - - // Lower 5-bits (bits 0-4) are traditional sensor flags. - // If bit 5 is on, then bit 6 indicates gyro detection status. - uint16_t sensorBits = sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4; - sensorBits |= (1 << 5); // Extention indicator bit - sensorBits |= sensors(SENSOR_GYRO) << 6; - sbufWriteU16(dst, sensorBits); - + sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4 | sensors(SENSOR_GYRO) << 5); sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits sbufWriteU8(dst, getCurrentPidProfileIndex()); sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); From a6355762a67924246e21ab99ca4d2764976ce687 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 22:33:51 +0900 Subject: [PATCH 20/40] Remove merge arttifact --- src/main/fc/cli.c.orig | 3509 ---------------------------------------- 1 file changed, 3509 deletions(-) delete mode 100755 src/main/fc/cli.c.orig diff --git a/src/main/fc/cli.c.orig b/src/main/fc/cli.c.orig deleted file mode 100755 index 665991e146..0000000000 --- a/src/main/fc/cli.c.orig +++ /dev/null @@ -1,3509 +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 . - */ - -#include -#include -#include -#include -#include -#include -#include - -#include "platform.h" - -// FIXME remove this for targets that don't need a CLI. Perhaps use a no-op macro when USE_CLI is not enabled -// signal that we're in cli mode -uint8_t cliMode = 0; -extern uint8_t __config_start; // configured via linker script when building binaries. -extern uint8_t __config_end; - -#ifdef USE_CLI - -#include "blackbox/blackbox.h" - -#include "build/build_config.h" -#include "build/debug.h" -#include "build/version.h" - -#include "cms/cms.h" - -#include "common/axis.h" -#include "common/color.h" -#include "common/maths.h" -#include "common/printf.h" -#include "common/typeconversion.h" -#include "common/utils.h" - -#include "config/config_eeprom.h" -#include "config/feature.h" -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" - -#include "drivers/accgyro/accgyro.h" -#include "drivers/buf_writer.h" -#include "drivers/bus_i2c.h" -#include "drivers/compass/compass.h" -#include "drivers/display.h" -#include "drivers/dma.h" -#include "drivers/flash.h" -#include "drivers/io.h" -#include "drivers/io_impl.h" -#include "drivers/inverter.h" -#include "drivers/rx_pwm.h" -#include "drivers/sdcard.h" -#include "drivers/sensor.h" -#include "drivers/serial.h" -#include "drivers/serial_escserial.h" -#include "drivers/sonar_hcsr04.h" -#include "drivers/stack_check.h" -#include "drivers/system.h" -#include "drivers/transponder_ir.h" -#include "drivers/time.h" -#include "drivers/timer.h" -#include "drivers/vcd.h" -#include "drivers/light_led.h" - -#include "fc/settings.h" -#include "fc/cli.h" -#include "fc/config.h" -#include "fc/controlrate_profile.h" -#include "fc/fc_core.h" -#include "fc/rc_adjustments.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" -#include "fc/fc_msp.h" - -#include "flight/altitude.h" -#include "flight/failsafe.h" -#include "flight/imu.h" -#include "flight/mixer.h" -#include "flight/navigation.h" -#include "flight/pid.h" -#include "flight/servos.h" - -#include "io/asyncfatfs/asyncfatfs.h" -#include "io/beeper.h" -#include "io/flashfs.h" -#include "io/displayport_max7456.h" -#include "io/displayport_msp.h" -#include "io/gimbal.h" -#include "io/gps.h" -#include "io/ledstrip.h" -#include "io/osd.h" -#include "io/serial.h" -#include "io/transponder_ir.h" -#include "io/vtx_rtc6705.h" -#include "io/vtx_control.h" - -#include "rx/rx.h" -#include "rx/spektrum.h" - -#include "scheduler/scheduler.h" - -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/battery.h" -#include "sensors/boardalignment.h" -#include "sensors/compass.h" -#include "sensors/gyro.h" -#include "sensors/sensors.h" - -#include "telemetry/frsky.h" -#include "telemetry/telemetry.h" - - -static serialPort_t *cliPort; - -#ifdef STM32F1 -#define CLI_IN_BUFFER_SIZE 128 -#define CLI_OUT_BUFFER_SIZE 64 -#else -// Space required to set array parameters -#define CLI_IN_BUFFER_SIZE 256 -#define CLI_OUT_BUFFER_SIZE 256 -#endif - -static bufWriter_t *cliWriter; -static uint8_t cliWriteBuffer[sizeof(*cliWriter) + CLI_IN_BUFFER_SIZE]; - -static char cliBuffer[CLI_OUT_BUFFER_SIZE]; -static uint32_t bufferIndex = 0; - -static bool configIsInCopy = false; - -static const char* const emptyName = "-"; - -#ifndef USE_QUAD_MIXER_ONLY -// sync this with mixerMode_e -static const char * const mixerNames[] = { - "TRI", "QUADP", "QUADX", "BI", - "GIMBAL", "Y6", "HEX6", - "FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX", - "AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4", - "HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER", - "ATAIL4", "CUSTOM", "CUSTOMAIRPLANE", "CUSTOMTRI", "QUADX1234", NULL -}; -#endif - -// sync this with features_e -static const char * const featureNames[] = { - "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", - "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", - "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", - "UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", - "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL -}; - -// sync this with rxFailsafeChannelMode_e -static const char rxFailsafeModeCharacters[] = "ahs"; - -static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT][RX_FAILSAFE_MODE_COUNT] = { - { RX_FAILSAFE_MODE_AUTO, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_INVALID }, - { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } -}; - -#if defined(USE_SENSOR_NAMES) -// sync this with sensors_e -static const char * const sensorTypeNames[] = { - "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL -}; - -#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) - -// sync with gyroSensor_e -static const char * const gyroNames[] = { - "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE" -}; - -static const char * const *sensorHardwareNames[] = { - gyroNames, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware - -}; -#endif // USE_SENSOR_NAMES - -static void cliPrint(const char *str) -{ - while (*str) { - bufWriterAppend(cliWriter, *str++); - } - bufWriterFlush(cliWriter); -} - -static void cliPrintLinefeed() -{ - cliPrint("\r\n"); -} - -static void cliPrintLine(const char *str) -{ - cliPrint(str); - cliPrintLinefeed(); -} - -#ifdef MINIMAL_CLI -#define cliPrintHashLine(str) -#else -static void cliPrintHashLine(const char *str) -{ - cliPrint("\r\n# "); - cliPrintLine(str); -} -#endif - -static void cliPutp(void *p, char ch) -{ - bufWriterAppend(p, ch); -} - -typedef enum { - DUMP_MASTER = (1 << 0), - DUMP_PROFILE = (1 << 1), - DUMP_RATES = (1 << 2), - DUMP_ALL = (1 << 3), - DO_DIFF = (1 << 4), - SHOW_DEFAULTS = (1 << 5), - HIDE_UNUSED = (1 << 6) -} dumpFlags_e; - -static void cliPrintfva(const char *format, va_list va) -{ - tfp_format(cliWriter, cliPutp, format, va); - bufWriterFlush(cliWriter); -} - -static void cliPrintLinefva(const char *format, va_list va) -{ - tfp_format(cliWriter, cliPutp, format, va); - bufWriterFlush(cliWriter); - cliPrintLinefeed(); -} - -static bool cliDumpPrintLinef(uint8_t dumpMask, bool equalsDefault, const char *format, ...) -{ - if (!((dumpMask & DO_DIFF) && equalsDefault)) { - va_list va; - va_start(va, format); - cliPrintLinefva(format, va); - va_end(va); - return true; - } else { - return false; - } -} - -static void cliWrite(uint8_t ch) -{ - bufWriterAppend(cliWriter, ch); -} - -static bool cliDefaultPrintLinef(uint8_t dumpMask, bool equalsDefault, const char *format, ...) -{ - if ((dumpMask & SHOW_DEFAULTS) && !equalsDefault) { - cliWrite('#'); - - va_list va; - va_start(va, format); - cliPrintLinefva(format, va); - va_end(va); - return true; - } else { - return false; - } -} - -static void cliPrintf(const char *format, ...) -{ - va_list va; - va_start(va, format); - cliPrintfva(format, va); - va_end(va); -} - - -static void cliPrintLinef(const char *format, ...) -{ - va_list va; - va_start(va, format); - cliPrintLinefva(format, va); - va_end(va); -} - -static void printValuePointer(const clivalue_t *var, const void *valuePointer, bool full) -{ - if ((var->type & VALUE_MODE_MASK) == MODE_ARRAY) { - for (int i = 0; i < var->config.array.length; i++) { - uint8_t value = ((uint8_t *)valuePointer)[i]; - - cliPrintf("%d", value); - if (i < var->config.array.length - 1) { - cliPrint(","); - } - } - } else { - int value = 0; - - switch (var->type & VALUE_TYPE_MASK) { - case VAR_UINT8: - value = *(uint8_t *)valuePointer; - break; - - case VAR_INT8: - value = *(int8_t *)valuePointer; - break; - - case VAR_UINT16: - case VAR_INT16: - value = *(int16_t *)valuePointer; - break; - } - - switch(var->type & VALUE_MODE_MASK) { - case MODE_DIRECT: - cliPrintf("%d", value); - if (full) { - cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); - } - break; - case MODE_LOOKUP: - cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]); - break; - } - } -} - -static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptrDefault) -{ - bool result = false; - switch (type & VALUE_TYPE_MASK) { - case VAR_UINT8: - result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; - break; - - case VAR_INT8: - result = *(int8_t *)ptr == *(int8_t *)ptrDefault; - break; - - case VAR_UINT16: - case VAR_INT16: - result = *(int16_t *)ptr == *(int16_t *)ptrDefault; - break; - } - - return result; -} - -static uint16_t getValueOffset(const clivalue_t *value) -{ - switch (value->type & VALUE_SECTION_MASK) { - case MASTER_VALUE: - return value->offset; - case PROFILE_VALUE: - return value->offset + sizeof(pidProfile_t) * getCurrentPidProfileIndex(); - case PROFILE_RATE_VALUE: - return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfileIndex(); - } - return 0; -} - -static void *getValuePointer(const clivalue_t *value) -{ - const pgRegistry_t* rec = pgFind(value->pgn); - return CONST_CAST(void *, rec->address + getValueOffset(value)); -} - -static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) -{ - const pgRegistry_t *pg = pgFind(value->pgn); -#ifdef DEBUG - if (!pg) { - cliPrintLinef("VALUE %s ERROR", value->name); - return; // if it's not found, the pgn shouldn't be in the value table! - } -#endif - - const char *format = "set %s = "; - const char *defaultFormat = "#set %s = "; - const int valueOffset = getValueOffset(value); - const bool equalsDefault = valuePtrEqualsDefault(value->type, pg->copy + valueOffset, pg->address + valueOffset); - - if (((dumpMask & DO_DIFF) == 0) || !equalsDefault) { - if (dumpMask & SHOW_DEFAULTS && !equalsDefault) { - cliPrintf(defaultFormat, value->name); - printValuePointer(value, (uint8_t*)pg->address + valueOffset, false); - cliPrintLinefeed(); - } - cliPrintf(format, value->name); - printValuePointer(value, pg->copy + valueOffset, false); - cliPrintLinefeed(); - } -} - -static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) -{ - for (uint32_t i = 0; i < valueTableEntryCount; i++) { - const clivalue_t *value = &valueTable[i]; - bufWriterFlush(cliWriter); - if ((value->type & VALUE_SECTION_MASK) == valueSection) { - dumpPgValue(value, dumpMask); - } - } -} - -static void cliPrintVar(const clivalue_t *var, bool full) -{ - const void *ptr = getValuePointer(var); - - printValuePointer(var, ptr, full); -} - -static void cliPrintVarRange(const clivalue_t *var) -{ - switch (var->type & VALUE_MODE_MASK) { - case (MODE_DIRECT): { - cliPrintLinef("Allowed range: %d - %d", var->config.minmax.min, var->config.minmax.max); - } - break; - case (MODE_LOOKUP): { - const lookupTableEntry_t *tableEntry = &lookupTables[var->config.lookup.tableIndex]; - cliPrint("Allowed values:"); - for (uint32_t i = 0; i < tableEntry->valueCount ; i++) { - if (i > 0) - cliPrint(","); - cliPrintf(" %s", tableEntry->values[i]); - } - cliPrintLinefeed(); - } - break; - case (MODE_ARRAY): { - cliPrintLinef("Array length: %d", var->config.array.length); - } - - break; - } -} - -static void cliSetVar(const clivalue_t *var, const int16_t value) -{ - void *ptr = getValuePointer(var); - - switch (var->type & VALUE_TYPE_MASK) { - case VAR_UINT8: - *(uint8_t *)ptr = value; - break; - - case VAR_INT8: - *(int8_t *)ptr = value; - break; - - case VAR_UINT16: - case VAR_INT16: - *(int16_t *)ptr = value; - break; - } -} - -#if defined(USE_RESOURCE_MGMT) && !defined(MINIMAL_CLI) -static void cliRepeat(char ch, uint8_t len) -{ - for (int i = 0; i < len; i++) { - bufWriterAppend(cliWriter, ch); - } - cliPrintLinefeed(); -} -#endif - -static void cliPrompt(void) -{ - cliPrint("\r\n# "); -} - -static void cliShowParseError(void) -{ - cliPrintLine("Parse error"); -} - -static void cliShowArgumentRangeError(char *name, int min, int max) -{ - cliPrintLinef("%s not between %d and %d", name, min, max); -} - -static const char *nextArg(const char *currentArg) -{ - const char *ptr = strchr(currentArg, ' '); - while (ptr && *ptr == ' ') { - ptr++; - } - - return ptr; -} - -static const char *processChannelRangeArgs(const char *ptr, channelRange_t *range, uint8_t *validArgumentCount) -{ - for (uint32_t argIndex = 0; argIndex < 2; argIndex++) { - ptr = nextArg(ptr); - if (ptr) { - int val = atoi(ptr); - val = CHANNEL_VALUE_TO_STEP(val); - if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) { - if (argIndex == 0) { - range->startStep = val; - } else { - range->endStep = val; - } - (*validArgumentCount)++; - } - } - } - - return ptr; -} - -// Check if a string's length is zero -static bool isEmpty(const char *string) -{ - return (string == NULL || *string == '\0') ? true : false; -} - -static void printRxFailsafe(uint8_t dumpMask, const rxFailsafeChannelConfig_t *rxFailsafeChannelConfigs, const rxFailsafeChannelConfig_t *defaultRxFailsafeChannelConfigs) -{ - // print out rxConfig failsafe settings - for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { - const rxFailsafeChannelConfig_t *channelFailsafeConfig = &rxFailsafeChannelConfigs[channel]; - const rxFailsafeChannelConfig_t *defaultChannelFailsafeConfig = &defaultRxFailsafeChannelConfigs[channel]; - const bool equalsDefault = channelFailsafeConfig->mode == defaultChannelFailsafeConfig->mode - && channelFailsafeConfig->step == defaultChannelFailsafeConfig->step; - const bool requireValue = channelFailsafeConfig->mode == RX_FAILSAFE_MODE_SET; - if (requireValue) { - const char *format = "rxfail %u %c %d"; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - channel, - rxFailsafeModeCharacters[defaultChannelFailsafeConfig->mode], - RXFAIL_STEP_TO_CHANNEL_VALUE(defaultChannelFailsafeConfig->step) - ); - cliDumpPrintLinef(dumpMask, equalsDefault, format, - channel, - rxFailsafeModeCharacters[channelFailsafeConfig->mode], - RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step) - ); - } else { - const char *format = "rxfail %u %c"; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - channel, - rxFailsafeModeCharacters[defaultChannelFailsafeConfig->mode] - ); - cliDumpPrintLinef(dumpMask, equalsDefault, format, - channel, - rxFailsafeModeCharacters[channelFailsafeConfig->mode] - ); - } - } -} - -static void cliRxFailsafe(char *cmdline) -{ - uint8_t channel; - char buf[3]; - - if (isEmpty(cmdline)) { - // print out rxConfig failsafe settings - for (channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { - cliRxFailsafe(itoa(channel, buf, 10)); - } - } else { - const char *ptr = cmdline; - channel = atoi(ptr++); - if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)) { - - rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigsMutable(channel); - - const rxFailsafeChannelType_e type = (channel < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_TYPE_FLIGHT : RX_FAILSAFE_TYPE_AUX; - rxFailsafeChannelMode_e mode = channelFailsafeConfig->mode; - bool requireValue = channelFailsafeConfig->mode == RX_FAILSAFE_MODE_SET; - - ptr = nextArg(ptr); - if (ptr) { - const char *p = strchr(rxFailsafeModeCharacters, *(ptr)); - if (p) { - const uint8_t requestedMode = p - rxFailsafeModeCharacters; - mode = rxFailsafeModesTable[type][requestedMode]; - } else { - mode = RX_FAILSAFE_MODE_INVALID; - } - if (mode == RX_FAILSAFE_MODE_INVALID) { - cliShowParseError(); - return; - } - - requireValue = mode == RX_FAILSAFE_MODE_SET; - - ptr = nextArg(ptr); - if (ptr) { - if (!requireValue) { - cliShowParseError(); - return; - } - uint16_t value = atoi(ptr); - value = CHANNEL_VALUE_TO_RXFAIL_STEP(value); - if (value > MAX_RXFAIL_RANGE_STEP) { - cliPrintLine("Value out of range"); - return; - } - - channelFailsafeConfig->step = value; - } else if (requireValue) { - cliShowParseError(); - return; - } - channelFailsafeConfig->mode = mode; - } - - char modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfig->mode]; - - // double use of cliPrintf below - // 1. acknowledge interpretation on command, - // 2. query current setting on single item, - - if (requireValue) { - cliPrintLinef("rxfail %u %c %d", - channel, - modeCharacter, - RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step) - ); - } else { - cliPrintLinef("rxfail %u %c", - channel, - modeCharacter - ); - } - } else { - cliShowArgumentRangeError("channel", 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1); - } - } -} - -static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActivationConditions, const modeActivationCondition_t *defaultModeActivationConditions) -{ - const char *format = "aux %u %u %u %u %u"; - // print out aux channel settings - for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - const modeActivationCondition_t *mac = &modeActivationConditions[i]; - bool equalsDefault = false; - if (defaultModeActivationConditions) { - const modeActivationCondition_t *macDefault = &defaultModeActivationConditions[i]; - equalsDefault = mac->modeId == macDefault->modeId - && mac->auxChannelIndex == macDefault->auxChannelIndex - && mac->range.startStep == macDefault->range.startStep - && mac->range.endStep == macDefault->range.endStep; - const box_t *box = findBoxByBoxId(macDefault->modeId); - if (box) { - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - i, - box->permanentId, - macDefault->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep) - ); - } - } - const box_t *box = findBoxByBoxId(mac->modeId); - if (box) { - cliDumpPrintLinef(dumpMask, equalsDefault, format, - i, - box->permanentId, - mac->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep) - ); - } - } -} - -static void cliAux(char *cmdline) -{ - int i, val = 0; - const char *ptr; - - if (isEmpty(cmdline)) { - printAux(DUMP_MASTER, modeActivationConditions(0), NULL); - } else { - ptr = cmdline; - i = atoi(ptr++); - if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { - modeActivationCondition_t *mac = modeActivationConditionsMutable(i); - uint8_t validArgumentCount = 0; - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - const box_t *box = findBoxByPermanentId(val); - if (box) { - mac->modeId = box->boxId; - validArgumentCount++; - } - } - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { - mac->auxChannelIndex = val; - validArgumentCount++; - } - } - ptr = processChannelRangeArgs(ptr, &mac->range, &validArgumentCount); - - if (validArgumentCount != 4) { - memset(mac, 0, sizeof(modeActivationCondition_t)); - } - } else { - cliShowArgumentRangeError("index", 0, MAX_MODE_ACTIVATION_CONDITION_COUNT - 1); - } - } -} - -static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, const serialConfig_t *serialConfigDefault) -{ - const char *format = "serial %d %d %ld %ld %ld %ld"; - for (uint32_t i = 0; i < SERIAL_PORT_COUNT; i++) { - if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { - continue; - }; - bool equalsDefault = false; - if (serialConfigDefault) { - equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier - && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask - && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex - && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex - && serialConfig->portConfigs[i].telemetry_baudrateIndex == serialConfigDefault->portConfigs[i].telemetry_baudrateIndex - && serialConfig->portConfigs[i].blackbox_baudrateIndex == serialConfigDefault->portConfigs[i].blackbox_baudrateIndex; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - serialConfigDefault->portConfigs[i].identifier, - serialConfigDefault->portConfigs[i].functionMask, - baudRates[serialConfigDefault->portConfigs[i].msp_baudrateIndex], - baudRates[serialConfigDefault->portConfigs[i].gps_baudrateIndex], - baudRates[serialConfigDefault->portConfigs[i].telemetry_baudrateIndex], - baudRates[serialConfigDefault->portConfigs[i].blackbox_baudrateIndex] - ); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, - serialConfig->portConfigs[i].identifier, - serialConfig->portConfigs[i].functionMask, - baudRates[serialConfig->portConfigs[i].msp_baudrateIndex], - baudRates[serialConfig->portConfigs[i].gps_baudrateIndex], - baudRates[serialConfig->portConfigs[i].telemetry_baudrateIndex], - baudRates[serialConfig->portConfigs[i].blackbox_baudrateIndex] - ); - } -} - -static void cliSerial(char *cmdline) -{ - if (isEmpty(cmdline)) { - printSerial(DUMP_MASTER, serialConfig(), NULL); - return; - } - serialPortConfig_t portConfig; - memset(&portConfig, 0 , sizeof(portConfig)); - - serialPortConfig_t *currentConfig; - - uint8_t validArgumentCount = 0; - - const char *ptr = cmdline; - - int val = atoi(ptr++); - currentConfig = serialFindPortConfiguration(val); - if (currentConfig) { - portConfig.identifier = val; - validArgumentCount++; - } - - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - portConfig.functionMask = val & 0xFFFF; - validArgumentCount++; - } - - for (int i = 0; i < 4; i ++) { - ptr = nextArg(ptr); - if (!ptr) { - break; - } - - val = atoi(ptr); - - uint8_t baudRateIndex = lookupBaudRateIndex(val); - if (baudRates[baudRateIndex] != (uint32_t) val) { - break; - } - - switch(i) { - case 0: - if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_1000000) { - continue; - } - portConfig.msp_baudrateIndex = baudRateIndex; - break; - case 1: - if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_115200) { - continue; - } - portConfig.gps_baudrateIndex = baudRateIndex; - break; - case 2: - if (baudRateIndex != BAUD_AUTO && baudRateIndex > BAUD_115200) { - continue; - } - portConfig.telemetry_baudrateIndex = baudRateIndex; - break; - case 3: - if (baudRateIndex < BAUD_19200 || baudRateIndex > BAUD_2470000) { - continue; - } - portConfig.blackbox_baudrateIndex = baudRateIndex; - break; - } - - validArgumentCount++; - } - - if (validArgumentCount < 6) { - cliShowParseError(); - return; - } - - memcpy(currentConfig, &portConfig, sizeof(portConfig)); -} - -#ifndef SKIP_SERIAL_PASSTHROUGH -static void cliSerialPassthrough(char *cmdline) -{ - if (isEmpty(cmdline)) { - cliShowParseError(); - return; - } - - int id = -1; - uint32_t baud = 0; - unsigned mode = 0; - char *saveptr; - char* tok = strtok_r(cmdline, " ", &saveptr); - int index = 0; - - while (tok != NULL) { - switch(index) { - case 0: - id = atoi(tok); - break; - case 1: - baud = atoi(tok); - break; - case 2: - if (strstr(tok, "rx") || strstr(tok, "RX")) - mode |= MODE_RX; - if (strstr(tok, "tx") || strstr(tok, "TX")) - mode |= MODE_TX; - break; - } - index++; - tok = strtok_r(NULL, " ", &saveptr); - } - - tfp_printf("Port %d ", id); - serialPort_t *passThroughPort; - serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id); - if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) { - if (!baud) { - tfp_printf("closed, specify baud.\r\n"); - return; - } - if (!mode) - mode = MODE_RXTX; - - passThroughPort = openSerialPort(id, FUNCTION_NONE, NULL, - baud, mode, - SERIAL_NOT_INVERTED); - if (!passThroughPort) { - tfp_printf("could not be opened.\r\n"); - return; - } - tfp_printf("opened, baud = %d.\r\n", baud); - } else { - passThroughPort = passThroughPortUsage->serialPort; - // If the user supplied a mode, override the port's mode, otherwise - // leave the mode unchanged. serialPassthrough() handles one-way ports. - tfp_printf("already open.\r\n"); - if (mode && passThroughPort->mode != mode) { - tfp_printf("mode changed from %d to %d.\r\n", - passThroughPort->mode, mode); - serialSetMode(passThroughPort, mode); - } - // If this port has a rx callback associated we need to remove it now. - // Otherwise no data will be pushed in the serial port buffer! - if (passThroughPort->rxCallback) { - passThroughPort->rxCallback = 0; - } - } - - tfp_printf("forwarding, power cycle to exit.\r\n"); - - serialPassthrough(cliPort, passThroughPort, NULL, NULL); -} -#endif - -static void printAdjustmentRange(uint8_t dumpMask, const adjustmentRange_t *adjustmentRanges, const adjustmentRange_t *defaultAdjustmentRanges) -{ - const char *format = "adjrange %u %u %u %u %u %u %u"; - // print out adjustment ranges channel settings - for (uint32_t i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - const adjustmentRange_t *ar = &adjustmentRanges[i]; - bool equalsDefault = false; - if (defaultAdjustmentRanges) { - const adjustmentRange_t *arDefault = &defaultAdjustmentRanges[i]; - equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex - && ar->range.startStep == arDefault->range.startStep - && ar->range.endStep == arDefault->range.endStep - && ar->adjustmentFunction == arDefault->adjustmentFunction - && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex - && ar->adjustmentIndex == arDefault->adjustmentIndex; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - i, - arDefault->adjustmentIndex, - arDefault->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(arDefault->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(arDefault->range.endStep), - arDefault->adjustmentFunction, - arDefault->auxSwitchChannelIndex - ); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, - i, - ar->adjustmentIndex, - ar->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(ar->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(ar->range.endStep), - ar->adjustmentFunction, - ar->auxSwitchChannelIndex - ); - } -} - -static void cliAdjustmentRange(char *cmdline) -{ - int i, val = 0; - const char *ptr; - - if (isEmpty(cmdline)) { - printAdjustmentRange(DUMP_MASTER, adjustmentRanges(0), NULL); - } else { - ptr = cmdline; - i = atoi(ptr++); - if (i < MAX_ADJUSTMENT_RANGE_COUNT) { - adjustmentRange_t *ar = adjustmentRangesMutable(i); - uint8_t validArgumentCount = 0; - - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - if (val >= 0 && val < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { - ar->adjustmentIndex = val; - validArgumentCount++; - } - } - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { - ar->auxChannelIndex = val; - validArgumentCount++; - } - } - - ptr = processChannelRangeArgs(ptr, &ar->range, &validArgumentCount); - - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - if (val >= 0 && val < ADJUSTMENT_FUNCTION_COUNT) { - ar->adjustmentFunction = val; - validArgumentCount++; - } - } - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { - ar->auxSwitchChannelIndex = val; - validArgumentCount++; - } - } - - if (validArgumentCount != 6) { - memset(ar, 0, sizeof(adjustmentRange_t)); - cliShowParseError(); - } - } else { - cliShowArgumentRangeError("index", 0, MAX_ADJUSTMENT_RANGE_COUNT - 1); - } - } -} - -#ifndef USE_QUAD_MIXER_ONLY -static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer, const motorMixer_t *defaultCustomMotorMixer) -{ - const char *format = "mmix %d %s %s %s %s"; - char buf0[FTOA_BUFFER_LENGTH]; - char buf1[FTOA_BUFFER_LENGTH]; - char buf2[FTOA_BUFFER_LENGTH]; - char buf3[FTOA_BUFFER_LENGTH]; - for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - if (customMotorMixer[i].throttle == 0.0f) - break; - const float thr = customMotorMixer[i].throttle; - const float roll = customMotorMixer[i].roll; - const float pitch = customMotorMixer[i].pitch; - const float yaw = customMotorMixer[i].yaw; - bool equalsDefault = false; - if (defaultCustomMotorMixer) { - const float thrDefault = defaultCustomMotorMixer[i].throttle; - const float rollDefault = defaultCustomMotorMixer[i].roll; - const float pitchDefault = defaultCustomMotorMixer[i].pitch; - const float yawDefault = defaultCustomMotorMixer[i].yaw; - const bool equalsDefault = thr == thrDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault; - - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - i, - ftoa(thrDefault, buf0), - ftoa(rollDefault, buf1), - ftoa(pitchDefault, buf2), - ftoa(yawDefault, buf3)); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, - i, - ftoa(thr, buf0), - ftoa(roll, buf1), - ftoa(pitch, buf2), - ftoa(yaw, buf3)); - } -} -#endif // USE_QUAD_MIXER_ONLY - -static void cliMotorMix(char *cmdline) -{ -#ifdef USE_QUAD_MIXER_ONLY - UNUSED(cmdline); -#else - int check = 0; - uint8_t len; - const char *ptr; - - if (isEmpty(cmdline)) { - printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); - } else if (strncasecmp(cmdline, "reset", 5) == 0) { - // erase custom mixer - for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - customMotorMixerMutable(i)->throttle = 0.0f; - } - } else if (strncasecmp(cmdline, "load", 4) == 0) { - ptr = nextArg(cmdline); - if (ptr) { - len = strlen(ptr); - for (uint32_t i = 0; ; i++) { - if (mixerNames[i] == NULL) { - cliPrintLine("Invalid name"); - break; - } - if (strncasecmp(ptr, mixerNames[i], len) == 0) { - mixerLoadMix(i, customMotorMixerMutable(0)); - cliPrintLinef("Loaded %s", mixerNames[i]); - cliMotorMix(""); - break; - } - } - } - } else { - ptr = cmdline; - uint32_t i = atoi(ptr); // get motor number - if (i < MAX_SUPPORTED_MOTORS) { - ptr = nextArg(ptr); - if (ptr) { - customMotorMixerMutable(i)->throttle = fastA2F(ptr); - check++; - } - ptr = nextArg(ptr); - if (ptr) { - customMotorMixerMutable(i)->roll = fastA2F(ptr); - check++; - } - ptr = nextArg(ptr); - if (ptr) { - customMotorMixerMutable(i)->pitch = fastA2F(ptr); - check++; - } - ptr = nextArg(ptr); - if (ptr) { - customMotorMixerMutable(i)->yaw = fastA2F(ptr); - check++; - } - if (check != 4) { - cliShowParseError(); - } else { - printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); - } - } else { - cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1); - } - } -#endif -} - -static void printRxRange(uint8_t dumpMask, const rxChannelRangeConfig_t *channelRangeConfigs, const rxChannelRangeConfig_t *defaultChannelRangeConfigs) -{ - const char *format = "rxrange %u %u %u"; - for (uint32_t i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { - bool equalsDefault = false; - if (defaultChannelRangeConfigs) { - equalsDefault = channelRangeConfigs[i].min == defaultChannelRangeConfigs[i].min - && channelRangeConfigs[i].max == defaultChannelRangeConfigs[i].max; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - i, - defaultChannelRangeConfigs[i].min, - defaultChannelRangeConfigs[i].max - ); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, - i, - channelRangeConfigs[i].min, - channelRangeConfigs[i].max - ); - } -} - -static void cliRxRange(char *cmdline) -{ - int i, validArgumentCount = 0; - const char *ptr; - - if (isEmpty(cmdline)) { - printRxRange(DUMP_MASTER, rxChannelRangeConfigs(0), NULL); - } else if (strcasecmp(cmdline, "reset") == 0) { - resetAllRxChannelRangeConfigurations(rxChannelRangeConfigsMutable(0)); - } else { - ptr = cmdline; - i = atoi(ptr); - if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) { - int rangeMin, rangeMax; - - ptr = nextArg(ptr); - if (ptr) { - rangeMin = atoi(ptr); - validArgumentCount++; - } - - ptr = nextArg(ptr); - if (ptr) { - rangeMax = atoi(ptr); - validArgumentCount++; - } - - if (validArgumentCount != 2) { - cliShowParseError(); - } else if (rangeMin < PWM_PULSE_MIN || rangeMin > PWM_PULSE_MAX || rangeMax < PWM_PULSE_MIN || rangeMax > PWM_PULSE_MAX) { - cliShowParseError(); - } else { - rxChannelRangeConfig_t *channelRangeConfig = rxChannelRangeConfigsMutable(i); - channelRangeConfig->min = rangeMin; - channelRangeConfig->max = rangeMax; - } - } else { - cliShowArgumentRangeError("channel", 0, NON_AUX_CHANNEL_COUNT - 1); - } - } -} - -#ifdef LED_STRIP -static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledConfig_t *defaultLedConfigs) -{ - const char *format = "led %u %s"; - char ledConfigBuffer[20]; - char ledConfigDefaultBuffer[20]; - for (uint32_t i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t ledConfig = ledConfigs[i]; - generateLedConfig(&ledConfig, ledConfigBuffer, sizeof(ledConfigBuffer)); - bool equalsDefault = false; - if (defaultLedConfigs) { - ledConfig_t ledConfigDefault = defaultLedConfigs[i]; - equalsDefault = ledConfig == ledConfigDefault; - generateLedConfig(&ledConfigDefault, ledConfigDefaultBuffer, sizeof(ledConfigDefaultBuffer)); - cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, ledConfigDefaultBuffer); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, i, ledConfigBuffer); - } -} - -static void cliLed(char *cmdline) -{ - int i; - const char *ptr; - - if (isEmpty(cmdline)) { - printLed(DUMP_MASTER, ledStripConfig()->ledConfigs, NULL); - } else { - ptr = cmdline; - i = atoi(ptr); - if (i < LED_MAX_STRIP_LENGTH) { - ptr = nextArg(cmdline); - if (!parseLedStripConfig(i, ptr)) { - cliShowParseError(); - } - } else { - cliShowArgumentRangeError("index", 0, LED_MAX_STRIP_LENGTH - 1); - } - } -} - -static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColor_t *defaultColors) -{ - const char *format = "color %u %d,%u,%u"; - for (uint32_t i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - const hsvColor_t *color = &colors[i]; - bool equalsDefault = false; - if (defaultColors) { - const hsvColor_t *colorDefault = &defaultColors[i]; - equalsDefault = color->h == colorDefault->h - && color->s == colorDefault->s - && color->v == colorDefault->v; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, i,colorDefault->h, colorDefault->s, colorDefault->v); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, i, color->h, color->s, color->v); - } -} - -static void cliColor(char *cmdline) -{ - if (isEmpty(cmdline)) { - printColor(DUMP_MASTER, ledStripConfig()->colors, NULL); - } else { - const char *ptr = cmdline; - const int i = atoi(ptr); - if (i < LED_CONFIGURABLE_COLOR_COUNT) { - ptr = nextArg(cmdline); - if (!parseColor(i, ptr)) { - cliShowParseError(); - } - } else { - cliShowArgumentRangeError("index", 0, LED_CONFIGURABLE_COLOR_COUNT - 1); - } - } -} - -static void printModeColor(uint8_t dumpMask, const ledStripConfig_t *ledStripConfig, const ledStripConfig_t *defaultLedStripConfig) -{ - const char *format = "mode_color %u %u %u"; - for (uint32_t i = 0; i < LED_MODE_COUNT; i++) { - for (uint32_t j = 0; j < LED_DIRECTION_COUNT; j++) { - int colorIndex = ledStripConfig->modeColors[i].color[j]; - bool equalsDefault = false; - if (defaultLedStripConfig) { - int colorIndexDefault = defaultLedStripConfig->modeColors[i].color[j]; - equalsDefault = colorIndex == colorIndexDefault; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, j, colorIndexDefault); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, i, j, colorIndex); - } - } - - for (uint32_t j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { - const int colorIndex = ledStripConfig->specialColors.color[j]; - bool equalsDefault = false; - if (defaultLedStripConfig) { - const int colorIndexDefault = defaultLedStripConfig->specialColors.color[j]; - equalsDefault = colorIndex == colorIndexDefault; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, LED_SPECIAL, j, colorIndexDefault); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, LED_SPECIAL, j, colorIndex); - } - - const int ledStripAuxChannel = ledStripConfig->ledstrip_aux_channel; - bool equalsDefault = false; - if (defaultLedStripConfig) { - const int ledStripAuxChannelDefault = defaultLedStripConfig->ledstrip_aux_channel; - equalsDefault = ledStripAuxChannel == ledStripAuxChannelDefault; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannelDefault); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, LED_AUX_CHANNEL, 0, ledStripAuxChannel); -} - -static void cliModeColor(char *cmdline) -{ - if (isEmpty(cmdline)) { - printModeColor(DUMP_MASTER, ledStripConfig(), NULL); - } else { - enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; - int args[ARGS_COUNT]; - int argNo = 0; - char *saveptr; - const char* ptr = strtok_r(cmdline, " ", &saveptr); - while (ptr && argNo < ARGS_COUNT) { - args[argNo++] = atoi(ptr); - ptr = strtok_r(NULL, " ", &saveptr); - } - - if (ptr != NULL || argNo != ARGS_COUNT) { - cliShowParseError(); - return; - } - - int modeIdx = args[MODE]; - int funIdx = args[FUNCTION]; - int color = args[COLOR]; - if(!setModeColor(modeIdx, funIdx, color)) { - cliShowParseError(); - return; - } - // values are validated - cliPrintLinef("mode_color %u %u %u", modeIdx, funIdx, color); - } -} -#endif - -#ifdef USE_SERVOS -static void printServo(uint8_t dumpMask, const servoParam_t *servoParams, const servoParam_t *defaultServoParams) -{ - // print out servo settings - const char *format = "servo %u %d %d %d %d %d"; - for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - const servoParam_t *servoConf = &servoParams[i]; - bool equalsDefault = false; - if (defaultServoParams) { - const servoParam_t *defaultServoConf = &defaultServoParams[i]; - equalsDefault = servoConf->min == defaultServoConf->min - && servoConf->max == defaultServoConf->max - && servoConf->middle == defaultServoConf->middle - && servoConf->rate == defaultServoConf->rate - && servoConf->forwardFromChannel == defaultServoConf->forwardFromChannel; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - i, - defaultServoConf->min, - defaultServoConf->max, - defaultServoConf->middle, - defaultServoConf->rate, - defaultServoConf->forwardFromChannel - ); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, - i, - servoConf->min, - servoConf->max, - servoConf->middle, - servoConf->rate, - servoConf->forwardFromChannel - ); - } - // print servo directions - for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - const char *format = "smix reverse %d %d r"; - const servoParam_t *servoConf = &servoParams[i]; - const servoParam_t *servoConfDefault = &defaultServoParams[i]; - if (defaultServoParams) { - bool equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; - for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); - if (servoConfDefault->reversedSources & (1 << channel)) { - cliDefaultPrintLinef(dumpMask, equalsDefault, format, i , channel); - } - if (servoConf->reversedSources & (1 << channel)) { - cliDumpPrintLinef(dumpMask, equalsDefault, format, i , channel); - } - } - } else { - for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - if (servoConf->reversedSources & (1 << channel)) { - cliDumpPrintLinef(dumpMask, true, format, i , channel); - } - } - } - } -} - -static void cliServo(char *cmdline) -{ - enum { SERVO_ARGUMENT_COUNT = 6 }; - int16_t arguments[SERVO_ARGUMENT_COUNT]; - - servoParam_t *servo; - - int i; - char *ptr; - - if (isEmpty(cmdline)) { - printServo(DUMP_MASTER, servoParams(0), NULL); - } else { - int validArgumentCount = 0; - - ptr = cmdline; - - // Command line is integers (possibly negative) separated by spaces, no other characters allowed. - - // If command line doesn't fit the format, don't modify the config - while (*ptr) { - if (*ptr == '-' || (*ptr >= '0' && *ptr <= '9')) { - if (validArgumentCount >= SERVO_ARGUMENT_COUNT) { - cliShowParseError(); - return; - } - - arguments[validArgumentCount++] = atoi(ptr); - - do { - ptr++; - } while (*ptr >= '0' && *ptr <= '9'); - } else if (*ptr == ' ') { - ptr++; - } else { - cliShowParseError(); - return; - } - } - - enum {INDEX = 0, MIN, MAX, MIDDLE, RATE, FORWARD}; - - i = arguments[INDEX]; - - // Check we got the right number of args and the servo index is correct (don't validate the other values) - if (validArgumentCount != SERVO_ARGUMENT_COUNT || i < 0 || i >= MAX_SUPPORTED_SERVOS) { - cliShowParseError(); - return; - } - - servo = servoParamsMutable(i); - - if ( - arguments[MIN] < PWM_PULSE_MIN || arguments[MIN] > PWM_PULSE_MAX || - arguments[MAX] < PWM_PULSE_MIN || arguments[MAX] > PWM_PULSE_MAX || - arguments[MIDDLE] < arguments[MIN] || arguments[MIDDLE] > arguments[MAX] || - arguments[MIN] > arguments[MAX] || arguments[MAX] < arguments[MIN] || - arguments[RATE] < -100 || arguments[RATE] > 100 || - arguments[FORWARD] >= MAX_SUPPORTED_RC_CHANNEL_COUNT - ) { - cliShowParseError(); - return; - } - - servo->min = arguments[MIN]; - servo->max = arguments[MAX]; - servo->middle = arguments[MIDDLE]; - servo->rate = arguments[RATE]; - servo->forwardFromChannel = arguments[FORWARD]; - } -} -#endif - -#ifdef USE_SERVOS -static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixers, const servoMixer_t *defaultCustomServoMixers) -{ - const char *format = "smix %d %d %d %d %d %d %d %d"; - for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { - const servoMixer_t customServoMixer = customServoMixers[i]; - if (customServoMixer.rate == 0) { - break; - } - - bool equalsDefault = false; - if (defaultCustomServoMixers) { - servoMixer_t customServoMixerDefault = defaultCustomServoMixers[i]; - equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel - && customServoMixer.inputSource == customServoMixerDefault.inputSource - && customServoMixer.rate == customServoMixerDefault.rate - && customServoMixer.speed == customServoMixerDefault.speed - && customServoMixer.min == customServoMixerDefault.min - && customServoMixer.max == customServoMixerDefault.max - && customServoMixer.box == customServoMixerDefault.box; - - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - i, - customServoMixerDefault.targetChannel, - customServoMixerDefault.inputSource, - customServoMixerDefault.rate, - customServoMixerDefault.speed, - customServoMixerDefault.min, - customServoMixerDefault.max, - customServoMixerDefault.box - ); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, - i, - customServoMixer.targetChannel, - customServoMixer.inputSource, - customServoMixer.rate, - customServoMixer.speed, - customServoMixer.min, - customServoMixer.max, - customServoMixer.box - ); - } - - cliPrintLinefeed(); -} - -static void cliServoMix(char *cmdline) -{ - int args[8], check = 0; - int len = strlen(cmdline); - - if (len == 0) { - printServoMix(DUMP_MASTER, customServoMixers(0), NULL); - } else if (strncasecmp(cmdline, "reset", 5) == 0) { - // erase custom mixer - memset(customServoMixers_array(), 0, sizeof(*customServoMixers_array())); - for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servoParamsMutable(i)->reversedSources = 0; - } - } else if (strncasecmp(cmdline, "load", 4) == 0) { - const char *ptr = nextArg(cmdline); - if (ptr) { - len = strlen(ptr); - for (uint32_t i = 0; ; i++) { - if (mixerNames[i] == NULL) { - cliPrintLine("Invalid name"); - break; - } - if (strncasecmp(ptr, mixerNames[i], len) == 0) { - servoMixerLoadMix(i); - cliPrintLinef("Loaded %s", mixerNames[i]); - cliServoMix(""); - break; - } - } - } - } else if (strncasecmp(cmdline, "reverse", 7) == 0) { - enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT}; - char *ptr = strchr(cmdline, ' '); - - len = strlen(ptr); - if (len == 0) { - cliPrintf("s"); - for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) - cliPrintf("\ti%d", inputSource); - cliPrintLinefeed(); - - for (uint32_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { - cliPrintf("%d", servoIndex); - for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) - cliPrintf("\t%s ", (servoParams(servoIndex)->reversedSources & (1 << inputSource)) ? "r" : "n"); - cliPrintLinefeed(); - } - return; - } - - char *saveptr; - ptr = strtok_r(ptr, " ", &saveptr); - while (ptr != NULL && check < ARGS_COUNT - 1) { - args[check++] = atoi(ptr); - ptr = strtok_r(NULL, " ", &saveptr); - } - - if (ptr == NULL || check != ARGS_COUNT - 1) { - cliShowParseError(); - return; - } - - if (args[SERVO] >= 0 && args[SERVO] < MAX_SUPPORTED_SERVOS - && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT - && (*ptr == 'r' || *ptr == 'n')) { - if (*ptr == 'r') - servoParamsMutable(args[SERVO])->reversedSources |= 1 << args[INPUT]; - else - servoParamsMutable(args[SERVO])->reversedSources &= ~(1 << args[INPUT]); - } else - cliShowParseError(); - - cliServoMix("reverse"); - } else { - enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT}; - char *saveptr; - char *ptr = strtok_r(cmdline, " ", &saveptr); - while (ptr != NULL && check < ARGS_COUNT) { - args[check++] = atoi(ptr); - ptr = strtok_r(NULL, " ", &saveptr); - } - - if (ptr != NULL || check != ARGS_COUNT) { - cliShowParseError(); - return; - } - - int32_t i = args[RULE]; - if (i >= 0 && i < MAX_SERVO_RULES && - args[TARGET] >= 0 && args[TARGET] < MAX_SUPPORTED_SERVOS && - args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT && - args[RATE] >= -100 && args[RATE] <= 100 && - args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED && - args[MIN] >= 0 && args[MIN] <= 100 && - args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] && - args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) { - customServoMixersMutable(i)->targetChannel = args[TARGET]; - customServoMixersMutable(i)->inputSource = args[INPUT]; - customServoMixersMutable(i)->rate = args[RATE]; - customServoMixersMutable(i)->speed = args[SPEED]; - customServoMixersMutable(i)->min = args[MIN]; - customServoMixersMutable(i)->max = args[MAX]; - customServoMixersMutable(i)->box = args[BOX]; - cliServoMix(""); - } else { - cliShowParseError(); - } - } -} -#endif - -#ifdef USE_SDCARD - -static void cliWriteBytes(const uint8_t *buffer, int count) -{ - while (count > 0) { - cliWrite(*buffer); - buffer++; - count--; - } -} - -static void cliSdInfo(char *cmdline) -{ - UNUSED(cmdline); - - cliPrint("SD card: "); - - if (!sdcard_isInserted()) { - cliPrintLine("None inserted"); - return; - } - - if (!sdcard_isInitialized()) { - cliPrintLine("Startup failed"); - return; - } - - const sdcardMetadata_t *metadata = sdcard_getMetadata(); - - cliPrintf("Manufacturer 0x%x, %ukB, %02d/%04d, v%d.%d, '", - metadata->manufacturerID, - metadata->numBlocks / 2, /* One block is half a kB */ - metadata->productionMonth, - metadata->productionYear, - metadata->productRevisionMajor, - metadata->productRevisionMinor - ); - - cliWriteBytes((uint8_t*)metadata->productName, sizeof(metadata->productName)); - - cliPrint("'\r\n" "Filesystem: "); - - switch (afatfs_getFilesystemState()) { - case AFATFS_FILESYSTEM_STATE_READY: - cliPrint("Ready"); - break; - case AFATFS_FILESYSTEM_STATE_INITIALIZATION: - cliPrint("Initializing"); - break; - case AFATFS_FILESYSTEM_STATE_UNKNOWN: - case AFATFS_FILESYSTEM_STATE_FATAL: - cliPrint("Fatal"); - - switch (afatfs_getLastError()) { - case AFATFS_ERROR_BAD_MBR: - cliPrint(" - no FAT MBR partitions"); - break; - case AFATFS_ERROR_BAD_FILESYSTEM_HEADER: - cliPrint(" - bad FAT header"); - break; - case AFATFS_ERROR_GENERIC: - case AFATFS_ERROR_NONE: - ; // Nothing more detailed to print - break; - } - break; - } - cliPrintLinefeed(); -} - -#endif - -#ifdef USE_FLASHFS - -static void cliFlashInfo(char *cmdline) -{ - const flashGeometry_t *layout = flashfsGetGeometry(); - - UNUSED(cmdline); - - cliPrintLinef("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u, usedSize=%u", - layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize, flashfsGetOffset()); -} - - -static void cliFlashErase(char *cmdline) -{ - UNUSED(cmdline); - -#ifndef MINIMAL_CLI - uint32_t i = 0; - cliPrintLine("Erasing, please wait ... "); -#else - cliPrintLine("Erasing,"); -#endif - - bufWriterFlush(cliWriter); - flashfsEraseCompletely(); - - while (!flashfsIsReady()) { -#ifndef MINIMAL_CLI - cliPrintf("."); - if (i++ > 120) { - i=0; - cliPrintLinefeed(); - } - - bufWriterFlush(cliWriter); -#endif - delay(100); - } - beeper(BEEPER_BLACKBOX_ERASE); - cliPrintLinefeed(); - cliPrintLine("Done."); -} - -#ifdef USE_FLASH_TOOLS - -static void cliFlashWrite(char *cmdline) -{ - const uint32_t address = atoi(cmdline); - const char *text = strchr(cmdline, ' '); - - if (!text) { - cliShowParseError(); - } else { - flashfsSeekAbs(address); - flashfsWrite((uint8_t*)text, strlen(text), true); - flashfsFlushSync(); - - cliPrintLinef("Wrote %u bytes at %u.", strlen(text), address); - } -} - -static void cliFlashRead(char *cmdline) -{ - uint32_t address = atoi(cmdline); - - const char *nextArg = strchr(cmdline, ' '); - - if (!nextArg) { - cliShowParseError(); - } else { - uint32_t length = atoi(nextArg); - - cliPrintLinef("Reading %u bytes at %u:", length, address); - - uint8_t buffer[32]; - while (length > 0) { - int bytesRead = flashfsReadAbs(address, buffer, length < sizeof(buffer) ? length : sizeof(buffer)); - - for (int i = 0; i < bytesRead; i++) { - cliWrite(buffer[i]); - } - - length -= bytesRead; - address += bytesRead; - - if (bytesRead == 0) { - //Assume we reached the end of the volume or something fatal happened - break; - } - } - cliPrintLinefeed(); - } -} - -#endif -#endif - -#ifdef VTX_CONTROL -static void printVtx(uint8_t dumpMask, const vtxConfig_t *vtxConfig, const vtxConfig_t *vtxConfigDefault) -{ - // print out vtx channel settings - const char *format = "vtx %u %u %u %u %u %u"; - bool equalsDefault = false; - for (uint32_t i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { - const vtxChannelActivationCondition_t *cac = &vtxConfig->vtxChannelActivationConditions[i]; - if (vtxConfigDefault) { - const vtxChannelActivationCondition_t *cacDefault = &vtxConfigDefault->vtxChannelActivationConditions[i]; - equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex - && cac->band == cacDefault->band - && cac->channel == cacDefault->channel - && cac->range.startStep == cacDefault->range.startStep - && cac->range.endStep == cacDefault->range.endStep; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, - i, - cacDefault->auxChannelIndex, - cacDefault->band, - cacDefault->channel, - MODE_STEP_TO_CHANNEL_VALUE(cacDefault->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(cacDefault->range.endStep) - ); - } - cliDumpPrintLinef(dumpMask, equalsDefault, format, - i, - cac->auxChannelIndex, - cac->band, - cac->channel, - MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep) - ); - } -} - -// FIXME remove these and use the VTX API -#define VTX_BAND_MIN 1 -#define VTX_BAND_MAX 5 -#define VTX_CHANNEL_MIN 1 -#define VTX_CHANNEL_MAX 8 - -static void cliVtx(char *cmdline) -{ - int i, val = 0; - const char *ptr; - - if (isEmpty(cmdline)) { - printVtx(DUMP_MASTER, vtxConfig(), NULL); - } else { - ptr = cmdline; - i = atoi(ptr++); - if (i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT) { - vtxChannelActivationCondition_t *cac = &vtxConfigMutable()->vtxChannelActivationConditions[i]; - uint8_t validArgumentCount = 0; - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { - cac->auxChannelIndex = val; - validArgumentCount++; - } - } - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - // FIXME Use VTX API to get min/max - if (val >= VTX_BAND_MIN && val <= VTX_BAND_MAX) { - cac->band = val; - validArgumentCount++; - } - } - ptr = nextArg(ptr); - if (ptr) { - val = atoi(ptr); - // FIXME Use VTX API to get min/max - if (val >= VTX_CHANNEL_MIN && val <= VTX_CHANNEL_MAX) { - cac->channel = val; - validArgumentCount++; - } - } - ptr = processChannelRangeArgs(ptr, &cac->range, &validArgumentCount); - - if (validArgumentCount != 5) { - memset(cac, 0, sizeof(vtxChannelActivationCondition_t)); - } - } else { - cliShowArgumentRangeError("index", 0, MAX_CHANNEL_ACTIVATION_CONDITION_COUNT - 1); - } - } -} - -#endif // VTX_CONTROL - -static void printName(uint8_t dumpMask, const systemConfig_t *systemConfig) -{ - const bool equalsDefault = strlen(systemConfig->name) == 0; - cliDumpPrintLinef(dumpMask, equalsDefault, "name %s", equalsDefault ? emptyName : systemConfig->name); -} - -static void cliName(char *cmdline) -{ - const uint32_t len = strlen(cmdline); - if (len > 0) { - memset(systemConfigMutable()->name, 0, ARRAYLEN(systemConfig()->name)); - if (strncmp(cmdline, emptyName, len)) { - strncpy(systemConfigMutable()->name, cmdline, MIN(len, MAX_NAME_LENGTH)); - } - } - printName(DUMP_MASTER, systemConfig()); -} - -static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault) -{ - const uint32_t mask = featureConfig->enabledFeatures; - const uint32_t defaultMask = featureConfigDefault->enabledFeatures; - for (uint32_t i = 0; featureNames[i]; i++) { // disable all feature first - const char *format = "feature -%s"; - cliDefaultPrintLinef(dumpMask, (defaultMask | ~mask) & (1 << i), format, featureNames[i]); - cliDumpPrintLinef(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]); - } - for (uint32_t i = 0; featureNames[i]; i++) { // reenable what we want. - const char *format = "feature %s"; - if (defaultMask & (1 << i)) { - cliDefaultPrintLinef(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]); - } - if (mask & (1 << i)) { - cliDumpPrintLinef(dumpMask, (defaultMask | ~mask) & (1 << i), format, featureNames[i]); - } - } -} - -static void cliFeature(char *cmdline) -{ - uint32_t len = strlen(cmdline); - uint32_t mask = featureMask(); - - if (len == 0) { - cliPrint("Enabled: "); - for (uint32_t i = 0; ; i++) { - if (featureNames[i] == NULL) - break; - if (mask & (1 << i)) - cliPrintf("%s ", featureNames[i]); - } - cliPrintLinefeed(); - } else if (strncasecmp(cmdline, "list", len) == 0) { - cliPrint("Available:"); - for (uint32_t i = 0; ; i++) { - if (featureNames[i] == NULL) - break; - cliPrintf(" %s", featureNames[i]); - } - cliPrintLinefeed(); - return; - } else { - bool remove = false; - if (cmdline[0] == '-') { - // remove feature - remove = true; - cmdline++; // skip over - - len--; - } - - for (uint32_t i = 0; ; i++) { - if (featureNames[i] == NULL) { - cliPrintLine("Invalid name"); - break; - } - - if (strncasecmp(cmdline, featureNames[i], len) == 0) { - - mask = 1 << i; -#ifndef GPS - if (mask & FEATURE_GPS) { - cliPrintLine("unavailable"); - break; - } -#endif -#ifndef SONAR - if (mask & FEATURE_SONAR) { - cliPrintLine("unavailable"); - break; - } -#endif - if (remove) { - featureClear(mask); - cliPrint("Disabled"); - } else { - featureSet(mask); - cliPrint("Enabled"); - } - cliPrintLinef(" %s", featureNames[i]); - break; - } - } - } -} - -#ifdef BEEPER -static void printBeeper(uint8_t dumpMask, const beeperConfig_t *beeperConfig, const beeperConfig_t *beeperConfigDefault) -{ - const uint8_t beeperCount = beeperTableEntryCount(); - const uint32_t mask = beeperConfig->beeper_off_flags; - const uint32_t defaultMask = beeperConfigDefault->beeper_off_flags; - for (int32_t i = 0; i < beeperCount - 2; i++) { - const char *formatOff = "beeper -%s"; - const char *formatOn = "beeper %s"; - cliDefaultPrintLinef(dumpMask, ~(mask ^ defaultMask) & (1 << i), mask & (1 << i) ? formatOn : formatOff, beeperNameForTableIndex(i)); - cliDumpPrintLinef(dumpMask, ~(mask ^ defaultMask) & (1 << i), mask & (1 << i) ? formatOff : formatOn, beeperNameForTableIndex(i)); - } -} - -static void cliBeeper(char *cmdline) -{ - uint32_t len = strlen(cmdline); - uint8_t beeperCount = beeperTableEntryCount(); - uint32_t mask = getBeeperOffMask(); - - if (len == 0) { - cliPrintf("Disabled:"); - for (int32_t i = 0; ; i++) { - if (i == beeperCount - 2){ - if (mask == 0) - cliPrint(" none"); - break; - } - if (mask & (1 << i)) - cliPrintf(" %s", beeperNameForTableIndex(i)); - } - cliPrintLinefeed(); - } else if (strncasecmp(cmdline, "list", len) == 0) { - cliPrint("Available:"); - for (uint32_t i = 0; i < beeperCount; i++) - cliPrintf(" %s", beeperNameForTableIndex(i)); - cliPrintLinefeed(); - return; - } else { - bool remove = false; - if (cmdline[0] == '-') { - remove = true; // this is for beeper OFF condition - cmdline++; - len--; - } - - for (uint32_t i = 0; ; i++) { - if (i == beeperCount) { - cliPrintLine("Invalid name"); - break; - } - if (strncasecmp(cmdline, beeperNameForTableIndex(i), len) == 0) { - if (remove) { // beeper off - if (i == BEEPER_ALL-1) - beeperOffSetAll(beeperCount-2); - else - if (i == BEEPER_PREFERENCE-1) - setBeeperOffMask(getPreferredBeeperOffMask()); - else { - mask = 1 << i; - beeperOffSet(mask); - } - cliPrint("Disabled"); - } - else { // beeper on - if (i == BEEPER_ALL-1) - beeperOffClearAll(); - else - if (i == BEEPER_PREFERENCE-1) - setPreferredBeeperOffMask(getBeeperOffMask()); - else { - mask = 1 << i; - beeperOffClear(mask); - } - cliPrint("Enabled"); - } - cliPrintLinef(" %s", beeperNameForTableIndex(i)); - break; - } - } - } -} -#endif - -static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) -{ - bool equalsDefault = true; - char buf[16]; - char bufDefault[16]; - uint32_t i; - for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { - buf[rxConfig->rcmap[i]] = rcChannelLetters[i]; - if (defaultRxConfig) { - bufDefault[defaultRxConfig->rcmap[i]] = rcChannelLetters[i]; - equalsDefault = equalsDefault && (rxConfig->rcmap[i] == defaultRxConfig->rcmap[i]); - } - } - buf[i] = '\0'; - - const char *formatMap = "map %s"; - cliDefaultPrintLinef(dumpMask, equalsDefault, formatMap, bufDefault); - cliDumpPrintLinef(dumpMask, equalsDefault, formatMap, buf); -} - -static void cliMap(char *cmdline) -{ - uint32_t len; - char out[9]; - - len = strlen(cmdline); - - if (len == 8) { - // uppercase it - for (uint32_t i = 0; i < 8; i++) - cmdline[i] = toupper((unsigned char)cmdline[i]); - for (uint32_t i = 0; i < 8; i++) { - if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i])) - continue; - cliShowParseError(); - return; - } - parseRcChannels(cmdline, rxConfigMutable()); - } - cliPrint("Map: "); - uint32_t i; - for (i = 0; i < 8; i++) - out[rxConfig()->rcmap[i]] = rcChannelLetters[i]; - out[i] = '\0'; - cliPrintLine(out); -} - -static char *checkCommand(char *cmdLine, const char *command) -{ - if(!strncasecmp(cmdLine, command, strlen(command)) // command names match - && (isspace((unsigned)cmdLine[strlen(command)]) || cmdLine[strlen(command)] == 0)) { - return cmdLine + strlen(command) + 1; - } else { - return 0; - } -} - -static void cliRebootEx(bool bootLoader) -{ - cliPrint("\r\nRebooting"); - bufWriterFlush(cliWriter); - waitForSerialPortToFinishTransmitting(cliPort); - stopPwmAllMotors(); - if (bootLoader) { - systemResetToBootloader(); - return; - } - systemReset(); -} - -static void cliReboot(void) -{ - cliRebootEx(false); -} - -static void cliBootloader(char *cmdLine) -{ - UNUSED(cmdLine); - - cliPrintHashLine("restarting in bootloader mode"); - cliRebootEx(true); -} - -static void cliExit(char *cmdline) -{ - UNUSED(cmdline); - - cliPrintHashLine("leaving CLI mode, unsaved changes lost"); - bufWriterFlush(cliWriter); - - *cliBuffer = '\0'; - bufferIndex = 0; - cliMode = 0; - // incase a motor was left running during motortest, clear it here - mixerResetDisarmedMotors(); - cliReboot(); - - cliWriter = NULL; -} - -#ifdef GPS -static void cliGpsPassthrough(char *cmdline) -{ - UNUSED(cmdline); - - gpsEnablePassthrough(cliPort); -} -#endif - -#if defined(USE_ESCSERIAL) || defined(USE_DSHOT) - -#ifndef ALL_ESCS -#define ALL_ESCS 255 -#endif - -static int parseEscNumber(char *pch, bool allowAllEscs) { - int escNumber = atoi(pch); - if ((escNumber >= 0) && (escNumber < getMotorCount())) { - tfp_printf("Programming on ESC %d.\r\n", escNumber); - } else if (allowAllEscs && escNumber == ALL_ESCS) { - tfp_printf("Programming on all ESCs.\r\n"); - } else { - tfp_printf("Invalid ESC number, range: 0 to %d.\r\n", getMotorCount() - 1); - - return -1; - } - - return escNumber; -} -#endif - -#ifdef USE_DSHOT -static void cliDshotProg(char *cmdline) -{ - if (isEmpty(cmdline) || motorConfig()->dev.motorPwmProtocol < PWM_TYPE_DSHOT150) { - cliShowParseError(); - - return; - } - - char *saveptr; - char *pch = strtok_r(cmdline, " ", &saveptr); - int pos = 0; - int escNumber = 0; - while (pch != NULL) { - switch (pos) { - case 0: - escNumber = parseEscNumber(pch, true); - if (escNumber == -1) { - return; - } - - break; - default: - motorControlEnable = false; - - int command = atoi(pch); - if (command >= 0 && command < DSHOT_MIN_THROTTLE) { - if (escNumber == ALL_ESCS) { - for (unsigned i = 0; i < getMotorCount(); i++) { - pwmWriteDshotCommand(i, command); - } - } else { - pwmWriteDshotCommand(escNumber, command); - } - - if (command <= 5) { - delay(10); // wait for sound output to finish - } - - tfp_printf("Command %d written.\r\n", command); - } else { - tfp_printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1); - } - - break; - } - - pos++; - pch = strtok_r(NULL, " ", &saveptr); - } - - motorControlEnable = true; -} -#endif - -#ifdef USE_ESCSERIAL -static void cliEscPassthrough(char *cmdline) -{ - if (isEmpty(cmdline)) { - cliShowParseError(); - - return; - } - - char *saveptr; - char *pch = strtok_r(cmdline, " ", &saveptr); - int pos = 0; - uint8_t mode = 0; - int escNumber = 0; - while (pch != NULL) { - switch (pos) { - case 0: - if(strncasecmp(pch, "sk", strlen(pch)) == 0) { - mode = PROTOCOL_SIMONK; - } else if(strncasecmp(pch, "bl", strlen(pch)) == 0) { - mode = PROTOCOL_BLHELI; - } else if(strncasecmp(pch, "ki", strlen(pch)) == 0) { - mode = PROTOCOL_KISS; - } else if(strncasecmp(pch, "cc", strlen(pch)) == 0) { - mode = PROTOCOL_KISSALL; - } else { - cliShowParseError(); - - return; - } - break; - case 1: - escNumber = parseEscNumber(pch, mode == PROTOCOL_KISS); - if (escNumber == -1) { - return; - } - - break; - default: - cliShowParseError(); - - return; - - break; - - } - pos++; - pch = strtok_r(NULL, " ", &saveptr); - } - - escEnablePassthrough(cliPort, escNumber, mode); -} -#endif - -#ifndef USE_QUAD_MIXER_ONLY -static void cliMixer(char *cmdline) -{ - int len; - - len = strlen(cmdline); - - if (len == 0) { - cliPrintLinef("Mixer: %s", mixerNames[mixerConfig()->mixerMode - 1]); - return; - } else if (strncasecmp(cmdline, "list", len) == 0) { - cliPrint("Available:"); - for (uint32_t i = 0; ; i++) { - if (mixerNames[i] == NULL) - break; - cliPrintf(" %s", mixerNames[i]); - } - cliPrintLinefeed(); - return; - } - - for (uint32_t i = 0; ; i++) { - if (mixerNames[i] == NULL) { - cliPrintLine("Invalid name"); - return; - } - if (strncasecmp(cmdline, mixerNames[i], len) == 0) { - mixerConfigMutable()->mixerMode = i + 1; - break; - } - } - - cliMixer(""); -} -#endif - -static void cliMotor(char *cmdline) -{ - int motor_index = 0; - int motor_value = 0; - int index = 0; - char *pch = NULL; - char *saveptr; - - if (isEmpty(cmdline)) { - cliShowParseError(); - return; - } - - pch = strtok_r(cmdline, " ", &saveptr); - while (pch != NULL) { - switch (index) { - case 0: - motor_index = atoi(pch); - break; - case 1: - motor_value = atoi(pch); - break; - } - index++; - pch = strtok_r(NULL, " ", &saveptr); - } - - if (motor_index < 0 || motor_index >= MAX_SUPPORTED_MOTORS) { - cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1); - return; - } - - if (index == 2) { - if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) { - cliShowArgumentRangeError("value", 1000, 2000); - } else { - motor_disarmed[motor_index] = convertExternalToMotor(motor_value); - - cliPrintLinef("motor %d: %d", motor_index, convertMotorToExternal(motor_disarmed[motor_index])); - } - } - -} - -#ifndef MINIMAL_CLI -static void cliPlaySound(char *cmdline) -{ - int i; - const char *name; - static int lastSoundIdx = -1; - - if (isEmpty(cmdline)) { - i = lastSoundIdx + 1; //next sound index - if ((name=beeperNameForTableIndex(i)) == NULL) { - while (true) { //no name for index; try next one - if (++i >= beeperTableEntryCount()) - i = 0; //if end then wrap around to first entry - if ((name=beeperNameForTableIndex(i)) != NULL) - break; //if name OK then play sound below - if (i == lastSoundIdx + 1) { //prevent infinite loop - cliPrintLine("Error playing sound"); - return; - } - } - } - } else { //index value was given - i = atoi(cmdline); - if ((name=beeperNameForTableIndex(i)) == NULL) { - cliPrintLinef("No sound for index %d", i); - return; - } - } - lastSoundIdx = i; - beeperSilence(); - cliPrintLinef("Playing sound %d: %s", i, name); - beeper(beeperModeForTableIndex(i)); -} -#endif - -static void cliProfile(char *cmdline) -{ - if (isEmpty(cmdline)) { - cliPrintLinef("profile %d", getCurrentPidProfileIndex()); - return; - } else { - const int i = atoi(cmdline); - if (i >= 0 && i < MAX_PROFILE_COUNT) { - systemConfigMutable()->pidProfileIndex = i; - cliProfile(""); - } - } -} - -static void cliRateProfile(char *cmdline) -{ - if (isEmpty(cmdline)) { - cliPrintLinef("rateprofile %d", getCurrentControlRateProfileIndex()); - return; - } else { - const int i = atoi(cmdline); - if (i >= 0 && i < CONTROL_RATE_PROFILE_COUNT) { - changeControlRateProfile(i); - cliRateProfile(""); - } - } -} - -static void cliDumpPidProfile(uint8_t pidProfileIndex, uint8_t dumpMask) -{ - if (pidProfileIndex >= MAX_PROFILE_COUNT) { - // Faulty values - return; - } - changePidProfile(pidProfileIndex); - cliPrintHashLine("profile"); - cliProfile(""); - cliPrintLinefeed(); - dumpAllValues(PROFILE_VALUE, dumpMask); -} - -static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask) -{ - if (rateProfileIndex >= CONTROL_RATE_PROFILE_COUNT) { - // Faulty values - return; - } - changeControlRateProfile(rateProfileIndex); - cliPrintHashLine("rateprofile"); - cliRateProfile(""); - cliPrintLinefeed(); - dumpAllValues(PROFILE_RATE_VALUE, dumpMask); -} - -static void cliSave(char *cmdline) -{ - UNUSED(cmdline); - - cliPrintHashLine("saving"); - writeEEPROM(); - cliReboot(); -} - -static void cliDefaults(char *cmdline) -{ - UNUSED(cmdline); - - cliPrintHashLine("resetting to defaults"); - resetEEPROM(); - cliReboot(); -} - -static void cliGet(char *cmdline) -{ - const clivalue_t *val; - int matchedCommands = 0; - - for (uint32_t i = 0; i < valueTableEntryCount; i++) { - if (strstr(valueTable[i].name, cmdline)) { - val = &valueTable[i]; - cliPrintf("%s = ", valueTable[i].name); - cliPrintVar(val, 0); - cliPrintLinefeed(); - cliPrintVarRange(val); - cliPrintLinefeed(); - - matchedCommands++; - } - } - - - if (matchedCommands) { - return; - } - - cliPrintLine("Invalid name"); -} - -static char *skipSpace(char *buffer) -{ - while (*(buffer) == ' ') { - buffer++; - } - - return buffer; -} - -static uint8_t getWordLength(char *bufBegin, char *bufEnd) -{ - while (*(bufEnd - 1) == ' ') { - bufEnd--; - } - - return bufEnd - bufBegin; -} - -static void cliSet(char *cmdline) -{ - const uint32_t len = strlen(cmdline); - char *eqptr; - - if (len == 0 || (len == 1 && cmdline[0] == '*')) { - cliPrintLine("Current settings: "); - - for (uint32_t i = 0; i < valueTableEntryCount; i++) { - const clivalue_t *val = &valueTable[i]; - cliPrintf("%s = ", valueTable[i].name); - cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui - cliPrintLinefeed(); - } - } else if ((eqptr = strstr(cmdline, "=")) != NULL) { - // has equals - - uint8_t variableNameLength = getWordLength(cmdline, eqptr); - - // skip the '=' and any ' ' characters - eqptr++; - eqptr = skipSpace(eqptr); - - for (uint32_t i = 0; i < valueTableEntryCount; i++) { - const clivalue_t *val = &valueTable[i]; - // ensure exact match when setting to prevent setting variables with shorter names - if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { - - bool valueChanged = false; - int16_t value = 0; - switch (valueTable[i].type & VALUE_MODE_MASK) { - case MODE_DIRECT: { - int16_t value = atoi(eqptr); - - if (value >= valueTable[i].config.minmax.min && value <= valueTable[i].config.minmax.max) { - cliSetVar(val, value); - valueChanged = true; - } - } - - break; - case MODE_LOOKUP: { - const lookupTableEntry_t *tableEntry = &lookupTables[valueTable[i].config.lookup.tableIndex]; - bool matched = false; - for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { - matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0; - - if (matched) { - value = tableValueIndex; - - cliSetVar(val, value); - valueChanged = true; - } - } - } - - break; - case MODE_ARRAY: { - const uint8_t arrayLength = valueTable[i].config.array.length; - char *valPtr = eqptr; - uint8_t array[256]; - char curVal[4]; - for (int i = 0; i < arrayLength; i++) { - valPtr = skipSpace(valPtr); - char *valEnd = strstr(valPtr, ","); - if ((valEnd != NULL) && (i < arrayLength - 1)) { - uint8_t varLength = getWordLength(valPtr, valEnd); - if (varLength <= 3) { - strncpy(curVal, valPtr, getWordLength(valPtr, valEnd)); - curVal[varLength] = '\0'; - array[i] = (uint8_t)atoi((const char *)curVal); - valPtr = valEnd + 1; - } else { - break; - } - } else if ((valEnd == NULL) && (i == arrayLength - 1)) { - array[i] = atoi(valPtr); - - uint8_t *ptr = getValuePointer(val); - memcpy(ptr, array, arrayLength); - valueChanged = true; - } else { - break; - } - } - } - - break; - - } - - if (valueChanged) { - cliPrintf("%s set to ", valueTable[i].name); - cliPrintVar(val, 0); - } else { - cliPrintLine("Invalid value"); - cliPrintVarRange(val); - } - - return; - } - } - cliPrintLine("Invalid name"); - } else { - // no equals, check for matching variables. - cliGet(cmdline); - } -} - -static void cliStatus(char *cmdline) -{ - UNUSED(cmdline); - - cliPrintLinef("System Uptime: %d seconds", millis() / 1000); - cliPrintLinef("Voltage: %d * 0.1V (%dS battery - %s)", getBatteryVoltage(), getBatteryCellCount(), getBatteryStateString()); - - cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); - -#if defined(USE_SENSOR_NAMES) - const uint32_t detectedSensorsMask = sensorsMask(); - for (uint32_t i = 0; ; i++) { - if (sensorTypeNames[i] == NULL) { - break; - } - const uint32_t mask = (1 << i); - if ((detectedSensorsMask & mask) && (mask & SENSOR_NAMES_MASK)) { - const uint8_t sensorHardwareIndex = detectedSensors[i]; - const char *sensorHardware = sensorHardwareNames[i][sensorHardwareIndex]; - cliPrintf(", %s=%s", sensorTypeNames[i], sensorHardware); - if (mask == SENSOR_ACC && acc.dev.revisionCode) { - cliPrintf(".%c", acc.dev.revisionCode); - } - } - } -#endif /* USE_SENSOR_NAMES */ - cliPrintLinefeed(); - -#ifdef USE_SDCARD - cliSdInfo(NULL); -#endif - -#ifdef USE_I2C - const uint16_t i2cErrorCounter = i2cGetErrorCounter(); -#else - const uint16_t i2cErrorCounter = 0; -#endif - -#ifdef STACK_CHECK - cliPrintf("Stack used: %d, ", stackUsedSize()); -#endif - cliPrintLinef("Stack size: %d, Stack address: 0x%x", stackTotalSize(), stackHighMem()); - - cliPrintLinef("I2C Errors: %d, config size: %d, max available config: %d", i2cErrorCounter, getEEPROMConfigSize(), &__config_end - &__config_start); - - const int gyroRate = getTaskDeltaTime(TASK_GYROPID) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYROPID))); - const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX))); - const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM))); - cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d", - constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate); - -} - -#ifndef SKIP_TASK_STATISTICS -static void cliTasks(char *cmdline) -{ - UNUSED(cmdline); - int maxLoadSum = 0; - int averageLoadSum = 0; - -#ifndef MINIMAL_CLI - if (systemConfig()->task_statistics) { - cliPrintLine("Task list rate/hz max/us avg/us maxload avgload total/ms"); - } else { - cliPrintLine("Task list"); - } -#endif - for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { - cfTaskInfo_t taskInfo; - getTaskInfo(taskId, &taskInfo); - if (taskInfo.isEnabled) { - int taskFrequency; - int subTaskFrequency = 0; - if (taskId == TASK_GYROPID) { - subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime)); - taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom; - if (pidConfig()->pid_process_denom > 1) { - cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName); - } else { - taskFrequency = subTaskFrequency; - cliPrintf("%02d - (%11s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); - } - } else { - taskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime)); - cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName); - } - const int maxLoad = taskInfo.maxExecutionTime == 0 ? 0 :(taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000; - const int averageLoad = taskInfo.averageExecutionTime == 0 ? 0 : (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000; - if (taskId != TASK_SERIAL) { - maxLoadSum += maxLoad; - averageLoadSum += averageLoad; - } - if (systemConfig()->task_statistics) { - cliPrintLinef("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d", - taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, - maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000); - } else { - cliPrintLinef("%6d", taskFrequency); - } - if (taskId == TASK_GYROPID && pidConfig()->pid_process_denom > 1) { - cliPrintLinef(" - (%15s) %6d", taskInfo.subTaskName, subTaskFrequency); - } - } - } - if (systemConfig()->task_statistics) { - cfCheckFuncInfo_t checkFuncInfo; - getCheckFuncInfo(&checkFuncInfo); - cliPrintLinef("RX Check Function %19d %7d %25d", checkFuncInfo.maxExecutionTime, checkFuncInfo.averageExecutionTime, checkFuncInfo.totalExecutionTime / 1000); - cliPrintLinef("Total (excluding SERIAL) %25d.%1d%% %4d.%1d%%", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10); - } -} -#endif - -static void cliVersion(char *cmdline) -{ - UNUSED(cmdline); - - cliPrintLinef("# %s / %s %s %s / %s (%s)", - FC_FIRMWARE_NAME, - targetName, - FC_VERSION_STRING, - buildDate, - buildTime, - shortGitRevision - ); -} - -#if defined(USE_RESOURCE_MGMT) - -#define MAX_RESOURCE_INDEX(x) ((x) == 0 ? 1 : (x)) - -typedef struct { - const uint8_t owner; - pgn_t pgn; - uint16_t offset; - const uint8_t maxIndex; -} cliResourceValue_t; - -const cliResourceValue_t resourceTable[] = { -#ifdef BEEPER - { OWNER_BEEPER, PG_BEEPER_DEV_CONFIG, offsetof(beeperDevConfig_t, ioTag), 0 }, -#endif - { OWNER_MOTOR, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.ioTags[0]), MAX_SUPPORTED_MOTORS }, -#ifdef USE_SERVOS - { OWNER_SERVO, PG_SERVO_CONFIG, offsetof(servoConfig_t, dev.ioTags[0]), MAX_SUPPORTED_SERVOS }, -#endif -#if defined(USE_PWM) || defined(USE_PPM) - { OWNER_PPMINPUT, PG_PPM_CONFIG, offsetof(ppmConfig_t, ioTag), 0 }, - { OWNER_PWMINPUT, PG_PWM_CONFIG, offsetof(pwmConfig_t, ioTags[0]), PWM_INPUT_PORT_COUNT }, -#endif -#ifdef SONAR - { OWNER_SONAR_TRIGGER, PG_SONAR_CONFIG, offsetof(sonarConfig_t, triggerTag), 0 }, - { OWNER_SONAR_ECHO, PG_SONAR_CONFIG, offsetof(sonarConfig_t, echoTag), 0 }, -#endif -#ifdef LED_STRIP - { OWNER_LED_STRIP, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ioTag), 0 }, -#endif - { OWNER_SERIAL_TX, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagTx[0]), SERIAL_PORT_MAX_INDEX }, - { OWNER_SERIAL_RX, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagRx[0]), SERIAL_PORT_MAX_INDEX }, -#ifdef USE_INVERTER - { OWNER_INVERTER, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagInverter[0]), SERIAL_PORT_MAX_INDEX }, -#endif -#ifdef USE_I2C - { OWNER_I2C_SCL, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagScl[0]), I2CDEV_COUNT }, - { OWNER_I2C_SDA, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagSda[0]), I2CDEV_COUNT }, -#endif - { OWNER_LED, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, ioTags[0]), STATUS_LED_NUMBER }, -<<<<<<< HEAD -#ifdef TRANSPONDER - { OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 }, -#endif -======= ->>>>>>> betaflight/master -}; - -static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) -{ - const pgRegistry_t* rec = pgFind(value.pgn); - return CONST_CAST(ioTag_t *, rec->address + value.offset + index); -} - -static void printResource(uint8_t dumpMask) -{ - for (unsigned int i = 0; i < ARRAYLEN(resourceTable); i++) { - const char* owner = ownerNames[resourceTable[i].owner]; - const pgRegistry_t* pg = pgFind(resourceTable[i].pgn); - const void *currentConfig; - const void *defaultConfig; - if (configIsInCopy) { - currentConfig = pg->copy; - defaultConfig = pg->address; - } else { - currentConfig = pg->address; - defaultConfig = NULL; - } - - for (int index = 0; index < MAX_RESOURCE_INDEX(resourceTable[i].maxIndex); index++) { - const ioTag_t ioTag = *((const ioTag_t *)currentConfig + resourceTable[i].offset + index); - const ioTag_t ioTagDefault = *((const ioTag_t *)defaultConfig + resourceTable[i].offset + index); - - bool equalsDefault = ioTag == ioTagDefault; - const char *format = "resource %s %d %c%02d"; - const char *formatUnassigned = "resource %s %d NONE"; - if (!ioTagDefault) { - cliDefaultPrintLinef(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); - } else { - cliDefaultPrintLinef(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault)); - } - if (!ioTag) { - if (!(dumpMask & HIDE_UNUSED)) { - cliDumpPrintLinef(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); - } - } else { - cliDumpPrintLinef(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag)); - } - } - } -} - -static void printResourceOwner(uint8_t owner, uint8_t index) -{ - cliPrintf("%s", ownerNames[resourceTable[owner].owner]); - - if (resourceTable[owner].maxIndex > 0) { - cliPrintf(" %d", RESOURCE_INDEX(index)); - } -} - -static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t newTag) -{ - if (!newTag) { - return; - } - - const char * format = "\r\nNOTE: %c%02d already assigned to "; - for (int r = 0; r < (int)ARRAYLEN(resourceTable); r++) { - for (int i = 0; i < MAX_RESOURCE_INDEX(resourceTable[r].maxIndex); i++) { - ioTag_t *tag = getIoTag(resourceTable[r], i); - if (*tag == newTag) { - bool cleared = false; - if (r == resourceIndex) { - if (i == index) { - continue; - } - *tag = IO_TAG_NONE; - cleared = true; - } - - cliPrintf(format, DEFIO_TAG_GPIOID(newTag) + 'A', DEFIO_TAG_PIN(newTag)); - - printResourceOwner(r, i); - - if (cleared) { - cliPrintf(". "); - printResourceOwner(r, i); - cliPrintf(" disabled"); - } - - cliPrintLine("."); - } - } - } -} - -static void cliResource(char *cmdline) -{ - int len = strlen(cmdline); - - if (len == 0) { - printResource(DUMP_MASTER | HIDE_UNUSED); - - return; - } else if (strncasecmp(cmdline, "list", len) == 0) { -#ifdef MINIMAL_CLI - cliPrintLine("IO"); -#else - cliPrintLine("Currently active IO resource assignments:\r\n(reboot to update)"); - cliRepeat('-', 20); -#endif - for (int i = 0; i < DEFIO_IO_USED_COUNT; i++) { - const char* owner; - owner = ownerNames[ioRecs[i].owner]; - - cliPrintf("%c%02d: %s", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner); - if (ioRecs[i].index > 0) { - cliPrintf(" %d", ioRecs[i].index); - } - cliPrintLinefeed(); - } - - cliPrintLinefeed(); - -#ifdef MINIMAL_CLI - cliPrintLine("DMA:"); -#else - cliPrintLine("Currently active DMA:"); - cliRepeat('-', 20); -#endif - for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { - const char* owner; - owner = ownerNames[dmaGetOwner(i)]; - - cliPrintf(DMA_OUTPUT_STRING, i / DMA_MOD_VALUE + 1, (i % DMA_MOD_VALUE) + DMA_MOD_OFFSET); - uint8_t resourceIndex = dmaGetResourceIndex(i); - if (resourceIndex > 0) { - cliPrintLinef(" %s %d", owner, resourceIndex); - } else { - cliPrintLinef(" %s", owner); - } - } - -#ifndef MINIMAL_CLI - cliPrintLine("\r\nUse: 'resource' to see how to change resources."); -#endif - - return; - } - - uint8_t resourceIndex = 0; - int index = 0; - char *pch = NULL; - char *saveptr; - - pch = strtok_r(cmdline, " ", &saveptr); - for (resourceIndex = 0; ; resourceIndex++) { - if (resourceIndex >= ARRAYLEN(resourceTable)) { - cliPrintLine("Invalid"); - return; - } - - if (strncasecmp(pch, ownerNames[resourceTable[resourceIndex].owner], len) == 0) { - break; - } - } - - pch = strtok_r(NULL, " ", &saveptr); - index = atoi(pch); - - if (resourceTable[resourceIndex].maxIndex > 0 || index > 0) { - if (index <= 0 || index > MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)) { - cliShowArgumentRangeError("index", 1, MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)); - return; - } - index -= 1; - - pch = strtok_r(NULL, " ", &saveptr); - } - - ioTag_t *tag = getIoTag(resourceTable[resourceIndex], index); - - uint8_t pin = 0; - if (strlen(pch) > 0) { - if (strcasecmp(pch, "NONE") == 0) { - *tag = IO_TAG_NONE; -#ifdef MINIMAL_CLI - cliPrintLine("Freed"); -#else - cliPrintLine("Resource is freed"); -#endif - return; - } else { - uint8_t port = (*pch) - 'A'; - if (port >= 8) { - port = (*pch) - 'a'; - } - - if (port < 8) { - pch++; - pin = atoi(pch); - if (pin < 16) { - ioRec_t *rec = IO_Rec(IOGetByTag(DEFIO_TAG_MAKE(port, pin))); - if (rec) { - resourceCheck(resourceIndex, index, DEFIO_TAG_MAKE(port, pin)); -#ifdef MINIMAL_CLI - cliPrintLinef(" %c%02d set", port + 'A', pin); -#else - cliPrintLinef("\r\nResource is set to %c%02d", port + 'A', pin); -#endif - *tag = DEFIO_TAG_MAKE(port, pin); - } else { - cliShowParseError(); - } - return; - } - } - } - } - - cliShowParseError(); -} -#endif /* USE_RESOURCE_MGMT */ - -static void backupConfigs(void) -{ - // make copies of configs to do differencing - PG_FOREACH(pg) { - memcpy(pg->copy, pg->address, pg->size); - } - - configIsInCopy = true; -} - -static void restoreConfigs(void) -{ - PG_FOREACH(pg) { - memcpy(pg->address, pg->copy, pg->size); - } - - configIsInCopy = false; -} - -static void printConfig(char *cmdline, bool doDiff) -{ - uint8_t dumpMask = DUMP_MASTER; - char *options; - if ((options = checkCommand(cmdline, "master"))) { - dumpMask = DUMP_MASTER; // only - } else if ((options = checkCommand(cmdline, "profile"))) { - dumpMask = DUMP_PROFILE; // only - } else if ((options = checkCommand(cmdline, "rates"))) { - dumpMask = DUMP_RATES; // only - } else if ((options = checkCommand(cmdline, "all"))) { - dumpMask = DUMP_ALL; // all profiles and rates - } else { - options = cmdline; - } - - if (doDiff) { - dumpMask = dumpMask | DO_DIFF; - } - - backupConfigs(); - // reset all configs to defaults to do differencing - resetConfigs(); - -#if defined(TARGET_CONFIG) - targetConfiguration(); -#endif - if (checkCommand(options, "defaults")) { - dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values - } - - if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { - cliPrintHashLine("version"); - cliVersion(NULL); - - if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { - cliPrintHashLine("reset configuration to default settings"); - cliPrint("defaults"); - cliPrintLinefeed(); - } - - cliPrintHashLine("name"); - printName(dumpMask, &systemConfig_Copy); - -#ifdef USE_RESOURCE_MGMT - cliPrintHashLine("resources"); - printResource(dumpMask); -#endif - -#ifndef USE_QUAD_MIXER_ONLY - cliPrintHashLine("mixer"); - const bool equalsDefault = mixerConfig_Copy.mixerMode == mixerConfig()->mixerMode; - const char *formatMixer = "mixer %s"; - cliDefaultPrintLinef(dumpMask, equalsDefault, formatMixer, mixerNames[mixerConfig()->mixerMode - 1]); - cliDumpPrintLinef(dumpMask, equalsDefault, formatMixer, mixerNames[mixerConfig_Copy.mixerMode - 1]); - - cliDumpPrintLinef(dumpMask, customMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n"); - - printMotorMix(dumpMask, customMotorMixer_CopyArray, customMotorMixer(0)); - -#ifdef USE_SERVOS - cliPrintHashLine("servo"); - printServo(dumpMask, servoParams_CopyArray, servoParams(0)); - - cliPrintHashLine("servo mix"); - // print custom servo mixer if exists - cliDumpPrintLinef(dumpMask, customServoMixers(0)->rate == 0, "smix reset\r\n"); - printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0)); -#endif -#endif - - cliPrintHashLine("feature"); - printFeature(dumpMask, &featureConfig_Copy, featureConfig()); - -#ifdef BEEPER - cliPrintHashLine("beeper"); - printBeeper(dumpMask, &beeperConfig_Copy, beeperConfig()); -#endif - - cliPrintHashLine("map"); - printMap(dumpMask, &rxConfig_Copy, rxConfig()); - - cliPrintHashLine("serial"); - printSerial(dumpMask, &serialConfig_Copy, serialConfig()); - -#ifdef LED_STRIP - cliPrintHashLine("led"); - printLed(dumpMask, ledStripConfig_Copy.ledConfigs, ledStripConfig()->ledConfigs); - - cliPrintHashLine("color"); - printColor(dumpMask, ledStripConfig_Copy.colors, ledStripConfig()->colors); - - cliPrintHashLine("mode_color"); - printModeColor(dumpMask, &ledStripConfig_Copy, ledStripConfig()); -#endif - - cliPrintHashLine("aux"); - printAux(dumpMask, modeActivationConditions_CopyArray, modeActivationConditions(0)); - - cliPrintHashLine("adjrange"); - printAdjustmentRange(dumpMask, adjustmentRanges_CopyArray, adjustmentRanges(0)); - - cliPrintHashLine("rxrange"); - printRxRange(dumpMask, rxChannelRangeConfigs_CopyArray, rxChannelRangeConfigs(0)); - -#ifdef VTX_CONTROL - cliPrintHashLine("vtx"); - printVtx(dumpMask, &vtxConfig_Copy, vtxConfig()); -#endif - - cliPrintHashLine("rxfail"); - printRxFailsafe(dumpMask, rxFailsafeChannelConfigs_CopyArray, rxFailsafeChannelConfigs(0)); - - cliPrintHashLine("master"); - dumpAllValues(MASTER_VALUE, dumpMask); - - if (dumpMask & DUMP_ALL) { - const uint8_t pidProfileIndexSave = systemConfig_Copy.pidProfileIndex; - for (uint32_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { - cliDumpPidProfile(pidProfileIndex, dumpMask); - } - changePidProfile(pidProfileIndexSave); - cliPrintHashLine("restore original profile selection"); - cliProfile(""); - - const uint8_t controlRateProfileIndexSave = systemConfig_Copy.activeRateProfile; - for (uint32_t rateIndex = 0; rateIndex < CONTROL_RATE_PROFILE_COUNT; rateIndex++) { - cliDumpRateProfile(rateIndex, dumpMask); - } - changeControlRateProfile(controlRateProfileIndexSave); - cliPrintHashLine("restore original rateprofile selection"); - cliRateProfile(""); - - cliPrintHashLine("save configuration"); - cliPrint("save"); - } else { - cliDumpPidProfile(systemConfig_Copy.pidProfileIndex, dumpMask); - - cliDumpRateProfile(systemConfig_Copy.activeRateProfile, dumpMask); - } - } - - if (dumpMask & DUMP_PROFILE) { - cliDumpPidProfile(systemConfig_Copy.pidProfileIndex, dumpMask); - } - - if (dumpMask & DUMP_RATES) { - cliDumpRateProfile(systemConfig_Copy.activeRateProfile, dumpMask); - } - // restore configs from copies - restoreConfigs(); -} - -static void cliDump(char *cmdline) -{ - printConfig(cmdline, false); -} - -static void cliDiff(char *cmdline) -{ - printConfig(cmdline, true); -} - -typedef struct { - const char *name; -#ifndef MINIMAL_CLI - const char *description; - const char *args; -#endif - void (*func)(char *cmdline); -} clicmd_t; - -#ifndef MINIMAL_CLI -#define CLI_COMMAND_DEF(name, description, args, method) \ -{ \ - name , \ - description , \ - args , \ - method \ -} -#else -#define CLI_COMMAND_DEF(name, description, args, method) \ -{ \ - name, \ - method \ -} -#endif - -static void cliHelp(char *cmdline); - -// should be sorted a..z for bsearch() -const clicmd_t cmdTable[] = { - CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange), - CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux), -#ifdef BEEPER - CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" - "\t<+|->[name]", cliBeeper), -#endif -#ifdef LED_STRIP - CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), -#endif - CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), - CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), - CLI_COMMAND_DEF("diff", "list configuration changes from default", - "[master|profile|rates|all] {showdefaults}", cliDiff), -#ifdef USE_DSHOT - CLI_COMMAND_DEF("dshotprog", "program DShot ESC(s)", " +", cliDshotProg), -#endif - CLI_COMMAND_DEF("dump", "dump configuration", - "[master|profile|rates|all] {showdefaults}", cliDump), -#ifdef USE_ESCSERIAL - CLI_COMMAND_DEF("escprog", "passthrough esc to serial", " ", cliEscPassthrough), -#endif - CLI_COMMAND_DEF("exit", NULL, NULL, cliExit), - CLI_COMMAND_DEF("feature", "configure features", - "list\r\n" - "\t<+|->[name]", cliFeature), -#ifdef USE_FLASHFS - CLI_COMMAND_DEF("flash_erase", "erase flash chip", NULL, cliFlashErase), - CLI_COMMAND_DEF("flash_info", "show flash chip info", NULL, cliFlashInfo), -#ifdef USE_FLASH_TOOLS - CLI_COMMAND_DEF("flash_read", NULL, "
", cliFlashRead), - CLI_COMMAND_DEF("flash_write", NULL, "
", cliFlashWrite), -#endif -#endif - CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), -#ifdef GPS - CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough), -#endif - CLI_COMMAND_DEF("help", NULL, NULL, cliHelp), -#ifdef LED_STRIP - CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed), -#endif - CLI_COMMAND_DEF("map", "configure rc channel order", "[]", cliMap), -#ifndef USE_QUAD_MIXER_ONLY - CLI_COMMAND_DEF("mixer", "configure mixer", "list\r\n\t", cliMixer), -#endif - CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix), -#ifdef LED_STRIP - CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), -#endif - CLI_COMMAND_DEF("motor", "get/set motor", " []", cliMotor), - CLI_COMMAND_DEF("name", "name of craft", NULL, cliName), -#ifndef MINIMAL_CLI - CLI_COMMAND_DEF("play_sound", NULL, "[]", cliPlaySound), -#endif - CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), - CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), -#if defined(USE_RESOURCE_MGMT) - CLI_COMMAND_DEF("resource", "show/set resources", NULL, cliResource), -#endif - CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFailsafe), - CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), - CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), -#ifdef USE_SDCARD - CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo), -#endif - CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial), -#ifndef SKIP_SERIAL_PASSTHROUGH - CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", " [baud] [mode] : passthrough to serial", cliSerialPassthrough), -#endif -#ifdef USE_SERVOS - CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo), -#endif - CLI_COMMAND_DEF("set", "change setting", "[=]", cliSet), -#ifdef USE_SERVOS - CLI_COMMAND_DEF("smix", "servo mixer", " \r\n" - "\treset\r\n" - "\tload \r\n" - "\treverse r|n", cliServoMix), -#endif - CLI_COMMAND_DEF("status", "show status", NULL, cliStatus), -#ifndef SKIP_TASK_STATISTICS - CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks), -#endif - CLI_COMMAND_DEF("version", "show version", NULL, cliVersion), -#ifdef VTX_CONTROL - CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), -#endif -}; -static void cliHelp(char *cmdline) -{ - UNUSED(cmdline); - - for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) { - cliPrint(cmdTable[i].name); -#ifndef MINIMAL_CLI - if (cmdTable[i].description) { - cliPrintf(" - %s", cmdTable[i].description); - } - if (cmdTable[i].args) { - cliPrintf("\r\n\t%s", cmdTable[i].args); - } -#endif - cliPrintLinefeed(); - } -} - -void cliProcess(void) -{ - if (!cliWriter) { - return; - } - - // Be a little bit tricky. Flush the last inputs buffer, if any. - bufWriterFlush(cliWriter); - - while (serialRxBytesWaiting(cliPort)) { - uint8_t c = serialRead(cliPort); - if (c == '\t' || c == '?') { - // do tab completion - const clicmd_t *cmd, *pstart = NULL, *pend = NULL; - uint32_t i = bufferIndex; - for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { - if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0)) - continue; - if (!pstart) - pstart = cmd; - pend = cmd; - } - if (pstart) { /* Buffer matches one or more commands */ - for (; ; bufferIndex++) { - if (pstart->name[bufferIndex] != pend->name[bufferIndex]) - break; - if (!pstart->name[bufferIndex] && bufferIndex < sizeof(cliBuffer) - 2) { - /* Unambiguous -- append a space */ - cliBuffer[bufferIndex++] = ' '; - cliBuffer[bufferIndex] = '\0'; - break; - } - cliBuffer[bufferIndex] = pstart->name[bufferIndex]; - } - } - if (!bufferIndex || pstart != pend) { - /* Print list of ambiguous matches */ - cliPrint("\r\033[K"); - for (cmd = pstart; cmd <= pend; cmd++) { - cliPrint(cmd->name); - cliWrite('\t'); - } - cliPrompt(); - i = 0; /* Redraw prompt */ - } - for (; i < bufferIndex; i++) - cliWrite(cliBuffer[i]); - } else if (!bufferIndex && c == 4) { // CTRL-D - cliExit(cliBuffer); - return; - } else if (c == 12) { // NewPage / CTRL-L - // clear screen - cliPrint("\033[2J\033[1;1H"); - cliPrompt(); - } else if (bufferIndex && (c == '\n' || c == '\r')) { - // enter pressed - cliPrintLinefeed(); - - // Strip comment starting with # from line - char *p = cliBuffer; - p = strchr(p, '#'); - if (NULL != p) { - bufferIndex = (uint32_t)(p - cliBuffer); - } - - // Strip trailing whitespace - while (bufferIndex > 0 && cliBuffer[bufferIndex - 1] == ' ') { - bufferIndex--; - } - - // Process non-empty lines - if (bufferIndex > 0) { - cliBuffer[bufferIndex] = 0; // null terminate - - const clicmd_t *cmd; - char *options; - for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { - if ((options = checkCommand(cliBuffer, cmd->name))) { - break; - } - } - if(cmd < cmdTable + ARRAYLEN(cmdTable)) - cmd->func(options); - else - cliPrint("Unknown command, try 'help'"); - bufferIndex = 0; - } - - memset(cliBuffer, 0, sizeof(cliBuffer)); - - // 'exit' will reset this flag, so we don't need to print prompt again - if (!cliMode) - return; - - cliPrompt(); - } else if (c == 127) { - // backspace - if (bufferIndex) { - cliBuffer[--bufferIndex] = 0; - cliPrint("\010 \010"); - } - } else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) { - if (!bufferIndex && c == ' ') - continue; // Ignore leading spaces - cliBuffer[bufferIndex++] = c; - cliWrite(c); - } - } -} - -void cliEnter(serialPort_t *serialPort) -{ - cliMode = 1; - cliPort = serialPort; - setPrintfSerialPort(cliPort); - cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort); - - schedulerSetCalulateTaskStatistics(systemConfig()->task_statistics); - -#ifndef MINIMAL_CLI - cliPrintLine("\r\nEntering CLI Mode, type 'exit' to return, or 'help'"); -#else - cliPrintLine("\r\nCLI"); -#endif - cliPrompt(); - - ENABLE_ARMING_FLAG(PREVENT_ARMING); -} - -void cliInit(const serialConfig_t *serialConfig) -{ - UNUSED(serialConfig); -} -#endif // USE_CLI From 08281e8ec95d6065783284b0ebab5090d8f84574 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 30 Jun 2017 15:34:08 +0200 Subject: [PATCH 21/40] Fix SITL compile. --- src/main/fc/fc_init.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 7a4bb09c38..ff4bebf42c 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -478,8 +478,10 @@ void init(void) initBoardAlignment(boardAlignment()); if (!sensorsAutodetect()) { +#if !defined(SITL) // if gyro was not detected due to whatever reason, notify and don't arm. failureLedCode(FAILURE_MISSING_ACC, 2); +#endif setArmingDisabled(ARMING_DISABLED_NO_GYRO); } From fd7df8cc0ea618ec13336bf451a07f5ba99bcb2e Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 22:53:04 +0900 Subject: [PATCH 22/40] Dah! --- src/main/fc/cli.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 27a7dc68b8..6573aeb4da 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2841,7 +2841,6 @@ const cliResourceValue_t resourceTable[] = { #ifdef TRANSPONDER { OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 }, #endif -======= }; static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) From 3dc7bf23ed3260ae94187686a896e25759a4deb1 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 1 Jul 2017 12:45:52 +0900 Subject: [PATCH 23/40] Tidy --- src/main/io/transponder_ir.c | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/main/io/transponder_ir.c b/src/main/io/transponder_ir.c index 2a52ea6bb8..0559dbf75d 100644 --- a/src/main/io/transponder_ir.c +++ b/src/main/io/transponder_ir.c @@ -39,15 +39,6 @@ #include "io/transponder_ir.h" -#if 0 -PG_REGISTER_WITH_RESET_TEMPLATE(transponderConfig_t, transponderConfig, PG_TRANSPONDER_CONFIG, 0); - -PG_RESET_TEMPLATE(transponderConfig_t, transponderConfig, - .provider = TRANSPONDER_ILAP, - .reserved = 0, - .data = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0x0, 0x0, 0x0 }, // Note, this is NOT a valid transponder code, it's just for testing production hardware -); -#endif PG_REGISTER_WITH_RESET_FN(transponderConfig_t, transponderConfig, PG_TRANSPONDER_CONFIG, 0); void pgResetFn_transponderConfig(transponderConfig_t *transponderConfig) From b40d82ee7ef19397eff2aa9d3e15ff9478a52e0d Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 1 Jul 2017 17:04:33 +1000 Subject: [PATCH 24/40] Git Ignore updates --- .gitignore | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/.gitignore b/.gitignore index ef7592b64a..d5ba321dd7 100644 --- a/.gitignore +++ b/.gitignore @@ -19,20 +19,21 @@ startup_stm32f10x_md_gcc.s docs/Manual.pdf README.pdf -# artifacts of top-level Makefile +# artefacts of top-level Makefile /downloads /tools /build # local changes only make/local.mk +# artefacts for VisualGDB (running in Visual Studio) mcu.mak mcu.mak.old stm32.mak -# Artefacts for Visual Studio Code -/.vscode/ +# artefacts for Visual Studio Code +/.vscode -# Artefacts for CLion -/cmake-build-debug/ +# artefacts for CLion +/cmake-build-debug /CMakeLists.txt From 4c152221ad2ecc9363360f483c7969c812b08162 Mon Sep 17 00:00:00 2001 From: TheIsotopes Date: Sat, 1 Jul 2017 11:21:16 +0200 Subject: [PATCH 25/40] sdk update --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index d22f1ef085..4c9ca98726 100644 --- a/.travis.yml +++ b/.travis.yml @@ -47,7 +47,7 @@ install: - make arm_sdk_install before_script: - - tools/gcc-arm-none-eabi-6-2017-q1-update/bin/arm-none-eabi-gcc --version + - tools/gcc-arm-none-eabi-6-2017-q2-update/bin/arm-none-eabi-gcc --version - clang --version - clang++ --version - gcc --version From 67c4a57fa55e863c769e3601913650c2b72920e2 Mon Sep 17 00:00:00 2001 From: TheIsotopes Date: Sat, 1 Jul 2017 12:03:09 +0200 Subject: [PATCH 26/40] sdk update --- make/tools.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index c3f251404d..86f3fbeeb2 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -14,14 +14,14 @@ ############################## # Set up ARM (STM32) SDK -ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-6-2017-q1-update +ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-6-2017-q2-update # Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion) GCC_REQUIRED_VERSION ?= 6.3.1 ## arm_sdk_install : Install Arm SDK .PHONY: arm_sdk_install -ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6_1-2017q1/gcc-arm-none-eabi-6-2017-q1-update +ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2017q2/gcc-arm-none-eabi-6-2017-q2-update # source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads ifdef LINUX @@ -33,7 +33,7 @@ ifdef MACOSX endif ifdef WINDOWS - ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32-zip.zip + ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip endif ARM_SDK_FILE := $(notdir $(ARM_SDK_URL)) From 8df87bc47d3b0c837aaedfd0f0d4b947e8beaffd Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Jun 2017 05:40:31 +0100 Subject: [PATCH 27/40] Make SPI read and write register generic --- src/main/drivers/accgyro/accgyro_mpu.c | 6 +- .../drivers/accgyro/accgyro_spi_mpu6000.c | 61 ++++++------------- .../drivers/accgyro/accgyro_spi_mpu6000.h | 3 - src/main/drivers/bus_spi.c | 25 +++++++- src/main/drivers/bus_spi.h | 6 +- src/main/drivers/bus_spi_hal.c | 26 +++++++- 6 files changed, 74 insertions(+), 53 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 35e80f2060..3127c011a0 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -30,6 +30,7 @@ #include "common/utils.h" #include "drivers/bus_i2c.h" +#include "drivers/bus_spi.h" #include "drivers/exti.h" #include "drivers/io.h" #include "drivers/nvic.h" @@ -233,14 +234,15 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) { UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI #ifdef USE_GYRO_SPI_MPU6000 + gyro->bus.spi.instance = MPU6000_SPI_INSTANCE; #ifdef MPU6000_CS_PIN gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin; #endif if (mpu6000SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister; - gyro->mpuConfiguration.writeFn = mpu6000SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegister; + gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; } #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 3ce84e4a50..006f32eebd 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -99,42 +99,19 @@ static bool mpuSpi6000InitDone = false; #define MPU6000_REV_D9 0x59 #define MPU6000_REV_D10 0x5A -#define DISABLE_MPU6000(spiCsnPin) IOHi(spiCsnPin) -#define ENABLE_MPU6000(spiCsnPin) IOLo(spiCsnPin) - -bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) -{ - ENABLE_MPU6000(bus->spi.csnPin); - spiTransferByte(MPU6000_SPI_INSTANCE, reg); - spiTransferByte(MPU6000_SPI_INSTANCE, data); - DISABLE_MPU6000(bus->spi.csnPin); - - return true; -} - -bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) -{ - ENABLE_MPU6000(bus->spi.csnPin); - spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction - spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length); - DISABLE_MPU6000(bus->spi.csnPin); - - return true; -} - void mpu6000SpiGyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); mpu6000AccAndGyroInit(gyro); - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); // Accel and Gyro DLPF Setting - mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf); + spiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf); delayMicroseconds(1); - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); // 18 MHz SPI clock mpuGyroRead(gyro); @@ -157,14 +134,14 @@ bool mpu6000SpiDetect(const busDevice_t *bus) IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->spi.csnPin); - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); + spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); - mpu6000SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); + spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); do { delay(150); - mpu6000SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in); + spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in); if (in == MPU6000_WHO_AM_I_CONST) { break; } @@ -173,7 +150,7 @@ bool mpu6000SpiDetect(const busDevice_t *bus) } } while (attemptsRemaining--); - mpu6000SpiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in); + spiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in); /* look for a product ID we recognise */ @@ -203,48 +180,48 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro) return; } - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); // Device Reset - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); + spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(150); - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); + spiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); delay(150); // Clock Source PPL with Z axis gyro reference - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); delayMicroseconds(15); // Disable Primary I2C Interface - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); + spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); delayMicroseconds(15); - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00); + spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00); delayMicroseconds(15); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); + spiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); delayMicroseconds(15); // Gyro +/- 1000 DPS Full Scale - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + spiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); delayMicroseconds(15); // Accel +/- 8 G Full Scale - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + spiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); delayMicroseconds(15); - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR + spiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR delayMicroseconds(15); #ifdef USE_MPU_DATA_READY_SIGNAL - mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + spiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); delayMicroseconds(15); #endif - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); delayMicroseconds(1); mpuSpi6000InitDone = true; diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro/accgyro_spi_mpu6000.h index 7eb9b6918c..8435cebad0 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.h @@ -21,6 +21,3 @@ bool mpu6000SpiDetect(const busDevice_t *bus); bool mpu6000SpiAccDetect(accDev_t *acc); bool mpu6000SpiGyroDetect(gyroDev_t *gyro); - -bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index eb0c2fdbc6..9022ecde14 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -20,11 +20,12 @@ #include +#include "drivers/bus.h" #include "drivers/bus_spi.h" #include "drivers/exti.h" #include "drivers/io.h" -#include "io_impl.h" -#include "rcc.h" +#include "drivers/io_impl.h" +#include "drivers/rcc.h" /* for F30x processors */ #if defined(STM32F303xC) @@ -351,3 +352,23 @@ void spiResetErrorCounter(SPI_TypeDef *instance) if (device != SPIINVALID) spiHardwareMap[device].errorCount = 0; } + +bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +{ + IOLo(bus->spi.csnPin); + spiTransferByte(bus->spi.instance, reg); + spiTransferByte(bus->spi.instance, data); + IOHi(bus->spi.csnPin); + + return true; +} + +bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +{ + IOLo(bus->spi.csnPin); + spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction + spiTransfer(bus->spi.instance, data, NULL, length); + IOHi(bus->spi.csnPin); + + return true; +} diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 9803ae77d9..9e825abadc 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -18,7 +18,8 @@ #pragma once #include "drivers/io_types.h" -#include "rcc_types.h" +#include "drivers/bus.h" +#include "drivers/rcc_types.h" #if defined(STM32F4) || defined(STM32F3) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) @@ -101,3 +102,6 @@ SPIDevice spiDeviceByInstance(SPI_TypeDef *instance); SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance); DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size); #endif + +bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index e33b943214..467d713483 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -21,11 +21,11 @@ #include #include "drivers/bus_spi.h" -#include "dma.h" +#include "drivers/dma.h" #include "drivers/io.h" -#include "io_impl.h" +#include "drivers/io_impl.h" #include "drivers/nvic.h" -#include "rcc.h" +#include "drivers/rcc.h" #ifndef SPI1_SCK_PIN #define SPI1_NSS_PIN PA4 @@ -346,6 +346,26 @@ void spiResetErrorCounter(SPI_TypeDef *instance) spiHardwareMap[device].errorCount = 0; } +bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +{ + IOLo(bus->spi.csnPin); + spiTransferByte(bus->spi.instance, reg); + spiTransferByte(bus->spi.instance, data); + IOHi(bus->spi.csnPin); + + return true; +} + +bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +{ + IOLo(bus->spi.csnPin); + spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction + spiTransfer(bus->spi.instance, data, NULL, length); + IOHi(bus->spi.csnPin); + + return true; +} + void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor) { SPIDevice device = descriptor->userParam; From 79589f99cadccb0559f2852bc8572d3258312802 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Jun 2017 07:38:53 +0100 Subject: [PATCH 28/40] MPU6500 conversion --- src/main/drivers/accgyro/accgyro_mpu.c | 5 ++- .../drivers/accgyro/accgyro_spi_mpu6500.c | 41 +++---------------- .../drivers/accgyro/accgyro_spi_mpu6500.h | 4 -- src/main/drivers/compass/compass_ak8963.c | 22 ++++++++++ 4 files changed, 30 insertions(+), 42 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 3127c011a0..262bbd4ce3 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -248,14 +248,15 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_GYRO_SPI_MPU6500 + gyro->bus.spi.instance = MPU6500_SPI_INSTANCE; gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin; const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus); // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI if (mpu6500Sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = mpu6500Sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister; - gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegister; + gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; } #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 1afecf509a..947cee0c63 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -34,39 +34,8 @@ #include "accgyro_mpu6500.h" #include "accgyro_spi_mpu6500.h" -#define DISABLE_MPU6500(spiCsnPin) IOHi(spiCsnPin) -#define ENABLE_MPU6500(spiCsnPin) IOLo(spiCsnPin) - #define BIT_SLEEP 0x40 -bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) -{ - ENABLE_MPU6500(bus->spi.csnPin); - spiTransferByte(MPU6500_SPI_INSTANCE, reg); - spiTransferByte(MPU6500_SPI_INSTANCE, data); - DISABLE_MPU6500(bus->spi.csnPin); - - return true; -} - -bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data) -{ - mpu6500SpiWriteRegister(bus, reg, data); - delayMicroseconds(10); - - return true; -} - -bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) -{ - ENABLE_MPU6500(bus->spi.csnPin); - spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction - spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length); - DISABLE_MPU6500(bus->spi.csnPin); - - return true; -} - static void mpu6500SpiInit(const busDevice_t *bus) { static bool hardwareInitialised = false; @@ -79,7 +48,7 @@ static void mpu6500SpiInit(const busDevice_t *bus) IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->spi.csnPin); - spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST); hardwareInitialised = true; } @@ -89,7 +58,7 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus) mpu6500SpiInit(bus); uint8_t tmp; - mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); + spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); uint8_t mpuDetected = MPU_NONE; switch (tmp) { @@ -122,16 +91,16 @@ void mpu6500SpiAccInit(accDev_t *acc) void mpu6500SpiGyroInit(gyroDev_t *gyro) { - spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW); + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_SLOW); delayMicroseconds(1); mpu6500GyroInit(gyro); // Disable Primary I2C Interface - mpu6500SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); + spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); delay(100); - spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); delayMicroseconds(1); } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro/accgyro_spi_mpu6500.h index a23d2607c7..40cef8b3c3 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.h @@ -24,9 +24,5 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus); bool mpu6500SpiAccDetect(accDev_t *acc); bool mpu6500SpiGyroDetect(gyroDev_t *gyro); -bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); - void mpu6500SpiGyroInit(gyroDev_t *gyro); void mpu6500SpiAccInit(accDev_t *acc); diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 80c847ff82..db7970cd6b 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -30,6 +30,7 @@ #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" +#include "drivers/io.h" #include "drivers/sensor.h" #include "drivers/time.h" @@ -109,6 +110,27 @@ typedef enum { static queuedReadState_t queuedRead = { false, 0, 0}; +static bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data) +{ + IOLo(bus->spi.csnPin); + spiTransferByte(MPU6500_SPI_INSTANCE, reg); + spiTransferByte(MPU6500_SPI_INSTANCE, data); + IOHi(bus->spi.csnPin); + delayMicroseconds(10); + + return true; +} + +static bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +{ + IOLo(bus->spi.csnPin); + spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction + spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length); + IOHi(bus->spi.csnPin); + + return true; +} + static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) { verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read From aedfa1e49c587ecee4b40b5e5e5d0b635b69bad8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Jun 2017 07:45:20 +0100 Subject: [PATCH 29/40] ICM20689 conversion --- src/main/drivers/accgyro/accgyro_mpu.c | 5 ++- .../drivers/accgyro/accgyro_spi_icm20689.c | 37 ++++--------------- .../drivers/accgyro/accgyro_spi_icm20689.h | 3 -- 3 files changed, 10 insertions(+), 35 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 262bbd4ce3..fb4c181317 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -276,12 +276,13 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_GYRO_SPI_ICM20689 + gyro->bus.spi.instance = ICM20689_SPI_INSTANCE; gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin; if (icm20689SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = icm20689SpiReadRegister; - gyro->mpuConfiguration.writeFn = icm20689SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegister; + gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; } #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index cb73bf4311..d9c81875be 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -36,29 +36,6 @@ #include "accgyro_mpu.h" #include "accgyro_spi_icm20689.h" -#define DISABLE_ICM20689(spiCsnPin) IOHi(spiCsnPin) -#define ENABLE_ICM20689(spiCsnPin) IOLo(spiCsnPin) - -bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) -{ - ENABLE_ICM20689(bus->spi.csnPin); - spiTransferByte(ICM20689_SPI_INSTANCE, reg); - spiTransferByte(ICM20689_SPI_INSTANCE, data); - DISABLE_ICM20689(bus->spi.csnPin); - - return true; -} - -bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) -{ - ENABLE_ICM20689(bus->spi.csnPin); - spiTransferByte(ICM20689_SPI_INSTANCE, reg | 0x80); // read transaction - spiTransfer(ICM20689_SPI_INSTANCE, data, NULL, length); - DISABLE_ICM20689(bus->spi.csnPin); - - return true; -} - static void icm20689SpiInit(const busDevice_t *bus) { static bool hardwareInitialised = false; @@ -71,7 +48,7 @@ static void icm20689SpiInit(const busDevice_t *bus) IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->spi.csnPin); - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); + spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD); hardwareInitialised = true; } @@ -83,14 +60,14 @@ bool icm20689SpiDetect(const busDevice_t *bus) icm20689SpiInit(bus); - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed + spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed - icm20689SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); do { delay(150); - icm20689SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); + spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); if (tmp == ICM20689_WHO_AM_I_CONST) { break; } @@ -99,7 +76,7 @@ bool icm20689SpiDetect(const busDevice_t *bus) } } while (attemptsRemaining--); - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); + spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD); return true; @@ -126,7 +103,7 @@ void icm20689GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); delay(100); @@ -156,7 +133,7 @@ void icm20689GyroInit(gyroDev_t *gyro) gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif - spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_STANDARD); } bool icm20689SpiGyroDetect(gyroDev_t *gyro) diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.h b/src/main/drivers/accgyro/accgyro_spi_icm20689.h index 9eeb0c24a4..164f51decf 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.h +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.h @@ -31,6 +31,3 @@ bool icm20689SpiDetect(const busDevice_t *bus); bool icm20689SpiAccDetect(accDev_t *acc); bool icm20689SpiGyroDetect(gyroDev_t *gyro); - -bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); From bdd876b6aa3546bbdc22c6d821109eafee254eb5 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Jun 2017 08:39:27 +0100 Subject: [PATCH 30/40] MPU9250 conversion --- src/main/drivers/accgyro/accgyro_mpu.c | 5 +- src/main/drivers/accgyro/accgyro_mpu.h | 4 +- .../drivers/accgyro/accgyro_spi_mpu9250.c | 73 ++++++++----------- .../drivers/accgyro/accgyro_spi_mpu9250.h | 1 - src/main/drivers/compass/compass_ak8963.c | 45 +++++------- 5 files changed, 52 insertions(+), 76 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index fb4c181317..e419b1d007 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -262,13 +262,12 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_GYRO_SPI_MPU9250 + gyro->bus.spi.instance = MPU9250_SPI_INSTANCE; gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin; if (mpu9250SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = MPU_9250_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister; - gyro->mpuConfiguration.slowreadFn = mpu9250SpiSlowReadRegister; - gyro->mpuConfiguration.verifywriteFn = verifympu9250SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegister; gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister; gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro; return true; diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 07910797a0..2da4ce1097 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -127,15 +127,13 @@ typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data); typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data); -typedef void(*mpuResetFnPtr)(void); +typedef void (*mpuResetFnPtr)(void); extern mpuResetFnPtr mpuResetFn; typedef struct mpuConfiguration_s { mpuReadRegisterFnPtr readFn; mpuWriteRegisterFnPtr writeFn; - mpuReadRegisterFnPtr slowreadFn; - mpuWriteRegisterFnPtr verifywriteFn; mpuResetFnPtr resetFn; uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each } mpuConfiguration_t; diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 7467bf8a68..8d4953f1e2 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -50,8 +50,29 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro); static bool mpuSpi9250InitDone = false; -#define DISABLE_MPU9250(spiCsnPin) IOHi(spiCsnPin) -#define ENABLE_MPU9250(spiCsnPin) IOLo(spiCsnPin) +bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +{ + IOLo(bus->spi.csnPin); + delayMicroseconds(1); + spiTransferByte(bus->spi.instance, reg); + spiTransferByte(bus->spi.instance, data); + IOHi(bus->spi.csnPin); + delayMicroseconds(1); + + return true; +} + +static bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +{ + IOLo(bus->spi.csnPin); + delayMicroseconds(1); + spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction + spiTransfer(bus->spi.instance, data, NULL, length); + IOHi(bus->spi.csnPin); + delayMicroseconds(1); + + return true; +} void mpu9250SpiResetGyro(void) { @@ -63,49 +84,15 @@ void mpu9250SpiResetGyro(void) #endif } -bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) -{ - ENABLE_MPU9250(bus->spi.csnPin); - delayMicroseconds(1); - spiTransferByte(MPU9250_SPI_INSTANCE, reg); - spiTransferByte(MPU9250_SPI_INSTANCE, data); - DISABLE_MPU9250(bus->spi.csnPin); - delayMicroseconds(1); - - return true; -} - -bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) -{ - ENABLE_MPU9250(bus->spi.csnPin); - spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction - spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); - DISABLE_MPU9250(bus->spi.csnPin); - - return true; -} - -bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) -{ - ENABLE_MPU9250(bus->spi.csnPin); - delayMicroseconds(1); - spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction - spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); - DISABLE_MPU9250(bus->spi.csnPin); - delayMicroseconds(1); - - return true; -} - void mpu9250SpiGyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); mpu9250AccAndGyroInit(gyro); - spiResetErrorCounter(MPU9250_SPI_INSTANCE); + spiResetErrorCounter(gyro->bus.spi.instance); - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers mpuGyroRead(gyro); @@ -147,7 +134,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { return; } - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(50); @@ -175,7 +162,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. #endif - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); mpuSpi9250InitDone = true; //init done } @@ -188,13 +175,13 @@ bool mpu9250SpiDetect(const busDevice_t *bus) IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed + spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); do { delay(150); - mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in); + spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in); if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) { break; } @@ -203,7 +190,7 @@ bool mpu9250SpiDetect(const busDevice_t *bus) } } while (attemptsRemaining--); - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST); return true; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h index 4518bd8e26..71080cd69e 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h @@ -36,4 +36,3 @@ bool mpu9250SpiGyroDetect(gyroDev_t *gyro); bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); -bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index db7970cd6b..9c778d40e2 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -28,6 +28,7 @@ #include "common/maths.h" #include "common/utils.h" +#include "drivers/bus.h" #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" #include "drivers/io.h" @@ -90,8 +91,16 @@ static busDevice_t *bus = NULL; // HACK #if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE) #define MPU9250_SPI_INSTANCE #define verifympu9250SpiWriteRegister mpu6500SpiWriteRegisterDelayed -#define mpu9250SpiWriteRegister mpu6500SpiWriteRegister -#define mpu9250SpiReadRegister mpu6500SpiReadRegister +static bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data) +{ + IOLo(bus->spi.csnPin); + spiTransferByte(bus->spi.instance, reg); + spiTransferByte(bus->spi.instance, data); + IOHi(bus->spi.csnPin); + delayMicroseconds(10); + + return true; +} #endif #if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) @@ -110,27 +119,6 @@ typedef enum { static queuedReadState_t queuedRead = { false, 0, 0}; -static bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data) -{ - IOLo(bus->spi.csnPin); - spiTransferByte(MPU6500_SPI_INSTANCE, reg); - spiTransferByte(MPU6500_SPI_INSTANCE, data); - IOHi(bus->spi.csnPin); - delayMicroseconds(10); - - return true; -} - -static bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) -{ - IOLo(bus->spi.csnPin); - spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction - spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length); - IOHi(bus->spi.csnPin); - - return true; -} - static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) { verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read @@ -138,7 +126,7 @@ static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes delay(10); __disable_irq(); - bool ack = mpu9250SpiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C + bool ack = spiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C __enable_irq(); return ack; } @@ -197,7 +185,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf) queuedRead.waiting = false; - mpu9250SpiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer + spiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer return true; } #else @@ -339,7 +327,11 @@ bool ak8963Detect(magDev_t *mag) bus = &mag->bus; -#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) +#if defined(USE_SPI) +#if defined(MPU6500_SPI_INSTANCE) + bus->spi.instance = MPU6500_SPI_INSTANCE; +#elif defined(MPU9250_SPI_INSTANCE) + bus->spi.instance = MPU9250_SPI_INSTANCE; // initialze I2C master via SPI bus (MPU9250) verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); @@ -350,6 +342,7 @@ bool ak8963Detect(magDev_t *mag) verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only delay(10); +#endif #endif // check for AK8963 From 5d7f8cdd6e5b34ff3bd2312cb079b17073707ee6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Jun 2017 09:25:59 +0100 Subject: [PATCH 31/40] Function renaming as per ledvinap's suggestion --- src/main/drivers/accgyro/accgyro_mpu.c | 16 ++--- .../drivers/accgyro/accgyro_spi_icm20689.c | 11 ++-- .../drivers/accgyro/accgyro_spi_mpu6000.c | 62 +++++++++---------- .../drivers/accgyro/accgyro_spi_mpu6500.c | 7 +-- .../drivers/accgyro/accgyro_spi_mpu9250.c | 48 +++++++------- .../drivers/accgyro/accgyro_spi_mpu9250.h | 6 +- src/main/drivers/bus_spi.c | 15 ++++- src/main/drivers/bus_spi.h | 5 +- src/main/drivers/bus_spi_hal.c | 15 ++++- src/main/drivers/compass/compass_ak8963.c | 34 +++++----- 10 files changed, 116 insertions(+), 103 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index e419b1d007..b7f47e5e29 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -241,8 +241,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu6000SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = spiReadRegister; - gyro->mpuConfiguration.writeFn = spiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegBuf; + gyro->mpuConfiguration.writeFn = spiWriteReg; return true; } #endif @@ -255,8 +255,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu6500Sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = mpu6500Sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = spiReadRegister; - gyro->mpuConfiguration.writeFn = spiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegBuf; + gyro->mpuConfiguration.writeFn = spiWriteReg; return true; } #endif @@ -267,8 +267,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu9250SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = MPU_9250_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = spiReadRegister; - gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegBuf; + gyro->mpuConfiguration.writeFn = mpu9250SpiWriteReg; gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro; return true; } @@ -280,8 +280,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (icm20689SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = spiReadRegister; - gyro->mpuConfiguration.writeFn = spiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegBuf; + gyro->mpuConfiguration.writeFn = spiWriteReg; return true; } #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index d9c81875be..54eefdf532 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -55,20 +55,17 @@ static void icm20689SpiInit(const busDevice_t *bus) bool icm20689SpiDetect(const busDevice_t *bus) { - uint8_t tmp; - uint8_t attemptsRemaining = 20; - icm20689SpiInit(bus); spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed - spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + spiWriteReg(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + uint8_t attemptsRemaining = 20; do { delay(150); - - spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); - if (tmp == ICM20689_WHO_AM_I_CONST) { + const uint8_t whoAmI = spiReadReg(bus, MPU_RA_WHO_AM_I); + if (whoAmI == ICM20689_WHO_AM_I_CONST) { break; } if (!attemptsRemaining) { diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 006f32eebd..5dd924d4b5 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -108,7 +108,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro) spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); // Accel and Gyro DLPF Setting - spiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf); + spiWriteReg(&gyro->bus, MPU6000_CONFIG, gyro->lpf); delayMicroseconds(1); spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); // 18 MHz SPI clock @@ -127,22 +127,20 @@ void mpu6000SpiAccInit(accDev_t *acc) bool mpu6000SpiDetect(const busDevice_t *bus) { - uint8_t in; - uint8_t attemptsRemaining = 5; - IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->spi.csnPin); spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); - spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); + spiWriteReg(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); + uint8_t attemptsRemaining = 5; do { delay(150); - spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in); - if (in == MPU6000_WHO_AM_I_CONST) { + const uint8_t whoAmI = spiReadReg(bus, MPU_RA_WHO_AM_I); + if (whoAmI == MPU6000_WHO_AM_I_CONST) { break; } if (!attemptsRemaining) { @@ -150,25 +148,25 @@ bool mpu6000SpiDetect(const busDevice_t *bus) } } while (attemptsRemaining--); - spiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in); + const uint8_t productID = spiReadReg(bus, MPU_RA_PRODUCT_ID); /* look for a product ID we recognise */ // verify product revision - switch (in) { - case MPU6000ES_REV_C4: - case MPU6000ES_REV_C5: - case MPU6000_REV_C4: - case MPU6000_REV_C5: - case MPU6000ES_REV_D6: - case MPU6000ES_REV_D7: - case MPU6000ES_REV_D8: - case MPU6000_REV_D6: - case MPU6000_REV_D7: - case MPU6000_REV_D8: - case MPU6000_REV_D9: - case MPU6000_REV_D10: - return true; + switch (productID) { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + case MPU6000ES_REV_D6: + case MPU6000ES_REV_D7: + case MPU6000ES_REV_D8: + case MPU6000_REV_D6: + case MPU6000_REV_D7: + case MPU6000_REV_D8: + case MPU6000_REV_D9: + case MPU6000_REV_D10: + return true; } return false; @@ -183,41 +181,41 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro) spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); // Device Reset - spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); + spiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(150); - spiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); + spiWriteReg(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); delay(150); // Clock Source PPL with Z axis gyro reference - spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + spiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); delayMicroseconds(15); // Disable Primary I2C Interface - spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); + spiWriteReg(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); delayMicroseconds(15); - spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00); + spiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00); delayMicroseconds(15); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled - spiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); + spiWriteReg(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); delayMicroseconds(15); // Gyro +/- 1000 DPS Full Scale - spiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + spiWriteReg(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); delayMicroseconds(15); // Accel +/- 8 G Full Scale - spiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + spiWriteReg(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); delayMicroseconds(15); - spiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR + spiWriteReg(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR delayMicroseconds(15); #ifdef USE_MPU_DATA_READY_SIGNAL - spiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + spiWriteReg(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); delayMicroseconds(15); #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 947cee0c63..85dc688b28 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -57,11 +57,10 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus) { mpu6500SpiInit(bus); - uint8_t tmp; - spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); + const uint8_t whoAmI = spiReadReg(bus, MPU_RA_WHO_AM_I); uint8_t mpuDetected = MPU_NONE; - switch (tmp) { + switch (whoAmI) { case MPU6500_WHO_AM_I_CONST: mpuDetected = MPU_65xx_SPI; break; @@ -97,7 +96,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro) mpu6500GyroInit(gyro); // Disable Primary I2C Interface - spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); + spiWriteReg(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); delay(100); spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 8d4953f1e2..8f96184be5 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -50,7 +50,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro); static bool mpuSpi9250InitDone = false; -bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool mpu9250SpiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->spi.csnPin); delayMicroseconds(1); @@ -62,7 +62,7 @@ bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) return true; } -static bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +static bool mpu9250SpiSlowReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { IOLo(bus->spi.csnPin); delayMicroseconds(1); @@ -79,7 +79,7 @@ void mpu9250SpiResetGyro(void) // Device Reset #ifdef MPU9250_CS_PIN busDevice_t bus = { .spi = { .csnPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)) } }; - mpu9250SpiWriteRegister(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + mpu9250SpiWriteReg(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(150); #endif } @@ -107,21 +107,20 @@ void mpu9250SpiAccInit(accDev_t *acc) acc->acc_1G = 512 * 8; } -bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool mpu9250SpiWriteRegVerify(const busDevice_t *bus, uint8_t reg, uint8_t data) { - uint8_t in; - uint8_t attemptsRemaining = 20; - - mpu9250SpiWriteRegister(bus, reg, data); + mpu9250SpiWriteReg(bus, reg, data); delayMicroseconds(100); + uint8_t attemptsRemaining = 20; do { - mpu9250SpiSlowReadRegister(bus, reg, 1, &in); + uint8_t in; + mpu9250SpiSlowReadRegBuf(bus, reg, 1, &in); if (in == data) { return true; } else { debug[3]++; - mpu9250SpiWriteRegister(bus, reg, data); + mpu9250SpiWriteReg(bus, reg, data); delayMicroseconds(100); } } while (attemptsRemaining--); @@ -136,30 +135,30 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers - mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + mpu9250SpiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(50); - verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); //Fchoice_b defaults to 00 which makes fchoice 11 const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); + mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); if (gyro->lpf == 4) { - verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF + mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF } else if (gyro->lpf < 4) { - verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF + mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF } else if (gyro->lpf > 4) { - verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF + mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF } - verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); + mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); - verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); - verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN #if defined(USE_MPU_DATA_READY_SIGNAL) - verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. + mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. #endif spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); @@ -169,19 +168,16 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { bool mpu9250SpiDetect(const busDevice_t *bus) { - uint8_t in; - uint8_t attemptsRemaining = 20; - IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed - mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + mpu9250SpiWriteReg(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + uint8_t attemptsRemaining = 20; do { delay(150); - - spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in); + const uint8_t in = spiReadReg(bus, MPU_RA_WHO_AM_I); if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) { break; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h index 71080cd69e..c9a07e9af1 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h @@ -33,6 +33,6 @@ bool mpu9250SpiDetect(const busDevice_t *bus); bool mpu9250SpiAccDetect(accDev_t *acc); bool mpu9250SpiGyroDetect(gyroDev_t *gyro); -bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); +bool mpu9250SpiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool mpu9250SpiWriteRegVerify(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool mpu9250SpiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 9022ecde14..ce3fdce1af 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -353,7 +353,7 @@ void spiResetErrorCounter(SPI_TypeDef *instance) spiHardwareMap[device].errorCount = 0; } -bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->spi.csnPin); spiTransferByte(bus->spi.instance, reg); @@ -363,7 +363,7 @@ bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) return true; } -bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { IOLo(bus->spi.csnPin); spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction @@ -372,3 +372,14 @@ bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_ return true; } + +uint8_t spiReadReg(const busDevice_t *bus, uint8_t reg) +{ + uint8_t data; + IOLo(bus->spi.csnPin); + spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction + spiTransfer(bus->spi.instance, &data, NULL, 1); + IOHi(bus->spi.csnPin); + + return data; +} diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 9e825abadc..1546de0baa 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -103,5 +103,6 @@ SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance); DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size); #endif -bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); +bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); +uint8_t spiReadReg(const busDevice_t *bus, uint8_t reg); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 467d713483..2a1711f2e1 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -346,7 +346,7 @@ void spiResetErrorCounter(SPI_TypeDef *instance) spiHardwareMap[device].errorCount = 0; } -bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->spi.csnPin); spiTransferByte(bus->spi.instance, reg); @@ -356,7 +356,7 @@ bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) return true; } -bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { IOLo(bus->spi.csnPin); spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction @@ -366,6 +366,17 @@ bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_ return true; } +uint8_t spiReadReg(const busDevice_t *bus, uint8_t reg) +{ + uint8_t data; + IOLo(bus->spi.csnPin); + spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction + spiTransfer(bus->spi.instance, &data, NULL, 1); + IOHi(bus->spi.csnPin); + + return data; +} + void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor) { SPIDevice device = descriptor->userParam; diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 9c778d40e2..d8f23b9479 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -90,8 +90,8 @@ static busDevice_t *bus = NULL; // HACK // Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same. #if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE) #define MPU9250_SPI_INSTANCE -#define verifympu9250SpiWriteRegister mpu6500SpiWriteRegisterDelayed -static bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data) +#define mpu9250SpiWriteRegVerify mpu6500SpiWriteRegDelayed +static bool mpu6500SpiWriteRegDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->spi.csnPin); spiTransferByte(bus->spi.instance, reg); @@ -121,22 +121,22 @@ static queuedReadState_t queuedRead = { false, 0, 0}; static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) { - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes delay(10); __disable_irq(); - bool ack = spiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C + bool ack = spiReadRegBuf(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C __enable_irq(); return ack; } static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) { - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte return true; } @@ -148,9 +148,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) queuedRead.len = len_; - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes queuedRead.readStartedAt = micros(); queuedRead.waiting = true; @@ -185,7 +185,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf) queuedRead.waiting = false; - spiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer + spiReadRegBuf(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer return true; } #else @@ -334,13 +334,13 @@ bool ak8963Detect(magDev_t *mag) bus->spi.instance = MPU9250_SPI_INSTANCE; // initialze I2C master via SPI bus (MPU9250) - verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); + mpu9250SpiWriteRegVerify(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); delay(10); - verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz + mpu9250SpiWriteRegVerify(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz delay(10); - verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only + mpu9250SpiWriteRegVerify(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only delay(10); #endif #endif From 1c7777ecc5a23807c28ad5c0a5a8a972cbcf0f5a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Jun 2017 10:35:18 +0100 Subject: [PATCH 32/40] BMI160 conversion --- src/main/drivers/accgyro/accgyro_mpu.c | 5 +- src/main/drivers/accgyro/accgyro_spi_bmi160.c | 79 +++++-------------- src/main/drivers/accgyro/accgyro_spi_bmi160.h | 3 - 3 files changed, 22 insertions(+), 65 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index b7f47e5e29..710bf98e07 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -287,11 +287,12 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_ACCGYRO_BMI160 + gyro->bus.spi.instance = BMI160_SPI_INSTANCE; gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin; if (bmi160Detect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = BMI_160_SPI; - gyro->mpuConfiguration.readFn = bmi160SpiReadRegister; - gyro->mpuConfiguration.writeFn = bmi160SpiWriteRegister; + gyro->mpuConfiguration.readFn = spiReadRegBuf; + gyro->mpuConfiguration.writeFn = spiWriteReg; return true; } #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index 835ab25b2f..6deade00a4 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -90,29 +90,27 @@ static volatile bool bmi160ExtiInitDone = false; //! Private functions static int32_t BMI160_Config(const busDevice_t *bus); static int32_t BMI160_do_foc(const busDevice_t *bus); -static uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg); static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data); -#define DISABLE_BMI160(spiCsnPin) IOHi(spiCsnPin) -#define ENABLE_BMI160(spiCsnPin) IOLo(spiCsnPin) - bool bmi160Detect(const busDevice_t *bus) { - if (BMI160Detected) + if (BMI160Detected) { return true; + } + IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->spi.csnPin); - spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR); + spiSetDivisor(bus->spi.instance, BMI160_SPI_DIVISOR); /* Read this address to acticate SPI (see p. 84) */ - BMI160_ReadReg(bus, 0x7F); + spiReadReg(bus, 0x7F); delay(10); // Give SPI some time to start up /* Check the chip ID */ - if (BMI160_ReadReg(bus, BMI160_REG_CHIPID) != 0xd1){ + if (spiReadReg(bus, BMI160_REG_CHIPID) != 0xd1){ return false; } @@ -127,8 +125,9 @@ bool bmi160Detect(const busDevice_t *bus) */ static void BMI160_Init(const busDevice_t *bus) { - if (BMI160InitDone || !BMI160Detected) + if (BMI160InitDone || !BMI160Detected) { return; + } /* Configure the BMI160 Sensor */ if (BMI160_Config(bus) != 0){ @@ -164,7 +163,7 @@ static int32_t BMI160_Config(const busDevice_t *bus) delay(5); // can take up to 3.8ms // Verify that normal power mode was entered - uint8_t pmu_status = BMI160_ReadReg(bus, BMI160_REG_PMU_STAT); + uint8_t pmu_status = spiReadReg(bus, BMI160_REG_PMU_STAT); if ((pmu_status & 0x3C) != 0x14){ return -3; } @@ -193,7 +192,7 @@ static int32_t BMI160_Config(const busDevice_t *bus) delay(1); // Enable offset compensation - uint8_t val = BMI160_ReadReg(bus, BMI160_REG_OFFSET_0); + uint8_t val = spiReadReg(bus, BMI160_REG_OFFSET_0); if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0){ return -7; } @@ -234,7 +233,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus) // Wait for FOC to complete for (int i=0; i<50; i++) { - val = BMI160_ReadReg(bus, BMI160_REG_STATUS); + val = spiReadReg(bus, BMI160_REG_STATUS); if (val & BMI160_REG_STATUS_FOC_RDY) { break; } @@ -245,7 +244,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus) } // Program NVM - val = BMI160_ReadReg(bus, BMI160_REG_CONF); + val = spiReadReg(bus, BMI160_REG_CONF); if (BMI160_WriteReg(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) { return -4; } @@ -256,7 +255,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus) // Wait for NVM programming to complete for (int i=0; i<50; i++) { - val = BMI160_ReadReg(bus, BMI160_REG_STATUS); + val = spiReadReg(bus, BMI160_REG_STATUS); if (val & BMI160_REG_STATUS_NVM_RDY) { break; } @@ -269,41 +268,6 @@ static int32_t BMI160_do_foc(const busDevice_t *bus) return 0; } -/** - * @brief Read a register from BMI160 - * @returns The register value - * @param reg[in] Register address to be read - */ -uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg) -{ - uint8_t data; - - ENABLE_BMI160(bus->spi.csnPin); - - spiTransferByte(BMI160_SPI_INSTANCE, 0x80 | reg); // request byte - spiTransfer(BMI160_SPI_INSTANCE, &data, NULL, 1); // receive response - - DISABLE_BMI160(bus->spi.csnPin); - - return data; -} - -bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) -{ - ENABLE_BMI160(bus->spi.csnPin); - spiTransferByte(BMI160_SPI_INSTANCE, reg | 0x80); // read transaction - spiTransfer(BMI160_SPI_INSTANCE, data, NULL, length); - ENABLE_BMI160(bus->spi.csnPin); - - return true; -} - -/** - * @brief Writes one byte to the BMI160 register - * \param[in] reg Register address - * \param[in] data Byte to write - * @returns 0 when success - */ static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) { ENABLE_BMI160(bus->spi.csnPin); @@ -316,11 +280,6 @@ static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data return 0; } -bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) -{ - return BMI160_WriteReg(bus, reg, data); -} - extiCallbackRec_t bmi160IntCallbackRec; void bmi160ExtiHandler(extiCallbackRec_t *cb) @@ -366,9 +325,9 @@ bool bmi160AccRead(accDev_t *acc) uint8_t bmi160_rec_buf[BUFFER_SIZE]; uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; - ENABLE_BMI160(acc->bus.spi.csnPin); - spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response - DISABLE_BMI160(acc->bus.spi.csnPin); + IOLo(acc->bus.spi.csnPin); + spiTransfer(acc->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response + IOHi(acc->bus.spi.csnPin); acc->ADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_XOUT_L]); acc->ADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_YOUT_L]); @@ -394,9 +353,9 @@ bool bmi160GyroRead(gyroDev_t *gyro) uint8_t bmi160_rec_buf[BUFFER_SIZE]; uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; - ENABLE_BMI160(gyro->bus.spi.csnPin); - spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response - DISABLE_BMI160(gyro->bus.spi.csnPin); + IOLo(gyro->bus.spi.csnPin); + spiTransfer(gyro->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response + IOHi(gyro->bus.spi.csnPin); gyro->gyroADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_XOUT_L]); gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_YOUT_L]); diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.h b/src/main/drivers/accgyro/accgyro_spi_bmi160.h index 1c51c23c93..ad5c6e1e24 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.h +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.h @@ -68,9 +68,6 @@ enum bmi160_gyro_range { BMI160_RANGE_2000DPS = 0x00, }; -bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); - bool bmi160Detect(const busDevice_t *bus); bool bmi160SpiAccDetect(accDev_t *acc); bool bmi160SpiGyroDetect(gyroDev_t *gyro); From 6157ebe5e3df8e374bb74ef28ea27c533594818a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Jun 2017 10:48:32 +0100 Subject: [PATCH 33/40] Removed abbreviations from SPI read/write functions --- src/main/drivers/accgyro/accgyro_mpu.c | 20 +++++----- src/main/drivers/accgyro/accgyro_spi_bmi160.c | 14 +++---- .../drivers/accgyro/accgyro_spi_icm20689.c | 4 +- .../drivers/accgyro/accgyro_spi_mpu6000.c | 28 +++++++------- .../drivers/accgyro/accgyro_spi_mpu6500.c | 4 +- .../drivers/accgyro/accgyro_spi_mpu9250.c | 38 +++++++++---------- .../drivers/accgyro/accgyro_spi_mpu9250.h | 4 +- src/main/drivers/bus_spi.c | 6 +-- src/main/drivers/bus_spi.h | 6 +-- src/main/drivers/bus_spi_hal.c | 6 +-- src/main/drivers/compass/compass_ak8963.c | 32 ++++++++-------- 11 files changed, 81 insertions(+), 81 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 710bf98e07..bab5813918 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -241,8 +241,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu6000SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = spiReadRegBuf; - gyro->mpuConfiguration.writeFn = spiWriteReg; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; } #endif @@ -255,8 +255,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu6500Sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = mpu6500Sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = spiReadRegBuf; - gyro->mpuConfiguration.writeFn = spiWriteReg; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; } #endif @@ -267,8 +267,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu9250SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = MPU_9250_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = spiReadRegBuf; - gyro->mpuConfiguration.writeFn = mpu9250SpiWriteReg; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiWriteRegister; gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro; return true; } @@ -280,8 +280,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (icm20689SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = spiReadRegBuf; - gyro->mpuConfiguration.writeFn = spiWriteReg; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; } #endif @@ -291,8 +291,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin; if (bmi160Detect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = BMI_160_SPI; - gyro->mpuConfiguration.readFn = spiReadRegBuf; - gyro->mpuConfiguration.writeFn = spiWriteReg; + gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; } #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index 6deade00a4..ee3077421c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -106,11 +106,11 @@ bool bmi160Detect(const busDevice_t *bus) spiSetDivisor(bus->spi.instance, BMI160_SPI_DIVISOR); /* Read this address to acticate SPI (see p. 84) */ - spiReadReg(bus, 0x7F); + spiReadRegister(bus, 0x7F); delay(10); // Give SPI some time to start up /* Check the chip ID */ - if (spiReadReg(bus, BMI160_REG_CHIPID) != 0xd1){ + if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1){ return false; } @@ -163,7 +163,7 @@ static int32_t BMI160_Config(const busDevice_t *bus) delay(5); // can take up to 3.8ms // Verify that normal power mode was entered - uint8_t pmu_status = spiReadReg(bus, BMI160_REG_PMU_STAT); + uint8_t pmu_status = spiReadRegister(bus, BMI160_REG_PMU_STAT); if ((pmu_status & 0x3C) != 0x14){ return -3; } @@ -192,7 +192,7 @@ static int32_t BMI160_Config(const busDevice_t *bus) delay(1); // Enable offset compensation - uint8_t val = spiReadReg(bus, BMI160_REG_OFFSET_0); + uint8_t val = spiReadRegister(bus, BMI160_REG_OFFSET_0); if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0){ return -7; } @@ -233,7 +233,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus) // Wait for FOC to complete for (int i=0; i<50; i++) { - val = spiReadReg(bus, BMI160_REG_STATUS); + val = spiReadRegister(bus, BMI160_REG_STATUS); if (val & BMI160_REG_STATUS_FOC_RDY) { break; } @@ -244,7 +244,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus) } // Program NVM - val = spiReadReg(bus, BMI160_REG_CONF); + val = spiReadRegister(bus, BMI160_REG_CONF); if (BMI160_WriteReg(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) { return -4; } @@ -255,7 +255,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus) // Wait for NVM programming to complete for (int i=0; i<50; i++) { - val = spiReadReg(bus, BMI160_REG_STATUS); + val = spiReadRegister(bus, BMI160_REG_STATUS); if (val & BMI160_REG_STATUS_NVM_RDY) { break; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 54eefdf532..762ac1a61b 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -59,12 +59,12 @@ bool icm20689SpiDetect(const busDevice_t *bus) spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed - spiWriteReg(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); uint8_t attemptsRemaining = 20; do { delay(150); - const uint8_t whoAmI = spiReadReg(bus, MPU_RA_WHO_AM_I); + const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I); if (whoAmI == ICM20689_WHO_AM_I_CONST) { break; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 5dd924d4b5..94a9e2e561 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -108,7 +108,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro) spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); // Accel and Gyro DLPF Setting - spiWriteReg(&gyro->bus, MPU6000_CONFIG, gyro->lpf); + spiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf); delayMicroseconds(1); spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); // 18 MHz SPI clock @@ -133,13 +133,13 @@ bool mpu6000SpiDetect(const busDevice_t *bus) spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); - spiWriteReg(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); + spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); uint8_t attemptsRemaining = 5; do { delay(150); - const uint8_t whoAmI = spiReadReg(bus, MPU_RA_WHO_AM_I); + const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I); if (whoAmI == MPU6000_WHO_AM_I_CONST) { break; } @@ -148,7 +148,7 @@ bool mpu6000SpiDetect(const busDevice_t *bus) } } while (attemptsRemaining--); - const uint8_t productID = spiReadReg(bus, MPU_RA_PRODUCT_ID); + const uint8_t productID = spiReadRegister(bus, MPU_RA_PRODUCT_ID); /* look for a product ID we recognise */ @@ -181,41 +181,41 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro) spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); // Device Reset - spiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); + spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(150); - spiWriteReg(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); + spiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); delay(150); // Clock Source PPL with Z axis gyro reference - spiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); delayMicroseconds(15); // Disable Primary I2C Interface - spiWriteReg(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); + spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); delayMicroseconds(15); - spiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00); + spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00); delayMicroseconds(15); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled - spiWriteReg(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); + spiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); delayMicroseconds(15); // Gyro +/- 1000 DPS Full Scale - spiWriteReg(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + spiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); delayMicroseconds(15); // Accel +/- 8 G Full Scale - spiWriteReg(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + spiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); delayMicroseconds(15); - spiWriteReg(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR + spiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR delayMicroseconds(15); #ifdef USE_MPU_DATA_READY_SIGNAL - spiWriteReg(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + spiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); delayMicroseconds(15); #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 85dc688b28..8f2f4db210 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -57,7 +57,7 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus) { mpu6500SpiInit(bus); - const uint8_t whoAmI = spiReadReg(bus, MPU_RA_WHO_AM_I); + const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I); uint8_t mpuDetected = MPU_NONE; switch (whoAmI) { @@ -96,7 +96,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro) mpu6500GyroInit(gyro); // Disable Primary I2C Interface - spiWriteReg(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); + spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); delay(100); spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 8f96184be5..1df2200467 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -50,7 +50,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro); static bool mpuSpi9250InitDone = false; -bool mpu9250SpiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->spi.csnPin); delayMicroseconds(1); @@ -62,7 +62,7 @@ bool mpu9250SpiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) return true; } -static bool mpu9250SpiSlowReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +static bool mpu9250SpiSlowReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { IOLo(bus->spi.csnPin); delayMicroseconds(1); @@ -79,7 +79,7 @@ void mpu9250SpiResetGyro(void) // Device Reset #ifdef MPU9250_CS_PIN busDevice_t bus = { .spi = { .csnPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)) } }; - mpu9250SpiWriteReg(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + mpu9250SpiWriteRegister(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(150); #endif } @@ -107,20 +107,20 @@ void mpu9250SpiAccInit(accDev_t *acc) acc->acc_1G = 512 * 8; } -bool mpu9250SpiWriteRegVerify(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data) { - mpu9250SpiWriteReg(bus, reg, data); + mpu9250SpiWriteRegister(bus, reg, data); delayMicroseconds(100); uint8_t attemptsRemaining = 20; do { uint8_t in; - mpu9250SpiSlowReadRegBuf(bus, reg, 1, &in); + mpu9250SpiSlowReadRegisterBuffer(bus, reg, 1, &in); if (in == data) { return true; } else { debug[3]++; - mpu9250SpiWriteReg(bus, reg, data); + mpu9250SpiWriteRegister(bus, reg, data); delayMicroseconds(100); } } while (attemptsRemaining--); @@ -135,30 +135,30 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers - mpu9250SpiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(50); - mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); //Fchoice_b defaults to 00 which makes fchoice 11 const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); if (gyro->lpf == 4) { - mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF } else if (gyro->lpf < 4) { - mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF } else if (gyro->lpf > 4) { - mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF } - mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); - mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); - mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN #if defined(USE_MPU_DATA_READY_SIGNAL) - mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. #endif spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); @@ -172,12 +172,12 @@ bool mpu9250SpiDetect(const busDevice_t *bus) IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed - mpu9250SpiWriteReg(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); uint8_t attemptsRemaining = 20; do { delay(150); - const uint8_t in = spiReadReg(bus, MPU_RA_WHO_AM_I); + const uint8_t in = spiReadRegister(bus, MPU_RA_WHO_AM_I); if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) { break; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h index c9a07e9af1..a9d9b1b0d6 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h @@ -33,6 +33,6 @@ bool mpu9250SpiDetect(const busDevice_t *bus); bool mpu9250SpiAccDetect(accDev_t *acc); bool mpu9250SpiGyroDetect(gyroDev_t *gyro); -bool mpu9250SpiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool mpu9250SpiWriteRegVerify(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data); bool mpu9250SpiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index ce3fdce1af..0eb3815096 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -353,7 +353,7 @@ void spiResetErrorCounter(SPI_TypeDef *instance) spiHardwareMap[device].errorCount = 0; } -bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->spi.csnPin); spiTransferByte(bus->spi.instance, reg); @@ -363,7 +363,7 @@ bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) return true; } -bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { IOLo(bus->spi.csnPin); spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction @@ -373,7 +373,7 @@ bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t return true; } -uint8_t spiReadReg(const busDevice_t *bus, uint8_t reg) +uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg) { uint8_t data; IOLo(bus->spi.csnPin); diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 1546de0baa..8c6ad6f40e 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -103,6 +103,6 @@ SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance); DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size); #endif -bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); -uint8_t spiReadReg(const busDevice_t *bus, uint8_t reg); +bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); +uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 2a1711f2e1..1d0011e809 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -346,7 +346,7 @@ void spiResetErrorCounter(SPI_TypeDef *instance) spiHardwareMap[device].errorCount = 0; } -bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) +bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->spi.csnPin); spiTransferByte(bus->spi.instance, reg); @@ -356,7 +356,7 @@ bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) return true; } -bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { IOLo(bus->spi.csnPin); spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction @@ -366,7 +366,7 @@ bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t return true; } -uint8_t spiReadReg(const busDevice_t *bus, uint8_t reg) +uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg) { uint8_t data; IOLo(bus->spi.csnPin); diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index d8f23b9479..8523dfb1c0 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -90,7 +90,7 @@ static busDevice_t *bus = NULL; // HACK // Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same. #if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE) #define MPU9250_SPI_INSTANCE -#define mpu9250SpiWriteRegVerify mpu6500SpiWriteRegDelayed +#define mpu9250SpiWriteRegisterVerify mpu6500SpiWriteRegDelayed static bool mpu6500SpiWriteRegDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->spi.csnPin); @@ -121,22 +121,22 @@ static queuedReadState_t queuedRead = { false, 0, 0}; static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) { - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes delay(10); __disable_irq(); - bool ack = spiReadRegBuf(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C + bool ack = spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C __enable_irq(); return ack; } static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) { - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte return true; } @@ -148,9 +148,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) queuedRead.len = len_; - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes queuedRead.readStartedAt = micros(); queuedRead.waiting = true; @@ -185,7 +185,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf) queuedRead.waiting = false; - spiReadRegBuf(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer + spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer return true; } #else @@ -334,13 +334,13 @@ bool ak8963Detect(magDev_t *mag) bus->spi.instance = MPU9250_SPI_INSTANCE; // initialze I2C master via SPI bus (MPU9250) - mpu9250SpiWriteRegVerify(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); + mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); delay(10); - mpu9250SpiWriteRegVerify(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz + mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz delay(10); - mpu9250SpiWriteRegVerify(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only + mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only delay(10); #endif #endif From 1ff5ff7d98e19ac5f08d006ed4b27dd853fd6d99 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 2 Jul 2017 04:40:19 +1000 Subject: [PATCH 34/40] Remove SITL specific defines from fc_init Cleanup target.c indentation. --- src/main/fc/fc_init.c | 10 +- src/main/target/SITL/target.c | 540 ++++++++++++++++++---------------- 2 files changed, 291 insertions(+), 259 deletions(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index ff4bebf42c..90489fffd6 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -315,7 +315,7 @@ void init(void) } #endif -#if defined(USE_SPEKTRUM_BIND) && !defined(SITL) +#if defined(USE_SPEKTRUM_BIND) if (feature(FEATURE_RX_SERIAL)) { switch (rxConfig()->serialrx_provider) { case SERIALRX_SPEKTRUM1024: @@ -338,7 +338,7 @@ void init(void) busSwitchInit(); #endif -#if defined(USE_UART) && !defined(SITL) +#if defined(USE_UART) uartPinConfigure(serialPinConfig()); #endif @@ -384,12 +384,12 @@ void init(void) if (0) {} #if defined(USE_PPM) else if (feature(FEATURE_RX_PPM)) { - ppmRxInit(ppmConfig()); + ppmRxInit(ppmConfig()); } #endif #if defined(USE_PWM) else if (feature(FEATURE_RX_PARALLEL_PWM)) { - pwmRxInit(pwmConfig()); + pwmRxInit(pwmConfig()); } #endif @@ -478,10 +478,8 @@ void init(void) initBoardAlignment(boardAlignment()); if (!sensorsAutodetect()) { -#if !defined(SITL) // if gyro was not detected due to whatever reason, notify and don't arm. failureLedCode(FAILURE_MISSING_ACC, 2); -#endif setArmingDisabled(ARMING_DISABLED_NO_GYRO); } diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index efe3e18657..804ebe7b3d 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -43,6 +43,8 @@ const timerHardware_t timerHardware[1]; // unused #include "fc/config.h" #include "scheduler/scheduler.h" +#include "rx/rx.h" + #include "dyad.h" #include "target/SITL/udplink.h" @@ -60,259 +62,270 @@ static pthread_mutex_t mainLoopLock; int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y); int lockMainPID(void) { - return pthread_mutex_trylock(&mainLoopLock); + return pthread_mutex_trylock(&mainLoopLock); } #define RAD2DEG (180.0 / M_PI) #define ACC_SCALE (256 / 9.80665) #define GYRO_SCALE (16.4) void sendMotorUpdate() { - udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); + udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); } void updateState(const fdm_packet* pkt) { - static double last_timestamp = 0; // in seconds - static uint64_t last_realtime = 0; // in uS - static struct timespec last_ts; // last packet + static double last_timestamp = 0; // in seconds + static uint64_t last_realtime = 0; // in uS + static struct timespec last_ts; // last packet - struct timespec now_ts; - clock_gettime(CLOCK_MONOTONIC, &now_ts); + struct timespec now_ts; + clock_gettime(CLOCK_MONOTONIC, &now_ts); - const uint64_t realtime_now = micros64_real(); - if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout - last_timestamp = pkt->timestamp; - last_realtime = realtime_now; - sendMotorUpdate(); - return; - } + const uint64_t realtime_now = micros64_real(); + if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout + last_timestamp = pkt->timestamp; + last_realtime = realtime_now; + sendMotorUpdate(); + return; + } - const double deltaSim = pkt->timestamp - last_timestamp; // in seconds - if(deltaSim < 0) { // don't use old packet - return; - } + const double deltaSim = pkt->timestamp - last_timestamp; // in seconds + if(deltaSim < 0) { // don't use old packet + return; + } - int16_t x,y,z; - x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE; - y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE; - z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE; - fakeAccSet(fakeAccDev, x, y, z); + int16_t x,y,z; + x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE; + y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE; + z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE; + fakeAccSet(fakeAccDev, x, y, z); // printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]); - x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG; - y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG; - z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG; - fakeGyroSet(fakeGyroDev, x, y, z); + x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG; + y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG; + z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG; + fakeGyroSet(fakeGyroDev, x, y, z); // printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]); #if defined(SKIP_IMU_CALC) #if defined(SET_IMU_FROM_EULER) - // set from Euler - double qw = pkt->imu_orientation_quat[0]; - double qx = pkt->imu_orientation_quat[1]; - double qy = pkt->imu_orientation_quat[2]; - double qz = pkt->imu_orientation_quat[3]; - double ysqr = qy * qy; - double xf, yf, zf; + // set from Euler + double qw = pkt->imu_orientation_quat[0]; + double qx = pkt->imu_orientation_quat[1]; + double qy = pkt->imu_orientation_quat[2]; + double qz = pkt->imu_orientation_quat[3]; + double ysqr = qy * qy; + double xf, yf, zf; - // roll (x-axis rotation) - double t0 = +2.0 * (qw * qx + qy * qz); - double t1 = +1.0 - 2.0 * (qx * qx + ysqr); - xf = atan2(t0, t1) * RAD2DEG; + // roll (x-axis rotation) + double t0 = +2.0 * (qw * qx + qy * qz); + double t1 = +1.0 - 2.0 * (qx * qx + ysqr); + xf = atan2(t0, t1) * RAD2DEG; - // pitch (y-axis rotation) - double t2 = +2.0 * (qw * qy - qz * qx); - t2 = t2 > 1.0 ? 1.0 : t2; - t2 = t2 < -1.0 ? -1.0 : t2; - yf = asin(t2) * RAD2DEG; // from wiki + // pitch (y-axis rotation) + double t2 = +2.0 * (qw * qy - qz * qx); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + yf = asin(t2) * RAD2DEG; // from wiki - // yaw (z-axis rotation) - double t3 = +2.0 * (qw * qz + qx * qy); - double t4 = +1.0 - 2.0 * (ysqr + qz * qz); - zf = atan2(t3, t4) * RAD2DEG; - imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!! + // yaw (z-axis rotation) + double t3 = +2.0 * (qw * qz + qx * qy); + double t4 = +1.0 - 2.0 * (ysqr + qz * qz); + zf = atan2(t3, t4) * RAD2DEG; + imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!! #else - imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]); + imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]); #endif #endif #if defined(SIMULATOR_IMU_SYNC) - imuSetHasNewData(deltaSim*1e6); - imuUpdateAttitude(micros()); + imuSetHasNewData(deltaSim*1e6); + imuUpdateAttitude(micros()); #endif - if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz + if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz // simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5; - struct timespec out_ts; - timeval_sub(&out_ts, &now_ts, &last_ts); - simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec); - } + struct timespec out_ts; + timeval_sub(&out_ts, &now_ts, &last_ts); + simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec); + } // printf("simRate = %lf, millis64 = %lu, millis64_real = %lu, deltaSim = %lf\n", simRate, millis64(), millis64_real(), deltaSim*1e6); - last_timestamp = pkt->timestamp; - last_realtime = micros64_real(); + last_timestamp = pkt->timestamp; + last_realtime = micros64_real(); - last_ts.tv_sec = now_ts.tv_sec; - last_ts.tv_nsec = now_ts.tv_nsec; + last_ts.tv_sec = now_ts.tv_sec; + last_ts.tv_nsec = now_ts.tv_nsec; - pthread_mutex_unlock(&updateLock); // can send PWM output now + pthread_mutex_unlock(&updateLock); // can send PWM output now #if defined(SIMULATOR_GYROPID_SYNC) - pthread_mutex_unlock(&mainLoopLock); // can run main loop + pthread_mutex_unlock(&mainLoopLock); // can run main loop #endif } static void* udpThread(void* data) { - UNUSED(data); - int n = 0; + UNUSED(data); + int n = 0; - while (workerRunning) { - n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100); - if(n == sizeof(fdm_packet)) { + while (workerRunning) { + n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100); + if(n == sizeof(fdm_packet)) { // printf("[data]new fdm %d\n", n); - updateState(&fdmPkt); - } - } + updateState(&fdmPkt); + } + } - printf("udpThread end!!\n"); - return NULL; + printf("udpThread end!!\n"); + return NULL; } static void* tcpThread(void* data) { - UNUSED(data); + UNUSED(data); - dyad_init(); - dyad_setTickInterval(0.2f); - dyad_setUpdateTimeout(0.5f); + dyad_init(); + dyad_setTickInterval(0.2f); + dyad_setUpdateTimeout(0.5f); - while (workerRunning) { - dyad_update(); - } + while (workerRunning) { + dyad_update(); + } - dyad_shutdown(); - printf("tcpThread end!!\n"); - return NULL; + dyad_shutdown(); + printf("tcpThread end!!\n"); + return NULL; } // system void systemInit(void) { - int ret; + int ret; - clock_gettime(CLOCK_MONOTONIC, &start_time); - printf("[system]Init...\n"); + clock_gettime(CLOCK_MONOTONIC, &start_time); + printf("[system]Init...\n"); - SystemCoreClock = 500 * 1e6; // fake 500MHz - FLASH_Unlock(); + SystemCoreClock = 500 * 1e6; // fake 500MHz + FLASH_Unlock(); - if (pthread_mutex_init(&updateLock, NULL) != 0) { - printf("Create updateLock error!\n"); - exit(1); - } + if (pthread_mutex_init(&updateLock, NULL) != 0) { + printf("Create updateLock error!\n"); + exit(1); + } - if (pthread_mutex_init(&mainLoopLock, NULL) != 0) { - printf("Create mainLoopLock error!\n"); - exit(1); - } + if (pthread_mutex_init(&mainLoopLock, NULL) != 0) { + printf("Create mainLoopLock error!\n"); + exit(1); + } - ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL); - if(ret != 0) { - printf("Create tcpWorker error!\n"); - exit(1); - } + ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL); + if(ret != 0) { + printf("Create tcpWorker error!\n"); + exit(1); + } - ret = udpInit(&pwmLink, "127.0.0.1", 9002, false); - printf("init PwnOut UDP link...%d\n", ret); + ret = udpInit(&pwmLink, "127.0.0.1", 9002, false); + printf("init PwnOut UDP link...%d\n", ret); - ret = udpInit(&stateLink, NULL, 9003, true); - printf("start UDP server...%d\n", ret); + ret = udpInit(&stateLink, NULL, 9003, true); + printf("start UDP server...%d\n", ret); - ret = pthread_create(&udpWorker, NULL, udpThread, NULL); - if(ret != 0) { - printf("Create udpWorker error!\n"); - exit(1); - } + ret = pthread_create(&udpWorker, NULL, udpThread, NULL); + if(ret != 0) { + printf("Create udpWorker error!\n"); + exit(1); + } - // serial can't been slow down - rescheduleTask(TASK_SERIAL, 1); + // serial can't been slow down + rescheduleTask(TASK_SERIAL, 1); } void systemReset(void){ - printf("[system]Reset!\n"); - workerRunning = false; - pthread_join(tcpWorker, NULL); - pthread_join(udpWorker, NULL); - exit(0); + printf("[system]Reset!\n"); + workerRunning = false; + pthread_join(tcpWorker, NULL); + pthread_join(udpWorker, NULL); + exit(0); } void systemResetToBootloader(void) { - printf("[system]ResetToBootloader!\n"); - workerRunning = false; - pthread_join(tcpWorker, NULL); - pthread_join(udpWorker, NULL); - exit(0); + printf("[system]ResetToBootloader!\n"); + workerRunning = false; + pthread_join(tcpWorker, NULL); + pthread_join(udpWorker, NULL); + exit(0); } // drivers/light_led.c void ledInit(const statusLedConfig_t *statusLedConfig) { - UNUSED(statusLedConfig); - printf("[led]Init...\n"); -} -void timerInit(void) { - printf("[timer]Init...\n"); -} -void timerStart(void) { -} -void failureMode(failureMode_e mode) { - printf("[failureMode]!!! %d\n", mode); - while(1); + UNUSED(statusLedConfig); + printf("[led]Init...\n"); } +void timerInit(void) { + printf("[timer]Init...\n"); +} + +void timerStart(void) { +} + +void failureMode(failureMode_e mode) { + printf("[failureMode]!!! %d\n", mode); + while(1); +} + +void failureLedCode(failureMode_e mode, int repeatCount) +{ + UNUSED(repeatCount); + printf("Failure LED flash for: [failureMode]!!! %d\n", mode); +} // Time part // Thanks ArduPilot uint64_t nanos64_real() { - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec); + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec); } + uint64_t micros64_real() { - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9))); + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9))); } + uint64_t millis64_real() { - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9))); + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9))); } uint64_t micros64() { - static uint64_t last = 0; - static uint64_t out = 0; - uint64_t now = nanos64_real(); + static uint64_t last = 0; + static uint64_t out = 0; + uint64_t now = nanos64_real(); - out += (now - last) * simRate; - last = now; + out += (now - last) * simRate; + last = now; - return out*1e-3; + return out*1e-3; // return micros64_real(); } + uint64_t millis64() { - static uint64_t last = 0; - static uint64_t out = 0; - uint64_t now = nanos64_real(); + static uint64_t last = 0; + static uint64_t out = 0; + uint64_t now = nanos64_real(); - out += (now - last) * simRate; - last = now; + out += (now - last) * simRate; + last = now; - return out*1e-6; + return out*1e-6; // return millis64_real(); } uint32_t micros(void) { - return micros64() & 0xFFFFFFFF; + return micros64() & 0xFFFFFFFF; } uint32_t millis(void) { - return millis64() & 0xFFFFFFFF; + return millis64() & 0xFFFFFFFF; } void microsleep(uint32_t usec) { @@ -321,18 +334,21 @@ void microsleep(uint32_t usec) { ts.tv_nsec = usec*1000UL; while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ; } -void delayMicroseconds(uint32_t us) { - microsleep(us / simRate); -} -void delayMicroseconds_real(uint32_t us) { - microsleep(us); -} -void delay(uint32_t ms) { - uint64_t start = millis64(); - while ((millis64() - start) < ms) { - microsleep(1000); - } +void delayMicroseconds(uint32_t us) { + microsleep(us / simRate); +} + +void delayMicroseconds_real(uint32_t us) { + microsleep(us); +} + +void delay(uint32_t ms) { + uint64_t start = millis64(); + + while ((millis64() - start) < ms) { + microsleep(1000); + } } // Subtract the ‘struct timespec’ values X and Y, storing the result in RESULT. @@ -340,21 +356,21 @@ void delay(uint32_t ms) { // result = x - y // from: http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y) { - unsigned int s_carry = 0; - unsigned int ns_carry = 0; - // Perform the carry for the later subtraction by updating y. - if (x->tv_nsec < y->tv_nsec) { - int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1; - ns_carry += 1000000000 * nsec; - s_carry += nsec; - } + unsigned int s_carry = 0; + unsigned int ns_carry = 0; + // Perform the carry for the later subtraction by updating y. + if (x->tv_nsec < y->tv_nsec) { + int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1; + ns_carry += 1000000000 * nsec; + s_carry += nsec; + } - // Compute the time remaining to wait. tv_usec is certainly positive. - result->tv_sec = x->tv_sec - y->tv_sec - s_carry; - result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry; + // Compute the time remaining to wait. tv_usec is certainly positive. + result->tv_sec = x->tv_sec - y->tv_sec - s_carry; + result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry; - // Return 1 if result is negative. - return x->tv_sec < y->tv_sec; + // Return 1 if result is negative. + return x->tv_sec < y->tv_sec; } @@ -369,68 +385,75 @@ static int16_t servosPwm[MAX_SUPPORTED_SERVOS]; static int16_t idlePulse; void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) { - UNUSED(motorConfig); - UNUSED(motorCount); + UNUSED(motorConfig); + UNUSED(motorCount); - idlePulse = _idlePulse; + idlePulse = _idlePulse; - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { - motors[motorIndex].enabled = true; - } - pwmMotorsEnabled = true; + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + motors[motorIndex].enabled = true; + } + pwmMotorsEnabled = true; } + void servoDevInit(const servoDevConfig_t *servoConfig) { - UNUSED(servoConfig); - for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { - servos[servoIndex].enabled = true; - } + UNUSED(servoConfig); + for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { + servos[servoIndex].enabled = true; + } } pwmOutputPort_t *pwmGetMotors(void) { - return motors; + return motors; } + bool pwmAreMotorsEnabled(void) { - return pwmMotorsEnabled; + return pwmMotorsEnabled; } + bool isMotorProtocolDshot(void) { return false; } + void pwmWriteMotor(uint8_t index, float value) { - motorsPwm[index] = value - idlePulse; + motorsPwm[index] = value - idlePulse; } + void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { - UNUSED(motorCount); - pwmMotorsEnabled = false; + UNUSED(motorCount); + pwmMotorsEnabled = false; } + void pwmCompleteMotorUpdate(uint8_t motorCount) { - UNUSED(motorCount); - // send to simulator - // for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0] + UNUSED(motorCount); + // send to simulator + // for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0] - double outScale = 1000.0; - if (feature(FEATURE_3D)) { - outScale = 500.0; - } + double outScale = 1000.0; + if (feature(FEATURE_3D)) { + outScale = 500.0; + } - pwmPkt.motor_speed[3] = motorsPwm[0] / outScale; - pwmPkt.motor_speed[0] = motorsPwm[1] / outScale; - pwmPkt.motor_speed[1] = motorsPwm[2] / outScale; - pwmPkt.motor_speed[2] = motorsPwm[3] / outScale; + pwmPkt.motor_speed[3] = motorsPwm[0] / outScale; + pwmPkt.motor_speed[0] = motorsPwm[1] / outScale; + pwmPkt.motor_speed[1] = motorsPwm[2] / outScale; + pwmPkt.motor_speed[2] = motorsPwm[3] / outScale; - // get one "fdm_packet" can only send one "servo_packet"!! - if(pthread_mutex_trylock(&updateLock) != 0) return; - udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); + // get one "fdm_packet" can only send one "servo_packet"!! + if(pthread_mutex_trylock(&updateLock) != 0) return; + udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); // printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]); } + void pwmWriteServo(uint8_t index, float value) { - servosPwm[index] = value; + servosPwm[index] = value; } // ADC part uint16_t adcGetChannel(uint8_t channel) { - UNUSED(channel); - return 0; + UNUSED(channel); + return 0; } // stack part @@ -438,62 +461,73 @@ char _estack; char _Min_Stack_Size; // fake EEPROM -extern uint8_t __config_start; -extern uint8_t __config_end; +uint8_t __config_start; +uint8_t __config_end; static FILE *eepromFd = NULL; void FLASH_Unlock(void) { - uint8_t * const eeprom = &__config_start; + uint8_t * const eeprom = &__config_start; - if (eepromFd != NULL) { - printf("[FLASH_Unlock] eepromFd != NULL\n"); - return; - } + if (eepromFd != NULL) { + printf("[FLASH_Unlock] eepromFd != NULL\n"); + return; + } - // open or create - eepromFd = fopen(EEPROM_FILENAME,"r+"); - if (eepromFd != NULL) { - // obtain file size: - fseek(eepromFd , 0 , SEEK_END); - long lSize = ftell(eepromFd); - rewind(eepromFd); + // open or create + eepromFd = fopen(EEPROM_FILENAME,"r+"); + if (eepromFd != NULL) { + // obtain file size: + fseek(eepromFd , 0 , SEEK_END); + long lSize = ftell(eepromFd); + rewind(eepromFd); - printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start)); - for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) { - int c = fgetc(eepromFd); - if(c == EOF) break; - eeprom[i] = (uint8_t)c; - } - } else { - eepromFd = fopen(EEPROM_FILENAME, "w+"); - fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd); - } + printf("[FLASH_Unlock]size = %ld, %d\n", lSize, (uintptr_t)(&__config_end - &__config_start)); + for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) { + int c = fgetc(eepromFd); + if(c == EOF) break; + eeprom[i] = (uint8_t)c; + } + } else { + eepromFd = fopen(EEPROM_FILENAME, "w+"); + fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd); + } } void FLASH_Lock(void) { - // flush & close - if (eepromFd != NULL) { - const uint8_t *eeprom = &__config_start; - fseek(eepromFd, 0, SEEK_SET); - fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd); - fclose(eepromFd); - eepromFd = NULL; - } + // flush & close + if (eepromFd != NULL) { + const uint8_t *eeprom = &__config_start; + fseek(eepromFd, 0, SEEK_SET); + fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd); + fclose(eepromFd); + eepromFd = NULL; + } } FLASH_Status FLASH_ErasePage(uintptr_t Page_Address) { - UNUSED(Page_Address); + UNUSED(Page_Address); // printf("[FLASH_ErasePage]%x\n", Page_Address); - return FLASH_COMPLETE; + return FLASH_COMPLETE; } + FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data) { - if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) { - *((uint32_t*)addr) = Data; - printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr)); - } else { - printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr); - } - return FLASH_COMPLETE; + if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) { + *((uint32_t*)addr) = Data; + printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr)); + } else { + printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr); + } + return FLASH_COMPLETE; } +void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig) +{ + UNUSED(pSerialPinConfig); + printf("uartPinConfigure"); +} +void spektrumBind(rxConfig_t *rxConfig) +{ + UNUSED(rxConfig); + printf("spektrumBind"); +} From b525d5d7b854e70f412022700914eb47eb6bb661 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 2 Jul 2017 05:05:26 +1000 Subject: [PATCH 35/40] Remove warnings if built under 32bit windows (cygwin) --- src/main/target/SITL/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 804ebe7b3d..b94d75c391 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -481,7 +481,7 @@ void FLASH_Unlock(void) { long lSize = ftell(eepromFd); rewind(eepromFd); - printf("[FLASH_Unlock]size = %ld, %d\n", lSize, (uintptr_t)(&__config_end - &__config_start)); + printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (long)(&__config_end - &__config_start)); for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) { int c = fgetc(eepromFd); if(c == EOF) break; From 6edb4b9c19273262229d4bb68d93da9db6b819f7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 1 Jul 2017 09:18:43 +0100 Subject: [PATCH 36/40] Cache SPI handle to improve performance --- src/main/drivers/accgyro/accgyro_mpu.c | 10 ++--- src/main/drivers/bus.h | 3 ++ src/main/drivers/bus_spi.c | 5 +++ src/main/drivers/bus_spi.h | 1 + src/main/drivers/bus_spi_hal.c | 54 ++++++++++++++++------- src/main/drivers/compass/compass_ak8963.c | 13 +++--- 6 files changed, 59 insertions(+), 27 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index bab5813918..971deffc05 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -234,7 +234,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) { UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI #ifdef USE_GYRO_SPI_MPU6000 - gyro->bus.spi.instance = MPU6000_SPI_INSTANCE; + spiBusSetInstance(&gyro->bus, MPU6000_SPI_INSTANCE); #ifdef MPU6000_CS_PIN gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin; #endif @@ -248,7 +248,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_GYRO_SPI_MPU6500 - gyro->bus.spi.instance = MPU6500_SPI_INSTANCE; + spiBusSetInstance(&gyro->bus, MPU6500_SPI_INSTANCE); gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin; const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus); // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI @@ -262,7 +262,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_GYRO_SPI_MPU9250 - gyro->bus.spi.instance = MPU9250_SPI_INSTANCE; + spiBusSetInstance(&gyro->bus, MPU9250_SPI_INSTANCE); gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin; if (mpu9250SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = MPU_9250_SPI; @@ -275,7 +275,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_GYRO_SPI_ICM20689 - gyro->bus.spi.instance = ICM20689_SPI_INSTANCE; + spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE); gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin; if (icm20689SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = ICM_20689_SPI; @@ -287,7 +287,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #endif #ifdef USE_ACCGYRO_BMI160 - gyro->bus.spi.instance = BMI160_SPI_INSTANCE; + spiBusSetInstance(&gyro->bus, BMI160_SPI_INSTANCE); gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin; if (bmi160Detect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = BMI_160_SPI; diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index e8c1436018..a1367d598b 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -25,6 +25,9 @@ typedef union busDevice_u { struct deviceSpi_s { SPI_TypeDef *instance; +#if defined(USE_HAL_DRIVER) + SPI_HandleTypeDef* handle; // cached here for efficiency +#endif IO_t csnPin; } spi; struct deviceI2C_s { diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 0eb3815096..06ab586f0a 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -383,3 +383,8 @@ uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg) return data; } + +void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) +{ + bus->spi.instance = instance; +} diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 8c6ad6f40e..e9ee1eb0b6 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -106,3 +106,4 @@ DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channe bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg); +void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 1d0011e809..9dfdbe5210 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -68,6 +68,8 @@ #define SPI4_NSS_PIN NONE #endif +#define SPI_DEFAULT_TIMEOUT 10 + static spiDevice_t spiHardwareMap[] = { { .dev = SPI1, .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER }, @@ -239,13 +241,6 @@ uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance) return spiHardwareMap[device].errorCount; } -// return uint8_t value or -1 when failure -uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in) -{ - spiTransfer(instance, &in, &in, 1); - return in; -} - /** * Return true if the bus is currently in the middle of a transmission. */ @@ -263,8 +258,6 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len SPIDevice device = spiDeviceByInstance(instance); HAL_StatusTypeDef status; -#define SPI_DEFAULT_TIMEOUT 10 - if(!out) // Tx only { status = HAL_SPI_Transmit(&spiHardwareMap[device].hspi, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT); @@ -284,6 +277,31 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len return true; } +static bool spiBusReadBuffer(const busDevice_t *bus, uint8_t *out, int len) +{ + const HAL_StatusTypeDef status = HAL_SPI_Receive(bus->spi.handle, out, len, SPI_DEFAULT_TIMEOUT); + if (status != HAL_OK) { + spiTimeoutUserCallback(bus->spi.instance); + } + return true; +} + +// return uint8_t value or -1 when failure +uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in) +{ + spiTransfer(instance, &in, &in, 1); + return in; +} + +// return uint8_t value or -1 when failure +static uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t in) +{ + const HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(bus->spi.handle, &in, &in, 1, SPI_DEFAULT_TIMEOUT); + if (status != HAL_OK) { + spiTimeoutUserCallback(bus->spi.instance); + } + return in; +} void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) { @@ -349,8 +367,8 @@ void spiResetErrorCounter(SPI_TypeDef *instance) bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { IOLo(bus->spi.csnPin); - spiTransferByte(bus->spi.instance, reg); - spiTransferByte(bus->spi.instance, data); + spiBusTransferByte(bus, reg); + spiBusTransferByte(bus, data); IOHi(bus->spi.csnPin); return true; @@ -359,8 +377,8 @@ bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { IOLo(bus->spi.csnPin); - spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction - spiTransfer(bus->spi.instance, data, NULL, length); + spiBusTransferByte(bus, reg | 0x80); // read transaction + spiBusReadBuffer(bus, data, length); IOHi(bus->spi.csnPin); return true; @@ -370,13 +388,19 @@ uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg) { uint8_t data; IOLo(bus->spi.csnPin); - spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction - spiTransfer(bus->spi.instance, &data, NULL, 1); + spiBusTransferByte(bus, reg | 0x80); // read transaction + spiBusReadBuffer(bus, &data, 1); IOHi(bus->spi.csnPin); return data; } +void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) +{ + bus->spi.instance = instance; + bus->spi.handle = spiHandleByInstance(instance); +} + void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor) { SPIDevice device = descriptor->userParam; diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 8523dfb1c0..981d8e3923 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -84,7 +84,9 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f }; +#if defined(USE_SPI) static busDevice_t *bus = NULL; // HACK +#endif // FIXME pretend we have real MPU9250 support // Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same. @@ -325,21 +327,18 @@ bool ak8963Detect(magDev_t *mag) { uint8_t sig = 0; - bus = &mag->bus; - #if defined(USE_SPI) + bus = &mag->bus; #if defined(MPU6500_SPI_INSTANCE) - bus->spi.instance = MPU6500_SPI_INSTANCE; + spiBusSetInstance(&mag->bus, MPU6500_SPI_INSTANCE); #elif defined(MPU9250_SPI_INSTANCE) - bus->spi.instance = MPU9250_SPI_INSTANCE; - // initialze I2C master via SPI bus (MPU9250) + spiBusSetInstance(&mag->bus, MPU9250_SPI_INSTANCE); + // initialze I2C master via SPI bus (MPU9250) mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); delay(10); - mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz delay(10); - mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only delay(10); #endif From 67d2c3ef256914dcce239eba3d613397b966cae1 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 2 Jul 2017 06:55:14 +1000 Subject: [PATCH 37/40] Minor makefile change to support windows better Removed uninitialised warnings --- Makefile | 8 +++----- src/main/cms/cms.c | 2 +- src/main/fc/cli.c | 2 +- src/main/io/asyncfatfs/asyncfatfs.c | 4 ++-- src/main/rx/spektrum.c | 2 +- src/main/sensors/acceleration.c | 2 +- 6 files changed, 9 insertions(+), 11 deletions(-) diff --git a/Makefile b/Makefile index ebf1d21898..64c788fba8 100644 --- a/Makefile +++ b/Makefile @@ -1345,22 +1345,20 @@ targets-group-rest: $(GROUP_OTHER_TARGETS) $(VALID_TARGETS): - $(V0) echo "" && \ + $(V0) @echo "" && \ echo "Building $@" && \ time $(MAKE) binary hex TARGET=$@ && \ echo "Building $@ succeeded." - - CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) ) TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) ) ## clean : clean up temporary / machine-generated files clean: - $(V0) echo "Cleaning $(TARGET)" + $(V0) @echo "Cleaning $(TARGET)" $(V0) rm -f $(CLEAN_ARTIFACTS) $(V0) rm -rf $(OBJECT_DIR)/$(TARGET) - $(V0) echo "Cleaning $(TARGET) succeeded." + $(V0) @echo "Cleaning $(TARGET) succeeded." ## clean_test : clean up temporary / machine-generated files (tests) clean_test: diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index fc8eb1249e..6df56ffefa 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -872,7 +872,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, uint8_t key, int repeatCount) { - uint16_t ret; + uint16_t ret = 0; for (int i = 0 ; i < repeatCount ; i++) { ret = cmsHandleKey(pDisplay, key); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index cd055811b2..d5d0db1b0f 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1164,7 +1164,7 @@ static void cliRxRange(char *cmdline) ptr = cmdline; i = atoi(ptr); if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) { - int rangeMin, rangeMax; + int rangeMin = 0, rangeMax = 0; ptr = nextArg(ptr); if (ptr) { diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 040a2e5848..d3c926929c 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -1639,7 +1639,7 @@ static afatfsOperationStatus_e afatfs_appendSuperclusterContinue(afatfsFile_t *f { afatfsAppendSupercluster_t *opState = &file->operation.state.appendSupercluster; - afatfsOperationStatus_e status; + afatfsOperationStatus_e status = AFATFS_OPERATION_FAILURE; doMore: switch (opState->phase) { @@ -2387,7 +2387,7 @@ static afatfsFilePtr_t afatfs_allocateFileHandle() static afatfsOperationStatus_e afatfs_ftruncateContinue(afatfsFilePtr_t file, bool markDeleted) { afatfsTruncateFile_t *opState = &file->operation.state.truncateFile; - afatfsOperationStatus_e status; + afatfsOperationStatus_e status = AFATFS_OPERATION_FAILURE; #ifdef AFATFS_USE_FREEFILE uint32_t oldFreeFileStart, freeFileGrow; diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index f73e2f2a84..a3a176373d 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -212,7 +212,7 @@ void spektrumBind(rxConfig_t *rxConfig) } // Determine a pin to use - ioTag_t bindPin; + ioTag_t bindPin = IO_TAG_NONE; if (rxConfig->spektrum_bind_pin_override_ioTag) { bindPin = rxConfig->spektrum_bind_pin_override_ioTag; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 5f173eaf81..10c164d154 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -120,7 +120,7 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) { - accelerationSensor_e accHardware; + accelerationSensor_e accHardware = ACC_NONE; #ifdef USE_ACC_ADXL345 drv_adxl345_config_t acc_params; From b512f006ca5bf674aea26ce0aae27ae9f9cc35e9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 2 Jul 2017 07:52:24 +1000 Subject: [PATCH 38/40] Fix for missing F7 push pull --- src/main/drivers/serial_uart_stm32f7xx.c | 9 +++++++-- src/main/target/SITL/target.c | 1 - 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c index 7bde2a94cd..aeb6906a14 100644 --- a/src/main/drivers/serial_uart_stm32f7xx.c +++ b/src/main/drivers/serial_uart_stm32f7xx.c @@ -341,9 +341,14 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po IO_t rxIO = IOGetByTag(uartdev->rx); if ((options & SERIAL_BIDIR) && txIO) { - // XXX BIDIR_PP handling is missing + ioConfig_t ioCfg = IO_CONFIG( + ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_MODE_AF_PP : GPIO_MODE_AF_OD, + GPIO_SPEED_FREQ_HIGH, + ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_PULLDOWN : GPIO_PULLUP + ); + IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(txIO, IOCFG_AF_PP, hardware->af); + IOConfigGPIOAF(txIO, ioCfg, hardware->af); } else { if ((mode & MODE_TX) && txIO) { diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index b94d75c391..1e573f77fa 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -22,7 +22,6 @@ #include #include -#include #include "drivers/io.h" #include "drivers/dma.h" From 82b73ce00e035585e65fbc1423768a4c6e1a81e6 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 2 Jul 2017 10:06:38 +1000 Subject: [PATCH 39/40] Moved disabled flags text to CLI where other flag text is located --- src/main/fc/cli.c | 8 +++++++- src/main/fc/runtime_config.c | 2 -- src/main/fc/runtime_config.h | 15 --------------- 3 files changed, 7 insertions(+), 18 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d5d0db1b0f..66033bd951 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -190,10 +190,16 @@ static const char * const gyroNames[] = { static const char * const *sensorHardwareNames[] = { gyroNames, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware - }; #endif // USE_SENSOR_NAMES +#ifndef MINIMAL_CLI +static const char *armingDisableFlagNames[] = { + "NOGYRO", "FAILSAFE", "BOXFAILSAFE", "THROTTLE", + "ANGLE", "LOAD", "CALIB", "CLI", "CMS", "OSD", "BST" +}; +#endif + static void cliPrint(const char *str) { while (*str) { diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 3d49ed4884..17bd29105b 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -23,8 +23,6 @@ #include "fc/runtime_config.h" #include "io/beeper.h" -const char *armingDisableFlagNames[] = { ARMING_DISBALED_FLAG_NAMES }; - uint8_t armingFlags = 0; uint8_t stateFlags = 0; uint16_t flightModeFlags = 0; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 59470af1a5..a54f287bd3 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -47,21 +47,6 @@ typedef enum { ARMING_DISABLED_BST = (1 << 10), } armingDisableFlags_e; -extern const char *armingDisableFlagNames[]; - -#define ARMING_DISBALED_FLAG_NAMES \ - "NOGYRO", \ - "FAILSAFE", \ - "BOXFAILSAFE", \ - "THROTTLE", \ - "ANGLE", \ - "LOAD", \ - "CALIB", \ - "CLI", \ - "CMS", \ - "OSD", \ - "BST", - void setArmingDisabled(armingDisableFlags_e flag); void unsetArmingDisabled(armingDisableFlags_e flag); bool isArmingDisabled(void); From ba298eebb5ae7692772a5be7b2e303e2b0a73742 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 2 Jul 2017 10:49:45 +1000 Subject: [PATCH 40/40] Moving gyro to rest of hardware in settings.c --- src/main/fc/cli.c | 8 +------- src/main/fc/settings.c | 9 ++++++++- src/main/fc/settings.h | 3 +++ 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 66033bd951..ebdadfda3f 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -182,14 +182,8 @@ static const char * const sensorTypeNames[] = { #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) -// sync with gyroSensor_e -static const char * const gyroNames[] = { - "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE" -}; - static const char * const *sensorHardwareNames[] = { - gyroNames, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware + lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware }; #endif // USE_SENSOR_NAMES diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index e29d9ff5e0..8f3a028ec5 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -74,13 +74,19 @@ #include "telemetry/frsky.h" #include "telemetry/telemetry.h" - // Sensor names (used in lookup tables for *_hardware settings and in status command output) // sync with accelerationSensor_e const char * const lookupTableAccHardware[] = { "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20689", "BMI160", "FAKE" }; + +// sync with gyroSensor_e +const char * const lookupTableGyroHardware[] = { + "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", + "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE" +}; + #if defined(USE_SENSOR_NAMES) || defined(BARO) // sync with baroSensor_e const char * const lookupTableBaroHardware[] = { @@ -272,6 +278,7 @@ const lookupTableEntry_t lookupTables[] = { { lookupTableRxSpi, sizeof(lookupTableRxSpi) / sizeof(char *) }, #endif { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }, + { lookupTableGyroHardware, sizeof(lookupTableGyroHardware) / sizeof(char *) }, { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) }, #ifdef BARO { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 52ddb0a3f5..a9f05e5803 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -41,6 +41,7 @@ typedef enum { TABLE_RX_SPI, #endif TABLE_GYRO_LPF, + TABLE_GYRO_HARDWARE, TABLE_ACC_HARDWARE, #ifdef BARO TABLE_BARO_HARDWARE, @@ -130,6 +131,8 @@ extern const uint16_t valueTableEntryCount; extern const clivalue_t valueTable[]; //extern const uint8_t lookupTablesEntryCount; +extern const char * const lookupTableGyroHardware[]; + extern const char * const lookupTableAccHardware[]; //extern const uint8_t lookupTableAccHardwareEntryCount;