diff --git a/examples/01.Quadcopter/01.Quadcopter.ino b/examples/01.Quadcopter/01.Quadcopter.ino index 67316cb..b83de67 100644 --- a/examples/01.Quadcopter/01.Quadcopter.ino +++ b/examples/01.Quadcopter/01.Quadcopter.ino @@ -27,9 +27,9 @@ LED Status ========== OFF - not powered startup: a couple blinks then ON while running gyro calibration (don't move) -blinking long OFF short ON - running loop() DISARMED -blinking long ON short OFF - running loop() ARMED -blink interval longer than 1 second - loop() is taking too much time +blinking long OFF short ON - DISARMED +blinking long ON short OFF - ARMED +blink interval longer than 1 second - imu_onUpdate() is taking too much time fast blinking - something is wrong, connect USB serial for info ##########################################################################################################################*/ @@ -157,8 +157,6 @@ int rcin_cfg_aux_max = 1815; //higest switch pwm // USER-SPECIFIED VARIABLES // //========================================================================================================================// -uint32_t loop_freq = 1000; //The main loop frequency in Hz. imu.h might lower this depending on the sensor used. Do not touch unless you know what you are doing. - //number of motors - out[0..out_MOTOR_COUNT-1] are motors, out[out_MOTOR_COUNT..HW_OUT_COUNT-1] are servos const int out_MOTOR_COUNT = 4; //name the outputs, to make code more readable @@ -200,13 +198,6 @@ float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too h // DECLARE GLOBAL VARIABLES // //========================================================================================================================// -//General stuff -float loop_dt; -uint32_t loop_time; //loop timestamp -uint32_t loop_cnt = 0; //loop counter -uint32_t loop_rt, loop_rt_imu; //runtime of loop and imu sensor retrieval -bool imu_loop_enable = true; //disable imu update during calibration - //Radio communication: int rcin_pwm[RCIN_NUM_CHANNELS]; //filtered raw PWM values float rcin_thro, rcin_roll, rcin_pitch, rcin_yaw; //rcin_thro 0(cutoff) to 1(full); rcin_roll, rcin_pitch, rcin_yaw -1(left,down) to 1(right,up) with 0 center stick @@ -235,6 +226,19 @@ const float rad_to_deg = 57.29577951; //radians to degrees conversion constant //Note: most madflight modules are header only. By placing the madflight include here allows the modules to access the global variables without declaring them extern. #include +//========================================================================================================================// +// SETUP1() LOOP1() EXECUTING ON SECOND CORE (only for dual core MCUs like ESP32 and RP2040) // +//========================================================================================================================// +//Uncomment setup1() and/or loop1() to use the second core on ESP32 / RP2040 +/* +void setup1() { + Serial.println("setup1()"); +} +void loop1() { + Serial.println("loop1()"); delay(100); +} +//*/ + //========================================================================================================================// // SETUP() // //========================================================================================================================// @@ -257,18 +261,18 @@ void setup() { cli.print_i2cScan(); //print i2c scan rcin.setup(); //Initialize radio communication. Set correct USE_RCIN_xxx user specified defines above. Note: rcin_Setup() function is defined in rcin.h, but normally no changes needed there. - //IMU + //IMU: keep on trying until no error is returned (some sensors need a couple of tries...) while(true) { - int rv = imu.setup(loop_freq); //returns 0 on success, positive on error, negative on warning - if(rv<=0) break; + int rv = imu.setup(1000); //request 1000 Hz sample rate, returns 0 on success, positive on error, negative on warning + if(rv<=0) break; warn("IMU: init failed rv= " + String(rv) + ". Retrying...\n"); } - loop_freq = imu.getSampleRate(); - //set filter parameters here, as imu_Setup can modify loop_freq - B_accel = lowpass_to_beta(LP_accel, loop_freq); - B_gyro = lowpass_to_beta(LP_gyro, loop_freq); - B_mag = lowpass_to_beta(LP_mag, loop_freq); - B_radio = lowpass_to_beta(LP_radio, loop_freq); + + //set filter parameters after imu.setup(), as imu.setup() can modify requested sample rate + B_accel = lowpass_to_beta(LP_accel, imu.getSampleRate()); + B_gyro = lowpass_to_beta(LP_gyro, imu.getSampleRate()); + B_mag = lowpass_to_beta(LP_mag, imu.getSampleRate()); + B_radio = lowpass_to_beta(LP_radio, imu.getSampleRate()); baro.setup(); //Barometer mag.setup(); //External Magnetometer @@ -301,13 +305,9 @@ void setup() { //set quarterion to initial yaw, so that AHRS settles faster ahrs_Setup(); - //set times for loop - loop_RateBegin(); - - //start IMU interrupt - do this last in setup(), as the IMU interrupt handler needs all modules configured - imu_interrupt_setup(); //setup imu interrupt handlers - delay(100); - if(loop_cnt==0) die("IMU interrupt not firing."); + //start IMU update handler - do this last in setup(), as the handler needs all modules configured + imu.onUpdate = imu_onUpdate; + if(!imu.waitNewSample()) die("IMU interrupt not firing."); cli.welcome(); @@ -320,7 +320,7 @@ void setup() { void loop() { #ifdef USE_IMU_BUS_SPI - //if IMU uses SPI bus, then read slower i2c sensors here in loop() to keep imu_loop() as fast as possible + //if IMU uses SPI bus, then read slower i2c sensors here in loop() to keep imu_interrupt_handler() as fast as possible i2c_sensors_update(); #endif @@ -348,34 +348,23 @@ void i2c_sensors_update() { } //========================================================================================================================// -// IMU_LOOP() // +// IMU UPDATE LOOP // //========================================================================================================================// - /* - * It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the - * background cause the loop rate to fluctuate. This function basically just waits at the end of every loop iteration until - * the correct time has passed since the start of the current loop for the desired loop rate in Hz. 2kHz is a good rate to - * be at because the loop nominally will run between 2.8kHz - 4.2kHz. This lets us have a little room to add extra computations - * and remain above 2kHz, without needing to retune all of our filtering parameters. - */ - -void imu_loop() { - if(!imu_loop_enable) return; //exit if calibration is running - - //update loop_ variables - uint32_t now = micros(); - loop_dt = (now - loop_time)/1000000.0; - loop_time = now; - loop_cnt++; +//This is _MAIN_ function of this program. It is called when new IMU data is available. +void imu_onUpdate() { //Blink LED - loop_Blink(); + led_Blink(); + + // Apply low-pass filters to remove noise + imu_Filter(); - //Get vehicle state - imu_GetData(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and applies low-pass filters to remove noise //ahrs filter method: Madgwick or Mahony - SELECT ONE: - //ahrs_Madgwick(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ, loop_dt); //Madgwick filter quaternion update - ahrs_Mahony(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ, loop_dt); //Mahony filter quaternion update - ahrs_ComputeAngles(&ahrs_roll, &ahrs_pitch, &ahrs_yaw); //get ahrs_roll, ahrs_pitch, and ahrs_yaw angle estimates (degrees) from quaterion + //ahrs_Madgwick(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ, imu.dt); //Madgwick filter quaternion update + ahrs_Mahony(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ, imu.dt); //Mahony filter quaternion update + + //get ahrs_roll, ahrs_pitch, and ahrs_yaw angle estimates (degrees) from quaterion + ahrs_ComputeAngles(&ahrs_roll, &ahrs_pitch, &ahrs_yaw); //Get radio state rcin_GetCommands(); //Pulls current available radio commands @@ -392,89 +381,26 @@ void imu_loop() { //Actuator mixing control_Mixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here - //Command actuators + //Motor output out_KillSwitchAndFailsafe(); //Cut all motor outputs if DISARMED or failsafe triggered. out_SetCommands(); //Sends command pulses to motors (only if out_armed=true) and servos -#ifdef USE_IMU_BUS_I2C - //if IMU uses i2c bus, then get i2c sensor readings in imu_loop() to prevent i2c bus collisions. Alternatively, put the IMU on a separate i2c bus. - i2c_sensors_update(); -#endif - - //record max runtime loop - uint32_t rt = micros() - loop_time; - if(loop_rt < rt) loop_rt = rt; - - //bb.log_imu(); //full speed black box logging imu data, memory fills up quickly... -} - -//========================================================================================================================// -// IMU INTERRUPT HANDLER // -//========================================================================================================================// -// This runs the IMU updates triggered from pin HW_PIN_IMU_EXTI interrupt. When using FreeRTOS with -// HW_USE_FREERTOS the IMU update is not executed directly in the interrupt handler, but a high priority task is used. -// This prevents FreeRTOS watchdog resets. The delay (latency) from rising edge INT pin to start of imu_loop is approx. -// 10 us on ESP32 and 50 us on RP2040. - -#ifdef HW_USE_FREERTOS - TaskHandle_t imu_task_handle; - - void imu_interrupt_setup() { - xTaskCreate(imu_task, "imu_task", 4096, NULL, HW_RTOS_IMUTASK_PRIORITY /*priority 0=lowest*/, &imu_task_handle); - //vTaskCoreAffinitySet(IsrTaskHandle, 0); - attachInterrupt(digitalPinToInterrupt(HW_PIN_IMU_EXTI), imu_interrupt_handler, RISING); - } - - void imu_task(void*) { - for(;;) { - ulTaskNotifyTake( pdTRUE, portMAX_DELAY ); - imu_loop(); - } - } - - void imu_interrupt_handler() { - //let imu_task handle the interrupt - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - vTaskNotifyGiveFromISR(imu_task_handle, &xHigherPriorityTaskWoken); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); - } -#else - volatile bool imu_interrupt_busy = false; - - void imu_interrupt_setup() { - attachInterrupt(digitalPinToInterrupt(HW_PIN_IMU_EXTI), imu_interrupt_handler, RISING); - } - - void imu_interrupt_handler() { - if(imu_interrupt_busy) return; - imu_interrupt_busy = true; - imu_loop(); - imu_interrupt_busy = false; - } -#endif - + //if IMU uses i2c bus, then get i2c sensor readings in imu_interrupt_handler() to prevent i2c bus collisions. Alternatively, put the IMU on a separate i2c bus. + #ifdef USE_IMU_BUS_I2C + i2c_sensors_update(); + #endif -//========================================================================================================================// -// SETUP1() LOOP1() EXECUTING ON SECOND CORE (only ESP32 and RP2040) // -//========================================================================================================================// -//Uncomment setup1() and/or loop1() to use the second core on ESP32 / RP2040 -/* -void setup1() { - Serial.println("setup1()"); + //bb.log_imu(); //full speed black box logging of IMU data, memory fills up quickly... } -void loop1() { - Serial.println("loop1()"); delay(100); -} -//*/ //========================================================================================================================// -// IMU_LOOP() FUNCTIONS - in same order as they are called from imu_loop() // +// IMU UPDATE LOOP FUNCTIONS - in same order as they are called from imu_onUpdate() // //========================================================================================================================// -void loop_Blink() { +void led_Blink() { //Blink LED once per second, if LED blinks slower then the loop takes too much time, use print_loop_Rate() to investigate. //DISARMED: long off, short on, ARMED: long on, short off - if(loop_cnt % loop_freq <= loop_freq / 10) + if(imu.update_cnt % imu.getSampleRate() <= imu.getSampleRate() / 10) led.set(!out_armed); //short interval else led.set(out_armed); //long interval @@ -483,12 +409,7 @@ void loop_Blink() { // Reads accelerometer, gyro, and magnetometer data from IMU and stores it as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ. // A simple first-order low-pass filter is used to get rid of high frequency noise in these raw signals. // Finally, the constant errors found in calibrate_IMU_error() on startup are subtracted from the accelerometer and gyro readings. -void imu_GetData() { - uint32_t t1 = micros(); - //imu_Read() returns correctly NED oriented and correctly scaled values in g's, deg/sec, and uT (see file imu.h) - imu.update(); - loop_rt_imu = micros() - t1; - +void imu_Filter() { //Accelerometer //LP filter corrected accelerometer data AccX = (1.0 - B_accel) * AccX + B_accel * (imu.ax - cfg.imu_cal_ax); @@ -623,23 +544,23 @@ void control_Angle(bool zero_integrators) { //Roll PID float error_roll = roll_des - ahrs_roll; - integral_roll += error_roll * loop_dt; + integral_roll += error_roll * imu.dt; integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_roll = GyroX; roll_PID = 0.01 * (Kp_ro_pi_angle*error_roll + Ki_ro_pi_angle*integral_roll - Kd_ro_pi_angle*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch PID float error_pitch = pitch_des - ahrs_pitch; - integral_pitch += error_pitch * loop_dt; + integral_pitch += error_pitch * imu.dt; integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_pitch = GyroY; pitch_PID = 0.01 * (Kp_ro_pi_angle*error_pitch + Ki_ro_pi_angle*integral_pitch - Kd_ro_pi_angle*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range //Yaw PID, stablize on rate from GyroZ float error_yaw = yawRate_des - GyroZ; - integral_yaw += error_yaw * loop_dt; + integral_yaw += error_yaw * imu.dt; integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup - float derivative_yaw = (error_yaw - error_yaw_prev) / loop_dt; + float derivative_yaw = (error_yaw - error_yaw_prev) / imu.dt; yaw_PID = 0.01 * (Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range //Update derivative variables @@ -678,16 +599,16 @@ void control_Angle2(bool zero_integrators) { //Outer loop - PID on angle for roll & pitch //Roll float error_roll = roll_des - ahrs_roll; - integral_roll_ol += error_roll * loop_dt; + integral_roll_ol += error_roll * imu.dt; integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup - float derivative_roll = (ahrs_roll - roll_IMU_prev) / loop_dt; + float derivative_roll = (ahrs_roll - roll_IMU_prev) / imu.dt; float roll_des_ol = Kp_ro_pi_angle*error_roll + Ki_ro_pi_angle*integral_roll_ol;// - Kd_ro_pi_angle*derivative_roll; //Pitch float error_pitch = pitch_des - ahrs_pitch; - integral_pitch_ol += error_pitch * loop_dt; + integral_pitch_ol += error_pitch * imu.dt; integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - float derivative_pitch = (ahrs_pitch - pitch_IMU_prev) / loop_dt; + float derivative_pitch = (ahrs_pitch - pitch_IMU_prev) / imu.dt; float pitch_des_ol = Kp_ro_pi_angle*error_pitch + Ki_ro_pi_angle*integral_pitch_ol;// - Kd_ro_pi_angle*derivative_pitch; //Apply loop gain, constrain, and LP filter for artificial damping @@ -702,24 +623,24 @@ void control_Angle2(bool zero_integrators) { //Inner loop - PID on rate for roll & pitch //Roll error_roll = roll_des_ol - GyroX; - integral_roll_il += error_roll * loop_dt; + integral_roll_il += error_roll * imu.dt; integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup - derivative_roll = (error_roll - error_roll_prev) / loop_dt; + derivative_roll = (error_roll - error_roll_prev) / imu.dt; roll_PID = 0.01 * (Kp_ro_pi_rate*error_roll + Ki_ro_pi_rate*integral_roll_il + Kd_ro_pi_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch error_pitch = pitch_des_ol - GyroY; - integral_pitch_il += error_pitch * loop_dt; + integral_pitch_il += error_pitch * imu.dt; integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup - derivative_pitch = (error_pitch - error_pitch_prev) / loop_dt; + derivative_pitch = (error_pitch - error_pitch_prev) / imu.dt; pitch_PID = 0.01 * (Kp_ro_pi_rate*error_pitch + Ki_ro_pi_rate*integral_pitch_il + Kd_ro_pi_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range //Single loop //Yaw float error_yaw = yawRate_des - GyroZ; - integral_yaw += error_yaw * loop_dt; + integral_yaw += error_yaw * imu.dt; integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup - float derivative_yaw = (error_yaw - error_yaw_prev) / loop_dt; + float derivative_yaw = (error_yaw - error_yaw_prev) / imu.dt; yaw_PID = 0.01 * (Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range //Update derivative variables @@ -760,23 +681,23 @@ void control_Rate(bool zero_integrators) { //Roll float error_roll = rollRate_des - GyroX; - integral_roll += error_roll * loop_dt; + integral_roll += error_roll * imu.dt; integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup - float derivative_roll = (error_roll - error_roll_prev) / loop_dt; + float derivative_roll = (error_roll - error_roll_prev) / imu.dt; roll_PID = 0.01 * (Kp_ro_pi_rate*error_roll + Ki_ro_pi_rate*integral_roll + Kd_ro_pi_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch float error_pitch = pitchRate_des - GyroY; - integral_pitch += error_pitch * loop_dt; + integral_pitch += error_pitch * imu.dt; integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup - float derivative_pitch = (error_pitch - error_pitch_prev) / loop_dt; + float derivative_pitch = (error_pitch - error_pitch_prev) / imu.dt; pitch_PID = 0.01 * (Kp_ro_pi_rate*error_pitch + Ki_ro_pi_rate*integral_pitch + Kd_ro_pi_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range //Yaw, stablize on rate from GyroZ float error_yaw = yawRate_des - GyroZ; - integral_yaw += error_yaw * loop_dt; + integral_yaw += error_yaw * imu.dt; integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup - float derivative_yaw = (error_yaw - error_yaw_prev) / loop_dt; + float derivative_yaw = (error_yaw - error_yaw_prev) / imu.dt; yaw_PID = 0.01 * (Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range //Update derivative variables @@ -869,22 +790,16 @@ void out_SetCommands() { // SETUP() FUNCTIONS // //========================================================================================================================// -void loop_RateBegin() { - loop_time = micros(); - loop_cnt = 0; -} - //set initial quarterion void ahrs_Setup() { //estimate yaw based on mag only (assumes vehicle is horizontal) - //warm up imu by getting 100 samples + //warm up imu and mag by getting 100 samples for(int i=0;i<100;i++) { uint32_t start = micros(); mag.update(); - imu_GetData(); - while(micros() - start < 1000000/loop_freq); //wait until next sample time + while(micros() - start < 1000000 / imu.getSampleRate()); //wait until next sample time } //calculate yaw angle diff --git a/readme.md b/readme.md index f8a2701..0fd3fbf 100644 --- a/readme.md +++ b/readme.md @@ -62,13 +62,13 @@ For full documention see [madflight.com](https://madflight.com) and the source c 3. Setup the USER-SPECIFIED DEFINES section 4. If you're not using a default pinout (see below) then setup your board pinout in the BOARD section. 5. Connect your IMU (gyro/acceleration) sensor as shown below. -6. Compile and upload Quadcopter.ino to your board. Connect the Serial Monitor at 115200 baud and check the messages. Type 'help' to see the available CLI commands. -7. Check that IMU sensor and AHRS are working correctly: use CLI print commands to show gyro, accelerometer, magnetometer and roll/pitch/yaw. -8. Use CLI to calibate the sensor. +6. Compile and upload Quadcopter.ino to your board. Connect the Serial Monitor at 115200 baud and check the messages. Type `help` to see the available CLI commands. +7. Use CLI print commands like `pimu`, `pgyro`, `proll` to Check that IMU sensor and AHRS are working correctly. +8. IMPORTANT: Use CLI `calimu` and `calmag` to calibate the sensors. 9. Connect radio receiver to your development board according to the configured pins. 10. Edit the RC RECEIVER CONFIG section. Either match you RC equipment to the settings, or change the settings to match your RC equipment. -11. Check your radio setup: Use CLI print commands to show pwm and scaled radio values. -12. Connect motors (no props) and battery and check that motor outputs are working correctly. For debugging, use CLI to show motor output. +11. Check your radio setup: Use CLI `ppwm` and `pradio` to show pwm and scaled radio values. +12. Connect motors (no props) and battery and check that motor outputs are working correctly. For debugging, use CLI `pmot` to show motor output. 13. Mount props, go to an wide open space, and FLY! @@ -95,7 +95,6 @@ By default madflight has these safety features enabled: - The madflight flight controller runs standard `setup()` and `loop()`. - It mainly uses plain Arduino functionality: Serial, Wire, and SPI. One custom hardware dependent library is used for PWM. Therefor, it can fairly easily ported to other 32 bit microcontrollers that support the Arduino framework. Also porting to other build environments like PlatformIO or CMake should not be a huge effort. - The following modules are used: - - `loop` Main loop control - `imu` Inertial Measurement Unit, retrieves accelerometer, gyroscope, and magnetometer sensor data - `ahrs` Attitude Heading Reference System, estimates roll, yaw, pitch - `rcin` RC INput, retrieves RC receiver data diff --git a/src/madflight/cli/cli.h b/src/madflight/cli/cli.h index 5a42cc0..a7aad7b 100644 --- a/src/madflight/cli/cli.h +++ b/src/madflight/cli/cli.h @@ -48,6 +48,7 @@ class CLI { "po Overview: pwm1, rcin_roll, gyroX, accX, magX, ahrs_roll, pid_roll, motor1, loop_rt\n" "ppwm Radio pwm (expected: 1000 to 2000)\n" "pradio Scaled radio (expected: -1 to 1)\n" + "pimu IMU loop timing (expected: imu%% < 50)\n" "pgyro Filtered gyro (expected: -250 to 250, 0 at rest)\n" "pacc Filtered accelerometer (expected: -2 to 2; x,y 0 when level, z 1 when level)\n" "pmag Filtered magnetometer (expected: -300 to 300)\n" @@ -55,7 +56,6 @@ class CLI { "ppid PID output (expected: -1 to 1)\n" "pmot Motor output (expected: 0 to 1)\n" "pservo Servo output (expected: 0 to 1)\n" - "ploop Loop timing in microseconds (expected: 1000000 / loop_freq)\n" "pbat Battery voltage, current, Ah used and Wh used\n" "-- BLACK BOX --\n" "bbls List files\n" @@ -131,7 +131,7 @@ class CLI { print_flag[8] = !print_flag[8]; }else if(cmd == "pservo") { print_flag[9] = !print_flag[9]; - }else if(cmd == "ploop") { + }else if(cmd == "pimu") { print_flag[10] = !print_flag[10]; }else if(cmd == "pbat") { print_flag[11] = !print_flag[11]; @@ -226,8 +226,6 @@ class CLI { //Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface void calibrate_IMU2(bool gyro_only = false) { - imu_loop_enable = false; //disable running of imu_loop() - //Read IMU values, and average the readings int cnt = 3000; float axerr = 0; @@ -237,14 +235,13 @@ class CLI { float gyerr = 0; float gzerr = 0; for(int i=0; i mpu_iface(i2c, IMU_I2C_ADR); MPUXXXX imu_Sensor(MPU9250, &mpu_iface); -//===================================================================================================================== -// IMU_USE_I2C_MPU9150 -//===================================================================================================================== #elif IMU_USE == IMU_USE_I2C_MPU9150 #define IMU_TYPE "IMU_USE_I2C_MPU9150" #define IMU_USE_BUS_I2C @@ -162,9 +146,6 @@ extern Imu &imu; MPU_InterfaceI2C mpu_iface(i2c, IMU_I2C_ADR); MPUXXXX imu_Sensor(MPUXXXX::MPU9150, &mpu_iface); -//===================================================================================================================== -// IMU_USE_I2C_MPU6500 -//===================================================================================================================== #elif IMU_USE == IMU_USE_I2C_MPU6500 #define IMU_TYPE "IMU_USE_I2C_MPU6500" #define IMU_USE_BUS_I2C @@ -174,9 +155,6 @@ extern Imu &imu; MPU_InterfaceI2C mpu_iface(i2c, IMU_I2C_ADR); MPUXXXX imu_Sensor(MPUXXXX::MPU6500, &mpu_iface); -//===================================================================================================================== -// IMU_USE_I2C_MPU6050 -//===================================================================================================================== #elif IMU_USE == IMU_USE_I2C_MPU6050 #define IMU_TYPE "IMU_USE_I2C_MPU6050" #define IMU_USE_BUS_I2C @@ -186,51 +164,137 @@ extern Imu &imu; MPU_InterfaceI2C mpu_iface(i2c, IMU_I2C_ADR); MPUXXXX imu_Sensor(MPUXXXX::MPU6050, &mpu_iface); -//===================================================================================================================== -// IMU_USE_I2C_MPU6000 -//===================================================================================================================== #elif IMU_USE == IMU_USE_I2C_MPU6000 #define IMU_TYPE "IMU_USE_I2C_MPU6000" #define IMU_USE_BUS_I2C #define IMU_HAS_MAG 0 MPU_InterfaceI2C mpu_iface(i2c, IMU_I2C_ADR); MPUXXXX imu_Sensor(MPUXXXX::MPU6000, &mpu_iface); - -//===================================================================================================================== + // None or undefined -//===================================================================================================================== #elif IMU_USE == IMU_USE_NONE || !defined IMU_USE #error "IMU_USE not defined" - -//===================================================================================================================== + // Invalid value -//===================================================================================================================== #else #error "invalid IMU_USE value" #endif -class ImuGeneric: public Imu { - public: - bool hasMag() { return IMU_HAS_MAG; } +//========================================================================================================================// +// Imu Class Implementation +//========================================================================================================================// + +void _imu_ll_interrupt_setup(); //prototype +volatile bool _imu_ll_interrupt_busy = false; +volatile uint32_t _imu_ll_interrupt_ts = 0; + +bool Imu::hasMag() { return IMU_HAS_MAG; } + +//returns 0 on success, positive on error, negative on warning +int Imu::setup(uint32_t sampleRate) { + int rv = imu_Sensor.begin(IMU_GYRO_DPS, IMU_ACCEL_G, sampleRate); + _sampleRate = imu_Sensor.get_rate(); + Serial.printf(IMU_TYPE " sample_rate=%dHz rv=%d\n", (int)_sampleRate, (int)rv); + onUpdate = NULL; + _imu_ll_interrupt_busy = false; + _imu_ll_interrupt_ts = 0; + overrun_cnt = 0; + update_cnt = 0; + runtime_int = 0; + runtime_bus = 0; + runtime_tot_max = 0; + dt = 0; + ts = micros(); + _imu_ll_interrupt_setup(); + return rv; +} + +//wait for new sample, returns false on fail +bool Imu::waitNewSample() { + uint32_t last_cnt = update_cnt; + uint32_t start = millis(); + while( last_cnt == update_cnt && millis() - start <= (10*1000) / _sampleRate ); + return (last_cnt != update_cnt); +} + +void Imu::_interrupt_handler() { + //local copy of timestamp (_imu_ll_interrupt_ts might change during execution of this function) + uint32_t sample_ts = _imu_ll_interrupt_ts; - //returns 0 on success, positive on error, negative on warning - int setup(uint32_t sampleRate) { - int rv = imu_Sensor.begin(IMU_GYRO_DPS, IMU_ACCEL_G, sampleRate); - _sampleRate = imu_Sensor.get_rate(); - Serial.printf(IMU_TYPE " sample_rate=%dHz rv=%d\n", (int)_sampleRate, (int)rv); - return rv; + //latency between start of low level interrupt handler and start of this method + runtime_int = micros() - sample_ts; + + //get sensor data and update timestamps, count + #if IMU_HAS_MAG + imu_Sensor.getMotion9NED(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); + #else + imu_Sensor.getMotion6NED(&ax, &ay, &az, &gx, &gy, &gz); + #endif + IMU_ROTATE(); + dt = (sample_ts - ts) / 1000000.0; + ts = sample_ts; + runtime_bus = micros() - sample_ts - runtime_int; //runtime of SPI/I2C transfer + update_cnt++; + + //call external update event handler + if(onUpdate) onUpdate(); + + uint32_t rt = micros() - sample_ts; //runtime of full update + if(runtime_tot_max < rt) runtime_tot_max = rt; //max runtime +} + +//global Imu class instance +Imu imu; + +//========================================================================================================================// +// _IMU_LL_ IMU Low Level Interrrupt Handler +//========================================================================================================================// +// This runs the IMU updates triggered from pin HW_PIN_IMU_EXTI interrupt. When using FreeRTOS with HW_USE_FREERTOS the +// IMU update is not executed directly in the interrupt handler, but a high priority task is used. This prevents FreeRTOS +// watchdog resets. The delay (latency) from rising edge INT pin to handler is approx. 10 us on ESP32 and 50 us on RP2040. + +void _imu_ll_interrupt_handler(); + +#ifdef HW_USE_FREERTOS + TaskHandle_t _imu_ll_task_handle = NULL; + + void _imu_ll_task(void*) { + for(;;) { + ulTaskNotifyTake( pdTRUE, portMAX_DELAY ); + imu._interrupt_handler(); } + } - void update() { - #if IMU_HAS_MAG - imu_Sensor.getMotion9NED(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); - #else - imu_Sensor.getMotion6NED(&ax, &ay, &az, &gx, &gy, &gz); - #endif - IMU_ROTATE(); + void _imu_ll_interrupt_setup() { + if(!_imu_ll_task_handle) { + xTaskCreate(_imu_ll_task, "_imu_ll_task", 4096, NULL, HW_RTOS_IMUTASK_PRIORITY /*priority 0=lowest*/, &_imu_ll_task_handle); + //vTaskCoreAffinitySet(IsrTaskHandle, 0); } -}; + attachInterrupt(digitalPinToInterrupt(HW_PIN_IMU_EXTI), _imu_ll_interrupt_handler, RISING); + } + +#else -ImuGeneric imu_instance; + void _imu_ll_interrupt_setup() { + attachInterrupt(digitalPinToInterrupt(HW_PIN_IMU_EXTI), _imu_ll_interrupt_handler, RISING); + } +#endif -Imu &imu = imu_instance; \ No newline at end of file +void _imu_ll_interrupt_handler() { + _imu_ll_interrupt_ts = micros(); + if (_imu_ll_interrupt_busy) { //note: time difference between check/update of _imu_ll_interrupt_busy can cause a race condition... + imu.overrun_cnt++; + } else { + _imu_ll_interrupt_busy = true; + #ifdef HW_USE_FREERTOS + //let RTOS task _imu_ll_task handle the interrupt + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + vTaskNotifyGiveFromISR(_imu_ll_task_handle, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + #else + //call interrupt handler directly from interrupt context + imu._interrupt_handler(); + #endif + _imu_ll_interrupt_busy = false; + } +} \ No newline at end of file diff --git a/src/madflight/interface.h b/src/madflight/interface.h index 324a913..812e5d7 100644 --- a/src/madflight/interface.h +++ b/src/madflight/interface.h @@ -27,6 +27,10 @@ extern Rcin &rcin; class Imu { public: + //sample data + uint32_t ts = 0; //sample low level interrupt trigger timestamp in us + float dt = 0; //time since last sample in seconds + uint32_t update_cnt = 0; //number of updates float ax = 0; //"North" acceleration in G float ay = 0; //"East" acceleration in G float az = 0; //"Down" acceleration in G @@ -36,15 +40,32 @@ class Imu { float mx = 0; //"North" magnetic flux in uT float my = 0; //"East" magnetic flux in uT float mz = 0; //"Down" magnetic flux in uT - virtual bool hasMag() = 0; //returns true if IMU has a magnetometer - virtual int setup(uint32_t sampleRate) = 0; - virtual void update() = 0; - uint32_t getSampleRate() {return _sampleRate;} + + //interrupt statistics + uint32_t overrun_cnt = 0; //number of interrupt overruns (should stay 0) + bool _imu_interrupt_busy = false; //is interrupt handler running? + uint32_t runtime_int = 0; //interrupt latency from start of interrupt handler to start of interrupt task in us + uint32_t runtime_bus = 0; //runtime of SPI/I2C bus transfer in us + uint32_t runtime_tot_max = 0; //max runtime imu update including transfer in us + + //pointer to onUpdate event handler + void (*onUpdate)(void) = NULL; + + //methods + int setup(uint32_t sampleRate); + bool waitNewSample(); //wait for new sample, returns false on fail + bool hasMag(); //returns true if IMU has a magnetometer + uint32_t getSampleRate() {return _sampleRate;} //sensor sample rate in Hz + uint32_t getSamplePeriod() {return (_sampleRate != 0 ? 1000000 / _sampleRate : 1000000);} //sensor sample period in us + + //low level interrupt handler (should be private, but is public, because called from interrupt) + void _interrupt_handler(); + protected: uint32_t _sampleRate = 0; //sensor sample rate in Hz }; -extern Imu &imu; +extern Imu imu; //================================================================================================= // Barometer