IMU more efficient dps calculation, now reaching targeted pid frequen…

…cy of 1.6khz.
IDV7 · May 31, 2024 · 4822f6c · 4822f6c
1 parent b0e2f8f
commit 4822f6c
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 15 deletions.
diff --git 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
@@ -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
@@ -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 ^^^^^ //
 
             }