From 4822f6c6e035d9be1cf4b614b113577a8be919ee Mon Sep 17 00:00:00 2001 From: Isaak Devos Date: Fri, 31 May 2024 23:11:57 +0200 Subject: [PATCH] IMU more efficient dps calculation, now reaching targeted pid frequency of 1.6khz. --- software/MySrc/Drivers/imu.c | 18 +++++++++++++----- software/MySrc/Flight/pid_controller.c | 9 ++++----- software/MySrc/mymain.c | 9 ++++----- 3 files changed, 21 insertions(+), 15 deletions(-) diff --git a/software/MySrc/Drivers/imu.c b/software/MySrc/Drivers/imu.c index 2c24505..0eb736d 100644 --- a/software/MySrc/Drivers/imu.c +++ b/software/MySrc/Drivers/imu.c @@ -26,6 +26,7 @@ static int8_t set_accel_gyro_config(struct bmi2_dev *bmi); static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); +static float lsb_to_dps_shifting(int16_t val, float dps, uint8_t bit_width); static int8_t set_accel_gyro_config(struct bmi2_dev *bmi); static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); @@ -124,8 +125,8 @@ void imu_process(IMU_handle_t *imu_h) { imu_h->last_err = IMU_OK; // "reset" last error if (imu_h->sensor_data.status & BMI2_DRDY_ACC) { - imu_h->acc.roll = lsb_to_mps2(imu_h->sensor_data.acc.x, 2, 16); - imu_h->acc.pitch = lsb_to_mps2(imu_h->sensor_data.acc.y, 2, 16); + imu_h->acc.pitch = lsb_to_mps2(imu_h->sensor_data.acc.x, 2, 16); // values are remapped acording to chip orientation on pcb + imu_h->acc.roll = lsb_to_mps2(imu_h->sensor_data.acc.y, 2, 16); imu_h->acc.yaw = lsb_to_mps2(imu_h->sensor_data.acc.z, 2, 16); } @@ -134,9 +135,9 @@ void imu_process(IMU_handle_t *imu_h) { } if (imu_h->sensor_data.status & BMI2_DRDY_GYR) { - imu_h->gyr.roll = roundf(lsb_to_dps(imu_h->sensor_data.gyr.x, 2000, 16) * 100) / 100; - imu_h->gyr.pitch = roundf(lsb_to_dps(imu_h->sensor_data.gyr.y, 2000, 16) * 100) / 100; - imu_h->gyr.yaw = roundf(lsb_to_dps(imu_h->sensor_data.gyr.z, 2000, 16) * 100) / 100; + imu_h->gyr.pitch = roundf(lsb_to_dps_shifting(imu_h->sensor_data.gyr.x, 2000, 16) * 100) / 100; // values are remapped acording to chip orientation on pcb + imu_h->gyr.roll = roundf(lsb_to_dps_shifting(imu_h->sensor_data.gyr.y, 2000, 16) * 100) / 100; + imu_h->gyr.yaw = roundf(lsb_to_dps_shifting(imu_h->sensor_data.gyr.z, 2000, 16) * 100) / 100; } else { if (imu_h->last_err == IMU_WARN_ACC_READ_NOT_READY) // if both acc and gyro are not ready @@ -390,6 +391,13 @@ static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) return (dps / (half_scale)) * (val); } +static float lsb_to_dps_shifting(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = (float)(1 << (bit_width - 1)); + + return (dps / half_scale) * (float)val; +} + void bmi2_error_codes_print_result(int8_t rslt) { switch (rslt) diff --git a/software/MySrc/Flight/pid_controller.c b/software/MySrc/Flight/pid_controller.c index 23264dd..552e2f1 100644 --- a/software/MySrc/Flight/pid_controller.c +++ b/software/MySrc/Flight/pid_controller.c @@ -11,12 +11,11 @@ -#define TAU 0.00065 -#define KP 0.35 -#define KI 0.2 -#define KD 1.2 //21:17 from 1.8 to 1.2 - +#define TAU 0.000625 +#define KP 0.5 +#define KI 1 +#define KD 0.5 diff --git a/software/MySrc/mymain.c b/software/MySrc/mymain.c index 81cb2cf..d94962a 100644 --- a/software/MySrc/mymain.c +++ b/software/MySrc/mymain.c @@ -128,10 +128,10 @@ uint32_t flight_cycle_time = 0; void log_stats(void){ if (cli_h.cli_connected_flag) { //LOGD("HAL_TIM_CNT: %d", __HAL_TIM_GET_COUNTER(&htim2)); -// LOGI("Flight cycle time (target is 625us): %dus", flight_cycle_time); + LOGI("Flight cycle time (target is 625us): %dus", flight_cycle_time); // LOGD("gyr: roll=%.2f pitch=%.2f yaw=%.2f (from imu_h)", imu_h.gyr.roll, imu_h.gyr.pitch, imu_h.gyr.yaw); - LOGD("gyr: roll=%d pitch=%d yaw=%d", imu_data.roll, imu_data.pitch, imu_data.yaw); +// LOGD("gyr: roll=%d pitch=%d yaw=%d", imu_data.roll, imu_data.pitch, imu_data.yaw); // LOGD("Armed: %d", is_armed_flag); } @@ -147,19 +147,18 @@ void mymain(void) { //imu_process(&imu_h); flight_ctrl_cycle(); - motors_process(&motors_h); // ^^^^^ end of time sensitive flight control code ^^^^^ // while (__HAL_TIM_GET_COUNTER(&htim_flight_cycle) < FLIGHT_CYCLE_FREQUENCY) { // do other stuff while waiting - if (__HAL_TIM_GET_COUNTER(&htim_flight_cycle) < (FLIGHT_CYCLE_FREQUENCY - (FLIGHT_CYCLE_FREQUENCY / 5))) { // if we are not near the last 20% of wait_time_us, we can do other stuff. else don't take risk of missing next flight cycle + if (__HAL_TIM_GET_COUNTER(&htim_flight_cycle) < (FLIGHT_CYCLE_FREQUENCY - (FLIGHT_CYCLE_FREQUENCY / 3))) { // if we are not near the last 20% of wait_time_us, we can do other stuff. else don't take risk of missing next flight cycle // ----- all non-critical code goes here ----- // none_blocking_delay(1000, &led_toggle_last_ms, (callback_t) LED_toggle, NULL); none_blocking_delay(25, &cli_process_last_ms, (callback_t) cli_process, &cli_h); -// none_blocking_delay(1000, &log_stats_last_ms, (callback_t) log_stats, NULL); + none_blocking_delay(1000, &log_stats_last_ms, (callback_t) log_stats, NULL); // ^^^^^ all non-critical code goes here ^^^^^ // }