Skip to content

Commit

Permalink
update examples
Browse files Browse the repository at this point in the history
  • Loading branch information
@qqqlab
qqqlab committed Aug 20, 2024
1 parent 1455491 commit f36d430
Show file tree
Hide file tree
Showing 3 changed files with 72 additions and 30 deletions.
3 changes: 2 additions & 1 deletion examples/01.Quadcopter/01.Quadcopter.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,10 +203,11 @@ void setup() {

//6 second startup delay
for(int i=20;i>0;i--) {
Serial.printf(MADFLIGHT_VERSION " on " HW_ARDUINO_STR " starting %d ...\n",i);
Serial.printf("Quad " MADFLIGHT_VERSION " starting %d ...\n", i);
delay(300);
led.toggle();
}
Serial.printf("Arduino library: " HW_ARDUINO_STR "\n");
led.on();

hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h)
Expand Down
3 changes: 1 addition & 2 deletions examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -248,9 +248,8 @@ void setup() {
Serial.begin(115200); //start console serial

//6 second startup delay
String ino = String(__FILE__).substring(ino.lastIndexOf("\\")+1);
for(int i=20;i>0;i--) {
Serial.printf("%s " MADFLIGHT_VERSION " starting %d ...\n", ino.c_str(), i);
Serial.printf("QuadAdv " MADFLIGHT_VERSION " starting %d ...\n", i);
delay(300);
led.toggle();
}
Expand Down
96 changes: 69 additions & 27 deletions examples/04.Plane/04.Plane.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,16 @@
WARNING: This program is highly experimental - not flight tested at all - it was only dry run tested!
This is just a quick first attempt to make a plane controller, it has two flight modes: MANUAL and FBWA.
This is just a quick first attempt to make a plane controller, it has 3 flight modes: MANUAL, ROLL and FBWA.
## MANUAL Mode
Regular RC control, no stabilization. All RC inputs are passed through to the servo outputs.
## ROLL Mode
Stabilize roll angle
## FBWA Fly By Wire A Mode (inspired by ArduPilot)
This is the most popular mode for assisted flying, and is the best mode for inexperienced flyers. In this mode the
Expand All @@ -25,7 +29,11 @@ In FBWA mode the rudder is under manual control.
First edit sections "PINS", "BOARD", "HARDWARE", "RC RECEIVER", "OUTPUTS". Use CLI to verify things work as expected.
Then do a dry run:
Calibrate gyro, accelerometer, and magnetometer --- important!!!
Connect power and then let plane sit for 15 seconds, during this time the gyro biases are re-calibrated.
Do a dry run:
Set to MANUAL and power up the plane. Move the rc controls and make sure that the aileron, elevator, and rudder move in
the correct direction. Arm the plane, and carefully test the motor, then disarm.
Expand Down Expand Up @@ -62,10 +70,10 @@ Copyright (c) 2024 https://github.com/qqqlab/madflight
//========================================================================================================================//
// PINS //
//========================================================================================================================//
// Default pinouts are defined in the board header files library/src/madflight_board_default_XXX.h
// Uncomment the defines below to create your own pinout, leave commented if you want to use the default pinout.

/*
/* Place an additional / at the beginning of this line to setup our own pinout below, otherwise default pins from library/src/madflight_board_default_XXX.h are used
//Example pinout for LOLIN S3 (ESP32S3) with SPI MPU-9250 module directly soldered
//LED:
#define HW_PIN_LED -1
Expand All @@ -79,8 +87,8 @@ Copyright (c) 2024 https://github.com/qqqlab/madflight
#define HW_PIN_IMU_EXTI 16 //external interrupt pin
//I2C for BARO, MAG, BAT sensors and for IMU if not using SPI
#define HW_PIN_I2C_SDA 13
#define HW_PIN_I2C_SCL 14
#define HW_PIN_I2C_SDA -1
#define HW_PIN_I2C_SCL -1
//Motor/Servo Outputs:
#define HW_OUT_COUNT 6 //number of outputs
Expand Down Expand Up @@ -213,10 +221,9 @@ const int out_MOTOR_COUNT = 1;
//========================================================================================================================//

//flight modes
enum rcin_fm_enum {MANUAL, FBWA}; //available flight modes: MANUAL send rc commands directly to motor and aileron/pitch/yaw servos, FBWA stabilize roll/pitch angles
const char* rcin_fm_str[] = {"MANUAL","FBWA"};
rcin_fm_enum rcin_fm_map[6] {MANUAL, MANUAL, MANUAL, FBWA, FBWA, FBWA}; //flightmode mapping from 6 pos switch to flight mode (simulates a 2-pos switch: MANUAL/FBWA)
rcin_fm_enum rcin_fm = (rcin_fm_enum)0; //current flight mode
enum rcin_fm_enum {MANUAL, ROLL, FBWA}; //available flight modes: MANUAL send rc commands directly to motor and aileron/pitch/yaw servos, ROLL stabilize roll angle, FBWA stabilize roll/pitch angles
const char* rcin_fm_str[] = {"MANUAL", "ROLL", "FBWA"}; //flight mode names used for telemetry
rcin_fm_enum rcin_fm_map[6] {MANUAL, MANUAL, ROLL, ROLL, FBWA, FBWA}; //flightmode mapping from 6 pos switch to flight mode (simulates a 3-pos switch: MANUAL/ROLL/FBWA)

const uint32_t imu_sample_rate = 1000; //imu sample rate in Hz (default 1000) NOTE: not all IMU drivers support a different rate
const uint32_t baro_sample_rate = 100; //baro sample rate in Hz (default 100)
Expand All @@ -228,18 +235,20 @@ float LP_mag = 1e10; //Magnetometer (default 1e10Hz, i.e. no filt
float LP_radio = 400; //Radio Input (default 400Hz)

//Controller parameters (take note of defaults before modifying!):
float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0)
float maxRoll = 45.0; //Max roll angle in degrees for FBWA mode
float maxPitch = 30.0; //Max pitch angle in degrees for FBWA mode
float i_limit = 25; //PID Integrator saturation level, mostly for safety
float maxRoll = 45; //Max roll angle in degrees for ROLL, FBWA modes
float maxPitch = 30; //Max pitch angle in degrees for FBWA mode
float fbwa_pitch_offset = 3; //FBWA pitch up angle for neutral stick

//Kp scaling: full control surface deflection on 100/Kp degree error
float Kp_roll = 1.1; //Roll P-gain
float Ki_roll = 0; //Roll I-gain
float Kd_roll = 0; //Roll D-gain
//roll PID constants
float Kp_roll = (1.0/90); //Roll P-gain - apply full aileron on 90 degree roll error
float Ki_roll = 0; //Roll I-gain
float Kd_roll = (1.0/180); //Roll D-gain - apply full opposite aileron when roll rate is 180 degrees/sec towards desired setpoint

float Kp_pitch = 3.3; //Pitch P-gain
float Ki_pitch = 0; //Pitch I-gain
float Kd_pitch = 0; //Pitch D-gain
//pitch PID constants
float Kp_pitch = (1.0/30); //Pitch P-gain - apply full elevator on 30 degree pitch error
float Ki_pitch = 0; //Pitch I-gain
float Kd_pitch = (1.0/90); //Pitch D-gain - apply full opposite elevator when pitch rate is 90 degrees/sec towards desired setpoint

//float Kp_yaw = 0.3; //Yaw P-gain
//float Ki_yaw = 0.05; //Yaw I-gain
Expand All @@ -262,6 +271,7 @@ float rcin_flaps; //flaps 0.0:up, 1.0:full down
float roll_PID = 0, pitch_PID = 0, yaw_PID = 0;

//Flight status
rcin_fm_enum rcin_fm = (rcin_fm_enum)0; //current flight mode
bool out_armed = false; //motors will only run if this flag is true

//Low pass filter parameters
Expand All @@ -282,9 +292,8 @@ void setup() {
Serial.begin(115200); //start console serial

//6 second startup delay
String ino = String(__FILE__).substring(ino.lastIndexOf("\\")+1);
for(int i=20;i>0;i--) {
Serial.printf("%s " MADFLIGHT_VERSION " starting %d ...\n", ino.c_str(), i);
Serial.printf("Plane " MADFLIGHT_VERSION " starting %d ...\n", i);
delay(300);
led.toggle();
}
Expand Down Expand Up @@ -401,6 +410,9 @@ void imu_loop() {

//PID Controller
switch(rcin_fm) {
case ROLL:
control_ROLL(rcin_thro_is_low); //Stabilize on roll angle setpoints
break;
case FBWA:
control_FBWA(rcin_thro_is_low); //Stabilize on pitch/roll angle setpoints
break;
Expand Down Expand Up @@ -517,7 +529,7 @@ In FBWA mode the rudder is under manual control.

//desired values
float roll_des = rcin_roll * maxRoll; //Between -maxRoll and +maxRoll
float pitch_des = rcin_pitch * maxPitch; //Between -maxPitch and +maxPitch
float pitch_des = rcin_pitch * maxPitch + fbwa_pitch_offset; //Between fbwa_pitch_offset-maxPitch and fbwa_pitch_offset+maxPitch

//state vars
static float integral_roll, integral_pitch, error_yaw_prev, integral_yaw;
Expand All @@ -534,14 +546,14 @@ In FBWA mode the rudder is under manual control.
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;
roll_PID = constrain(0.01 * (Kp_roll*error_roll + Ki_roll*integral_roll - Kd_roll*derivative_roll), -1.0f, 1.0f); //Scaled by .01 to bring within -1 to 1 range
roll_PID = Kp_roll*error_roll + Ki_roll*integral_roll - Kd_roll*derivative_roll; //nominal output -1 to 1 (can be larger)

//Pitch PID - stabilize desired pitch angle
float error_pitch = pitch_des - ahrs.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;
pitch_PID = constrain(0.01 * (Kp_pitch*error_pitch + Ki_pitch*integral_pitch - Kd_pitch*derivative_pitch), -1.0f, 1.0f); //Scaled by .01 to bring within -1 to 1 range
pitch_PID = Kp_pitch*error_pitch + Ki_pitch*integral_pitch - Kd_pitch*derivative_pitch; //nominal output -1 to 1 (can be larger)

//Yaw PID - passthru rcin
yaw_PID = rcin_yaw;
Expand All @@ -561,6 +573,36 @@ In FBWA mode the rudder is under manual control.
*/
}

void control_ROLL(bool zero_integrators) {
//inputs: roll_des, pitch_des, yawRate_des
//outputs: roll_PID, pitch_PID, yaw_PID

//desired values
float roll_des = rcin_roll * maxRoll; //Between -maxRoll and +maxRoll

//state vars
static float integral_roll;

//Zero the integrators (used to don't let integrator build if throttle is too low, or to re-start the controller)
if(zero_integrators) {
integral_roll = 0;
}

//Roll PID - stabilize desired roll angle
float error_roll = roll_des - ahrs.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;
roll_PID = Kp_roll*error_roll + Ki_roll*integral_roll - Kd_roll*derivative_roll; //nominal output -1 to 1 (can be larger)

//Pitch PID - passthru rcin
pitch_PID = rcin_pitch;

//Yaw PID - passthru rcin
yaw_PID = rcin_yaw;
}


void control_MANUAL() {
/* MANUAL Mode
Regular RC control, no stabilization. All RC inputs are passed through to the servo outputs.
Expand Down Expand Up @@ -592,7 +634,7 @@ void control_Mixer() {
yaw_PID -1: yaw left/stick left 1: yaw right/stick right
*/

//Plane mixing - PID values are -1 to +1, SERVO values are 0 to 1
//Plane mixing - PID values are -1 to +1 (nominal), SERVO values are 0 to 1 (clipped by pwm class)

//motor: full throttle on rcin_thro
#ifdef OUT_MOTOR1 //full throttle on high pwm
Expand Down

0 comments on commit f36d430

Please sign in to comment.