v2

Author: qqqlab

README.md

@@ -13,6 +13,12 @@ The source code and [website](https://madflight.com/) have extensive documentati
 
 <img src="extras/img/madflight RP2040 flight controller.jpeg" title="madflight RP2040 flight controller" width="38%" /> <img src="extras/img/madflight drone.jpeg" title="madflight drone" width="30%" /> <img src="extras/img/madflight ESP32 flight controller.jpeg" title="madflight ESP32 flight controller" width="29%" />
 
+## Version 2.0.0-DEV
+
+If you clone/download this repository you get version 2.0.0-DEV, which is BLEEDING EDGE - not flight tested at all, might not even compile, and will completely change in the next hour... 
+
+Use [release version 1.3.3](https://github.com/qqqlab/madflight/releases) if you want something that actually has logged flight hours.
+
 ## Required Hardware
 
 - [Development board](https://madflight.com/Controller-Boards/): 
@@ -44,10 +50,10 @@ The source code and [website](https://madflight.com/) have extensive documentati
 3. Open *Examples for custom libraries->madflight->Quadcopter.ino* in the Arduino IDE.
 4. Edit the HARDWARE section in madflight_config.h to enable the connected peripherals.
 5. If you're not using the default pinout then setup your board pinout in the CUSTOM PINS section.
-6. Compile Quadcopter.ino and upload it to your board. Connect the Serial Monitor at 115200 baud and check the messages. Type `help` to see the available CLI commands.
+6. Compile Quadcopter.ino and upload it to your board. Connect the Serial Monitor at 115200 baud and check the startup messages. Type `help` to see the available CLI commands.
 7. Type `calradio` and follow the prompts to setup your RC radio receiver.
 8. IMPORTANT: Use CLI `calimu` and `calmag` to calibate the sensors.
-9. Use CLI commands `pimu`, `pahrs`, `pradio`, `pmot`, etc. and check that IMU sensor, AHRS and RC Receiver are working correctly. 
+9. Use CLI commands `pimu`, `pahr`, `prcl`, `pmot`, etc. and check that IMU sensor, AHRS and RC Receiver are working correctly. 
 10. Connect motors (no props) and battery and check that motors are spinning correctly.
 11. Mount props, go to an wide open space, and FLY!
 
@@ -88,7 +94,7 @@ By default **madflight** has these safety features enabled:
     - `rcin` RC INput, retrieves RC receiver data
     - `veh` Vehicle information
 - Most modules are interfaced through a global object, for example the `imu` object has property `imu.gx` which is the current gyro x-axis rate in degrees per second for the selected IMU chip.
-- The module implementations are in subdirectories of the `src/madflight` directory. Here you find the module header file, e.g. `src/madflight/imu/imu.h`, and the interface declaration `src/madflight/imu/imu_interface.h`
+- The module implementations are in subdirectories of the `src/madflight` directory. Here you find the module header file, e.g. `src/madflight/imu/imu.h`, and the interface declaration `src/madflight/imu/imu.h`
 - The module files are usually header only, that is, the header also includes the implemention.
 
 ## Disclaimer

examples/01.Quadcopter/01.Quadcopter.ino

@@ -6,15 +6,18 @@ Minimal quadcopter demo program for madflight Arduino ESP32-S3 / ESP32 / RP2350
 
 See http://madflight.com for detailed description
 
-Needs: 
- - IMU sensor (SPI or I2C)
- - RC receiver with 5 channels (CRSF/ELRS preferred)
- - 4 brushless motors with ESCs (or brushed motors with MOSFETs)
+Required Hardware
 
-Connecting:
-IMU: connect IMU_EXTI, IMU_CS, SPI_MISO, SPI_MOSI, SPI_CLK for SPI (or IMU_EXTI, I2C_SDA, I2C_SCL for I2C)
-RC receiver: connect RCIN_RX to receiver TX pin
-ESCs: PWM1-4 to the ESC inputs
+    IMU sensor (SPI or I2C)
+    RC receiver with 5 channels (CRSF/ELRS preferred)
+    4 brushless motors with ESCs
+
+Connecting Hardware
+
+    SPI IMU: connect pin_imu_int, pin_imu_cs, pin_spi0_miso, pin_spi0_mosi, pin_spi0_sclk
+    or for I2C IMU: connect pin_imu_int, pin_i2c1_scl, pin_i2c1_sda
+    RC receiver: connect pin_ser0_rx to receiver TX pin
+    ESCs: pin_out0 ... pin_out3 to the ESC inputs of motor1 ... motor4
 
 Arming: Set throttle low, then flip arm switch from DISARMED to ARMED.
 Disarming: Flip arm switch from ARMED to DISARMED, at any throttle position. "Kill switch".
@@ -80,23 +83,26 @@ void setup() {
   //setup madflight components: Serial.begin(115200), imu, rcin, led, etc. See src/madflight/interface.h for full interface description of each component. 
   madflight_setup();  
 
-  //setup 4 motors for the quadcopter
-  for(int i=0;i<4;i++) {
-    //uncomment one line - sets pin, frequency (Hz), minimum (us), maximum (us)
-    out.setupMotor(i, HW_PIN_OUT[i], 400, 950, 2000); //Standard PWM: 400Hz, 950-2000 us
-    //out.setupMotor(i, (HW_PIN_OUT[i], 2000, 125, 250); //Oneshot125: 2000Hz, 125-250 us
-  }
+  // Uncomment ONE line - select output type
+  int out_hz = 400; int min_us = 950; int max_us = 2000; //Standard PWM: 400Hz, 950-2000 us
+  //int out_hz = 2000; int min_us = 125; int max_us = 250; //Oneshot125: 2000Hz, 125-250 us
+
+  // Setup 4 motors for the quadcopter
+  out.setupMotor(0, cfg.pin_out0, out_hz, min_us, max_us);
+  out.setupMotor(1, cfg.pin_out1, out_hz, min_us, max_us);
+  out.setupMotor(2, cfg.pin_out2, out_hz, min_us, max_us);
+  out.setupMotor(3, cfg.pin_out3, out_hz, min_us, max_us);
 
   //set initial desired yaw
-  yaw_desired = ahrs.yaw;
+  yaw_desired = ahr.yaw;
 }
 
 //========================================================================================================================//
 //                                                            LOOP()                                                      //
 //========================================================================================================================//
 
 void loop() {
-  cli.loop(); //process CLI commands
+  cli.update(); //process CLI commands
 }
 
 //========================================================================================================================//
@@ -108,17 +114,17 @@ void imu_loop() {
   //Blink LED
   led_Blink();
 
-  //Sensor fusion: update ahrs.roll, ahrs.pitch, and ahrs.yaw angle estimates (degrees) from IMU data
-  ahrs.update(); 
+  //Sensor fusion: update ahr.roll, ahr.pitch, and ahr.yaw angle estimates (degrees) from IMU data
+  ahr.update(); 
 
   //Get radio commands - Note: don't do this in loop() because loop() is a lower priority task than imu_loop(), so in worst case loop() will not get any processor time.
-  rcin.update();
+  rcl.update();
 
   //PID Controller RATE or ANGLE
   #ifdef FLIGHTMODE_ANGLE
-    control_Angle(rcin.throttle == 0); //Stabilize on pitch/roll angle setpoint, stabilize yaw on rate setpoint  //control_Angle2(rcin_thro_is_low); //Stabilize on pitch/roll setpoint using cascaded method. Rate controller must be tuned well first!
+    control_Angle(rcl.throttle == 0); //Stabilize on pitch/roll angle setpoint, stabilize yaw on rate setpoint  //control_Angle2(rcin_thro_is_low); //Stabilize on pitch/roll setpoint using cascaded method. Rate controller must be tuned well first!
   #else
-    control_Rate(rcin.throttle == 0); //Stabilize on rate setpoint
+    control_Rate(rcl.throttle == 0); //Stabilize on rate setpoint
   #endif
 
   //Updates out.arm, the output armed flag
@@ -133,13 +139,11 @@ void imu_loop() {
 //========================================================================================================================//
 
 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.
+  //Blink LED once per second, if LED blinks slower then the loop takes too much time, use CLI 'pimu' to investigate.
   //DISARMED: long off, short on, ARMED: long on, short off
-  if(imu.update_cnt % imu.getSampleRate() <= imu.getSampleRate() / 10) {
-    led.set(!out.armed); //short interval
-  }else{
-    led.set(out.armed); //long interval
-  }
+  uint32_t modulus = imu.update_cnt % imu.getSampleRate();
+  if( modulus == 0) led.set(!out.armed); //start of pulse
+  if( modulus == imu.getSampleRate() / 10)  led.set(out.armed); //end of pulse
 }
 
 //returns angle in range -180 to 180
@@ -156,7 +160,7 @@ void control_Angle(bool zero_integrators) {
   //DESCRIPTION: Computes control commands based on angle error
   /*
    * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des. Error
-   * is simply the desired state minus the actual state (ex. roll_des - ahrs.roll). Two safety features
+   * is simply the desired state minus the actual state (ex. roll_des - ahr.roll). Two safety features
    * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent 
    * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until
    * they've maxed out throttle... saturating I to a specified limit fixes this. The second feature defaults the I terms to 0
@@ -169,9 +173,9 @@ void control_Angle(bool zero_integrators) {
   //outputs: PIDroll.PID, PIDpitch.PID, PIDyaw.PID
 
   //desired values
-  float roll_des = rcin.roll * maxRoll; //Between -maxRoll and +maxRoll
-  float pitch_des = rcin.pitch * maxPitch; //Between -maxPitch and +maxPitch
-  float yawRate_des = rcin.yaw * maxYawRate; //Between -maxYawRate roll_PIDand +maxYawRate
+  float roll_des = rcl.roll * maxRoll; //Between -maxRoll and +maxRoll
+  float pitch_des = rcl.pitch * maxPitch; //Between -maxPitch and +maxPitch
+  float yawRate_des = rcl.yaw * maxYawRate; //Between -maxYawRate roll_PIDand +maxYawRate
 
   //state vars
   static float integral_roll, integral_pitch, error_yawRate_prev, integral_yawRate;
@@ -184,35 +188,35 @@ void control_Angle(bool zero_integrators) {
   }
 
   //Roll PID
-  float error_roll = roll_des - ahrs.roll;
+  float error_roll = roll_des - ahr.roll;
   integral_roll += error_roll * imu.dt;
   integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
-  float derivative_roll = ahrs.gx;
+  float derivative_roll = ahr.gx;
   PIDroll.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;
+  float error_pitch = pitch_des - ahr.pitch;
   integral_pitch += error_pitch * imu.dt;
   integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
-  float derivative_pitch = ahrs.gy; 
+  float derivative_pitch = ahr.gy; 
   PIDpitch.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
-  if(-0.02 < rcin.yaw && rcin.yaw < 0.02) {
+  if(-0.02 < rcl.yaw && rcl.yaw < 0.02) {
     //on reset, set desired yaw to current yaw
-    if(zero_integrators) yaw_desired = ahrs.yaw; 
+    if(zero_integrators) yaw_desired = ahr.yaw; 
 
     //Yaw stick centered: hold yaw_desired
-    float error_yaw = degreeModulus(yaw_desired - ahrs.yaw);
+    float error_yaw = degreeModulus(yaw_desired - ahr.yaw);
     float desired_yawRate = error_yaw / 0.5; //set desired yawRate such that it gets us to desired yaw in 0.5 second
-    float derivative_yaw = desired_yawRate - ahrs.gz;
+    float derivative_yaw = desired_yawRate - ahr.gz;
     PIDyaw.PID = 0.01 * (Kp_yaw_angle*error_yaw + Kd_yaw_angle*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range
 
     //update yaw rate controller
     error_yawRate_prev = 0;
   }else{
     //Yaw stick not centered: stablize on rate from GyroZ
-    float error_yawRate = yawRate_des - ahrs.gz;
+    float error_yawRate = yawRate_des - ahr.gz;
     integral_yawRate += error_yawRate * imu.dt;
     integral_yawRate = constrain(integral_yawRate, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
     float derivative_yawRate = (error_yawRate - error_yawRate_prev) / imu.dt; 
@@ -222,7 +226,7 @@ void control_Angle(bool zero_integrators) {
     error_yawRate_prev = error_yawRate;
 
     //update yaw controller: 
-    yaw_desired = ahrs.yaw; //set desired yaw to current yaw, the yaw angle controller will hold this value
+    yaw_desired = ahr.yaw; //set desired yaw to current yaw, the yaw angle controller will hold this value
   }
 }
 
@@ -234,9 +238,9 @@ void control_Rate(bool zero_integrators) {
   //outputs: PIDroll.PID, PIDpitch.PID, PIDyaw.PID
 
   //desired values
-  float rollRate_des = rcin.roll * maxRollRate; //Between -maxRoll and +maxRoll
-  float pitchRate_des = rcin.pitch * maxPitchRate; //Between -maxPitch and +maxPitch
-  float yawRate_des = rcin.yaw * maxYawRate; //Between -maxYawRate and +maxYawRate 
+  float rollRate_des = rcl.roll * maxRollRate; //Between -maxRoll and +maxRoll
+  float pitchRate_des = rcl.pitch * maxPitchRate; //Between -maxPitch and +maxPitch
+  float yawRate_des = rcl.yaw * maxYawRate; //Between -maxYawRate and +maxYawRate 
 
   //state vars
   static float integral_roll, error_roll_prev;
@@ -251,21 +255,21 @@ void control_Rate(bool zero_integrators) {
   }
 
   //Roll
-  float error_roll = rollRate_des - ahrs.gx;
+  float error_roll = rollRate_des - ahr.gx;
   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) / imu.dt;
   PIDroll.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 - ahrs.gy;
+  float error_pitch = pitchRate_des - ahr.gy;
   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) / imu.dt;   
   PIDpitch.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 - ahrs.gz;
+  float error_yaw = yawRate_des - ahr.gz;
   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) / imu.dt; 
@@ -281,25 +285,22 @@ void out_KillSwitchAndFailsafe() {
   static bool rcin_arm_prev = true; //initial value is true: forces out.armed false on startup even if arm switch is ON
 
   //Change to ARMED when throttle is zero and radio armed switch was flipped from disamed to armed position
-  if (!out.armed && rcin.throttle == 0 && rcin.arm && !rcin_arm_prev) {
+  if (!out.armed && rcl.throttle == 0 && rcl.arm && !rcin_arm_prev) {
     out.armed = true;
     Serial.println("OUT: ARMED");
-    bb.start(); //start blackbox logging
   }
 
   //Change to DISARMED when radio armed switch is in disarmed position, or if radio lost connection
-   if (out.armed && (!rcin.arm || !rcin.connected())) {
+   if (out.armed && (!rcl.arm || !rcl.connected())) {
     out.armed = false;
-    if(!rcin.arm) {
+    if(!rcl.arm) {
       Serial.println("OUT: DISARMED");
-      bb.stop(); //stop blackbox logging
     }else{
       Serial.println("OUT: DISARMED due to lost radio connection");
-      //keep on logging to document the crash...
     }
   }
 
-  rcin_arm_prev = rcin.arm;
+  rcin_arm_prev = rcl.arm;
 }
 
 void out_Mixer() {
@@ -308,13 +309,13 @@ void out_Mixer() {
    * Takes PIDroll.PID, PIDpitch.PID, and PIDyaw.PID computed from the PID controller and appropriately mixes them for the desired
    * vehicle configuration. For example on a quadcopter, the left two motors should have +PIDroll.PID while the right two motors
    * should have -PIDroll.PID. Front two should have +PIDpitch.PID and the back two should have -PIDpitch.PID etc... every motor has
-   * normalized (0 to 1) rcin.throttle command for throttle control. Can also apply direct unstabilized commands from the transmitter with 
-   * rcin.xxx variables are to be sent to the motor ESCs and servos.
+   * normalized (0 to 1) rcl.throttle command for throttle control. Can also apply direct unstabilized commands from the transmitter with 
+   * rcl.xxx variables are to be sent to the motor ESCs and servos.
    * 
    *Relevant variables:
-   *rcin.throtle - direct thottle control
+   *rcl.throtle - direct thottle control
    *PIDroll.PID, PIDpitch.PID, PIDyaw.PID - stabilized axis variables
-   *rcin.roll, rcin.pitch, rcin.yaw - direct unstabilized command passthrough
+   *rcl.roll, rcl.pitch, rcl.yaw - direct unstabilized command passthrough
    */
 /*
 Motor order diagram (Betaflight order)
@@ -336,7 +337,7 @@ Yaw right               (CCW+ CW-)       -++-
 
   // IMPORTANT: This is a safety feature to remind the pilot to disarm.
   // Set throttle to at least armed_min_throttle, to keep at least one prop spinning when armed. The [out] module will disable motors when out.armed == false
-  float thr = armed_min_throttle + (1 - armed_min_throttle) * rcin.throttle; //shift throttle range from [0.0 .. 1.0] to [armed_min_throttle .. 1.0]
+  float thr = armed_min_throttle + (1 - armed_min_throttle) * rcl.throttle; //shift throttle range from [0.0 .. 1.0] to [armed_min_throttle .. 1.0]
 
   //Quad mixing
   out.set(0, thr - PIDpitch.PID - PIDroll.PID - PIDyaw.PID); //M1 Back Right CW