From 7654f9f30cba9e8eb5b7b22c5b4900c5893d00b7 Mon Sep 17 00:00:00 2001 From: kikoqiu Date: Fri, 18 Aug 2023 12:53:23 +0800 Subject: [PATCH 1/4] fix at32f435 uart7 rx not working as it has 2 serial AFs(USART1_RX / UART7_RX). --- src/driver/serial.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/driver/serial.c b/src/driver/serial.c index 081265841..c78ddeba7 100644 --- a/src/driver/serial.c +++ b/src/driver/serial.c @@ -15,10 +15,10 @@ bool serial_is_soft(serial_ports_t port) { return true; } -static const gpio_af_t *serial_hard_find_af(gpio_pins_t pin) { +static const gpio_af_t *serial_hard_find_af(gpio_pins_t pin, serial_ports_t port) { for (uint32_t i = 0; i < GPIO_AF_MAX; i++) { const gpio_af_t *func = &gpio_pin_afs[i]; - if (func->pin == pin && RESOURCE_TAG_TYPE(func->tag) == RESOURCE_SERIAL) { + if (func->pin == pin && RESOURCE_TAG_TYPE(func->tag) == RESOURCE_SERIAL && SERIAL_TAG_PORT(func->tag) == port) { return func; } } @@ -43,7 +43,7 @@ static bool serial_hard_pin_init(serial_port_t *serial, serial_port_config_t con gpio_init.pull = GPIO_UP_PULL; } - const gpio_af_t *tx_af = serial_hard_find_af(dev->tx); + const gpio_af_t *tx_af = serial_hard_find_af(dev->tx, port); if (tx_af == NULL) { return false; } @@ -57,8 +57,8 @@ static bool serial_hard_pin_init(serial_port_t *serial, serial_port_config_t con gpio_init.output = GPIO_PUSHPULL; gpio_init.pull = GPIO_NO_PULL; - const gpio_af_t *rx_af = serial_hard_find_af(dev->rx); - const gpio_af_t *tx_af = serial_hard_find_af(dev->tx); + const gpio_af_t *rx_af = serial_hard_find_af(dev->rx, port); + const gpio_af_t *tx_af = serial_hard_find_af(dev->tx, port); if (tx_af == NULL || rx_af == NULL) { return false; } From 4ec8380f960ac1d5092d531d68ef72e3c8227e03 Mon Sep 17 00:00:00 2001 From: kikoqiu Date: Sat, 19 Aug 2023 22:10:56 +0800 Subject: [PATCH 2/4] implement MSP_ANALOG to enable DJI goggles osd voltage display. --- src/io/msp.c | 14 ++++++++++++++ src/io/msp.h | 1 + 2 files changed, 15 insertions(+) diff --git a/src/io/msp.c b/src/io/msp.c index db8ae1590..3031863c6 100644 --- a/src/io/msp.c +++ b/src/io/msp.c @@ -116,6 +116,20 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd, msp_send_reply(msp, magic, cmd, data, 12); break; } + case MSP_ANALOG: { + const int16_t current = (int16_t)constrain(state.ibat / 10, -320, 320); + const uint16_t rssi = (uint16_t)constrain(state.rx_rssi * 1023 / 100, 0, 1023); + const uint16_t vbat = (uint8_t)constrain(state.vbat_filtered / 0.01, 0, 255); + uint8_t data[9] = { + (uint8_t)constrain(state.vbat_filtered / 0.1, 0, 255), // battery voltage + 0x0, 0x0, // battery drawn in mAh + (rssi >> 8) & 0xFF, rssi & 0xFF, // rssi + (current >> 8) & 0xFF, current & 0xFF, // current in 0.01 A steps, range is -320A to 320A + (vbat >> 8) & 0xFF, vbat & 0xFF, // battery voltage + }; + msp_send_reply(msp, magic, cmd, data, 9); + break; + } case MSP_BATTERY_STATE: { const uint16_t current = state.ibat / 1000; uint8_t data[9] = { diff --git a/src/io/msp.h b/src/io/msp.h index 6d6fd8010..e71fc4728 100644 --- a/src/io/msp.h +++ b/src/io/msp.h @@ -23,6 +23,7 @@ #define MSP_EEPROM_WRITE 250 // in message no param #define MSP_REBOOT 68 // in message reboot settings +#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX #define MSP_BATTERY_STATE 130 // out message Connected/Disconnected, Voltage, Current Used #define MSP_UID 160 // out message Unique device ID From a2596165407f33efbcc38b9943be45041f299184 Mon Sep 17 00:00:00 2001 From: kikoqiu Date: Sat, 19 Aug 2023 22:26:29 +0800 Subject: [PATCH 3/4] enhance sixaxis_wait_for_still to avoid false positive signals of still. remove no-effect code for limit in sixaxis_gyro_cal. pause gyro bias calibration when move is detected. --- src/flight/sixaxis.c | 61 +++++++++++++++++++++++++++----------------- 1 file changed, 37 insertions(+), 24 deletions(-) diff --git a/src/flight/sixaxis.c b/src/flight/sixaxis.c index 4482d0d89..1a18242b7 100644 --- a/src/flight/sixaxis.c +++ b/src/flight/sixaxis.c @@ -136,8 +136,22 @@ void sixaxis_read() { } } -static void sixaxis_wait_for_still() { +static bool test_gyro_move(const gyro_data_t *last_data, const gyro_data_t *data){ + bool did_move = false; + for (uint8_t i = 0; i < 3; i++) { + const float delta = fabsf(fabsf(last_data->gyro.axis[i] * GYRO_RANGE) - fabsf(data->gyro.axis[i] * GYRO_RANGE)); + if (delta > 0.3f) { + did_move = true; + break; + } + } + return did_move; +} + +//returns true if it's already still, i.e. no move since the first loops +static bool sixaxis_wait_for_still(uint32_t timeout) { const uint32_t start = time_micros(); + int loop_counter = 0; // turn on led uint8_t move_counter = 15; @@ -146,17 +160,10 @@ static void sixaxis_wait_for_still() { memset(&last_data, 0, sizeof(gyro_data_t)); uint32_t now = start; - while (now - start < WAIT_TIME && move_counter > 0) { + while (now - start < timeout && move_counter > 0) { const gyro_data_t data = gyro_spi_read(); - bool did_move = false; - for (uint8_t i = 0; i < 3; i++) { - const float delta = fabsf(fabsf(last_data.gyro.axis[i] * GYRO_RANGE) - fabsf(data.gyro.axis[i] * GYRO_RANGE)); - if (delta > 0.3f) { - did_move = true; - break; - } - } + bool did_move = test_gyro_move(&last_data, &data); if (did_move) { move_counter = 15; @@ -171,24 +178,29 @@ static void sixaxis_wait_for_still() { now = time_micros(); last_data = data; + ++loop_counter; } + return loop_counter < 20; } void sixaxis_gyro_cal() { - float limit[3]; - for (int i = 0; i < 3; i++) { - limit[i] = gyrocal[i]; + for(int retry = 0; retry < 15 ; ++retry){ + if(sixaxis_wait_for_still(WAIT_TIME / 15)){ + //break only if it's already still, otherwise, wait and try again + break; + } + time_delay_ms(200); } - - sixaxis_wait_for_still(); gyro_spi_calibrate(); uint8_t brightness = 0; led_pwm(brightness, 1000); + gyro_data_t last_data = gyro_spi_read();; uint32_t start = time_micros(); uint32_t now = start; - while (now - start < CAL_TIME) { + int cal_counter = CAL_TIME / 1000; + for(int timeout = WAIT_TIME / 1000; timeout > 0; --timeout) { const gyro_data_t data = gyro_spi_read(); led_pwm(brightness, 1000); @@ -197,20 +209,21 @@ void sixaxis_gyro_cal() { brightness &= 0xF; } - for (uint8_t i = 0; i < 3; i++) { - if (data.gyro.axis[i] > limit[i]) - limit[i] += 0.1f; // 100 gyro bias / second change - if (data.gyro.axis[i] < limit[i]) - limit[i] -= 0.1f; - - limit[i] = constrain(limit[i], -GYRO_BIAS_LIMIT, GYRO_BIAS_LIMIT); - lpf(&gyrocal[i], data.gyro.axis[i], lpfcalc(1000, 0.5 * 1e6)); + bool did_move = test_gyro_move(&last_data, &data); + if(!did_move){//only cali gyro when it's still + for (uint8_t i = 0; i < 3; i++) { + lpf(&gyrocal[i], data.gyro.axis[i], lpfcalc(1000, 0.5 * 1e6)); + } + if(--cal_counter <= 0){ + break; + } } while ((time_micros() - now) < 1000) ; now = time_micros(); + last_data = data; } } From 49269af8b44b4f90be3bc94b9c851c9f3c32f49a Mon Sep 17 00:00:00 2001 From: kikoqiu Date: Mon, 21 Aug 2023 09:28:25 +0800 Subject: [PATCH 4/4] use sized integers. format code for consistent coding style. --- src/flight/sixaxis.c | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/flight/sixaxis.c b/src/flight/sixaxis.c index 1a18242b7..db767d731 100644 --- a/src/flight/sixaxis.c +++ b/src/flight/sixaxis.c @@ -136,7 +136,7 @@ void sixaxis_read() { } } -static bool test_gyro_move(const gyro_data_t *last_data, const gyro_data_t *data){ +static bool test_gyro_move(const gyro_data_t *last_data, const gyro_data_t *data) { bool did_move = false; for (uint8_t i = 0; i < 3; i++) { const float delta = fabsf(fabsf(last_data->gyro.axis[i] * GYRO_RANGE) - fabsf(data->gyro.axis[i] * GYRO_RANGE)); @@ -148,10 +148,10 @@ static bool test_gyro_move(const gyro_data_t *last_data, const gyro_data_t *dat return did_move; } -//returns true if it's already still, i.e. no move since the first loops +// returns true if it's already still, i.e. no move since the first loops static bool sixaxis_wait_for_still(uint32_t timeout) { const uint32_t start = time_micros(); - int loop_counter = 0; + uint32_t loop_counter = 0; // turn on led uint8_t move_counter = 15; @@ -184,9 +184,9 @@ static bool sixaxis_wait_for_still(uint32_t timeout) { } void sixaxis_gyro_cal() { - for(int retry = 0; retry < 15 ; ++retry){ - if(sixaxis_wait_for_still(WAIT_TIME / 15)){ - //break only if it's already still, otherwise, wait and try again + for (uint8_t retry = 0; retry < 15; ++retry) { + if (sixaxis_wait_for_still(WAIT_TIME / 15)) { + // break only if it's already still, otherwise, wait and try again break; } time_delay_ms(200); @@ -196,11 +196,12 @@ void sixaxis_gyro_cal() { uint8_t brightness = 0; led_pwm(brightness, 1000); - gyro_data_t last_data = gyro_spi_read();; + gyro_data_t last_data = gyro_spi_read(); + ; uint32_t start = time_micros(); uint32_t now = start; - int cal_counter = CAL_TIME / 1000; - for(int timeout = WAIT_TIME / 1000; timeout > 0; --timeout) { + int32_t cal_counter = CAL_TIME / 1000; + for (int32_t timeout = WAIT_TIME / 1000; timeout > 0; --timeout) { const gyro_data_t data = gyro_spi_read(); led_pwm(brightness, 1000); @@ -210,11 +211,11 @@ void sixaxis_gyro_cal() { } bool did_move = test_gyro_move(&last_data, &data); - if(!did_move){//only cali gyro when it's still + if (!did_move) { // only cali gyro when it's still for (uint8_t i = 0; i < 3; i++) { lpf(&gyrocal[i], data.gyro.axis[i], lpfcalc(1000, 0.5 * 1e6)); } - if(--cal_counter <= 0){ + if (--cal_counter <= 0) { break; } }