Skip to content

Commit

Permalink
altitude control WIP !!
Browse files Browse the repository at this point in the history
  • Loading branch information
@DanieleVeri
DanieleVeri committed Jan 8, 2024
1 parent b278925 commit f8e026f
Show file tree
Hide file tree
Showing 10 changed files with 163 additions and 101 deletions.
7 changes: 6 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
# Arduino quadcopter
![img](frame/video.gif)
A simple quadcopter filght controller designed to work in angle control mode ([diagram](https://docs.px4.io/v1.12/assets/img/mc_control_arch.21116ef0.jpg)).
All parameters are grouped in `config.h`.
Switch `OPERATING_MODE` to perform ESC calibration and IMU calibration.
Expand Down Expand Up @@ -29,4 +30,8 @@ Arduino libraries:
- 1x cover.stl

![img](frame/top.jpg)
![img](frame/bottom.jpg)
![img](frame/bottom.jpg)

## TODO:
- test altitude control
- include reversed imu in rotation
52 changes: 28 additions & 24 deletions config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,25 +6,27 @@
#define MODE_FLIGHT 0
#define MODE_CALIB_MOTORS 1
#define MODE_CALIB_IMU 2
#define OPERATING_MODE MODE_FLIGHT // <-- set the operating mode
#define DEBUG 0 // <-- disable motors and print debug messages
#define OPERATING_MODE MODE_FLIGHT // <-- set the operating mode
#define DEBUG 0 // <-- disable motors and print debug messages

// RC bounds
#define RC_PITCH_BOUND 30 // [-30, 30] deg
#define RC_ROLL_BOUND 30 // [-30, 30] deg
#define RC_YAW_BOUND 45 // [-30, 30] deg/s
#define RC_MIN_THROTTLE 1000 // pwm
#define RC_MAX_THROTTLE 2000 // pwm

// Smooth fall
#define SMOOTH_FALL_DIST 10 // cm
#define SMOOTH_FALL_PWM 1350 // pwm
#define RANGEFINDER_MAX_DIST 400 // cm
#define RC_PITCH_BOUND 30 // [-30, 30] deg
#define RC_ROLL_BOUND 30 // [-30, 30] deg
#define RC_YAW_BOUND 90 // [-90, 90] deg/s
#define RC_MIN_THROTTLE 1000 // pwm
#define RC_MAX_THROTTLE 2000 // pwm

// Altitude control
#define RANGEFINDER_MAX_DIST 400 // cm
#define GROUND_DIST_FLYING_THRESHOLD 10 // cm
#define SMOOTH_FALL_PWM 1350 // pwm
#define START_ALTITUDE_CONTROL_THROTTLE 1400 // pwm
#define STOP_ALTITUDE_CONTROL_THROTTLE 1600 // pwm

// Sensor bias
#define IMU_TO_FRAME_ROLL -6.0 // deg
#define IMU_TO_FRAME_PITCH 6.0 // deg
#define IMU_TO_FRAME_YAW -6.0 * DEG_TO_RAD // rad
#define IMU_TO_FRAME_YAW -6.0 // deg
#define ROLL_RATE_BIAS 1.04 // deg/s
#define PITCH_RATE_BIAS 0.7 // deg/s
#define YAW_RATE_BIAS -1.5 // deg/s
Expand All @@ -50,6 +52,8 @@
#define YAW_RATE_PID_KI 0
#define YAW_RATE_PID_KD 0

#define ALTITUDE_PID_KP 200

#define OUT_MINMAX 300
#define BANGBANG 300
#define TIMESTEP 10
Expand All @@ -58,21 +62,21 @@
#define LPFD 0.2

// Motor pins
#define PIN_MOTOR_0 6
#define PIN_MOTOR_1 3
#define PIN_MOTOR_2 4
#define PIN_MOTOR_3 5
#define PIN_MOTOR_0 6 // pwm pin
#define PIN_MOTOR_1 3 // pwm pin
#define PIN_MOTOR_2 4 // pwm pin
#define PIN_MOTOR_3 5 // pwm pin

// Radio pins
#define RX_PIN_ROLL 19
#define RX_PIN_PITCH 18
#define RX_PIN_YAW 2
#define RX_PIN_THROTTLE 9
#define RX_PIN_AUX1 10
#define RX_PIN_AUX2 11
#define RX_PIN_ROLL 19 // interrupt pin
#define RX_PIN_PITCH 18 // interrupt pin
#define RX_PIN_YAW 2 // interrupt pin
#define RX_PIN_THROTTLE 9 // pwm pin
#define RX_PIN_AUX1 10 // pwm pin (unused)
#define RX_PIN_AUX2 11 // pwm pin (unused)

// Rangefinder pins
#define TRIGGER_PIN 45
#define TRIGGER_PIN 45 // pwm pin
#define ECHO_PIN 37

#endif
4 changes: 2 additions & 2 deletions firmware.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,14 +8,12 @@

void setup() {
Serial.begin(115200);

Serial.println("\n Starting firmware setup...");

#if OPERATING_MODE == MODE_FLIGHT
setup_motors();
arm_motors();
setup_imu();
//calib_imu();
register_radio_interrupt();
setup_rangefinder();
setup_pid();
Expand All @@ -36,5 +34,7 @@ void setup() {
void loop() {
#if OPERATING_MODE == MODE_FLIGHT
control_loop();
#elif OPERATING_MODE == MODE_CALIB_IMU
debug_imu();
#endif
}
123 changes: 80 additions & 43 deletions flight_control.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
#include "Servo.h"
#include "Arduino.h"
#include "AutoPID.h"
#include "flight_control.h"
Expand All @@ -10,17 +9,30 @@

// State variables
static Linear acc;
static Asset angles;
static Asset rates;
static Asset filtered_rates;
static float ground_dist = 0;
static int throttle = 0;
static Asset angles_setpoint; // --> radio_input
static Asset rates_setpoint; // --> angles_output
static Asset rates_output;
static Attitude angles;
static Attitude rates;
static Attitude filtered_rates;
static double ground_dist = 0;

// RC input
static Attitude angles_setpoint;
static int rc_throttle = 0;

// Control values
static Attitude rates_setpoint; // --> angles_output
static Attitude rates_output;
static const double acc_setpoint = 1;
static double acc_correction = 0;

// Outputs
static int output_throttle = 0;
static int mixer_output[4] = { 0, 0, 0, 0 };

// PID controllers
static AutoPID PID_altitude_control(&acc.z, &acc_setpoint, &acc_correction,
-OUT_MINMAX, OUT_MINMAX,
ALTITUDE_PID_KP, 0, 0);

static AutoPID PID_roll_angle(&angles.roll, &angles_setpoint.roll, &rates_setpoint.roll,
-OUT_MINMAX, OUT_MINMAX,
ROLL_PID_KP, ROLL_PID_KI, ROLL_PID_KD);
Expand All @@ -41,23 +53,9 @@ static AutoPID PID_yaw_rate(&filtered_rates.yaw, &rates_setpoint.yaw, &rates_out
-OUT_MINMAX, OUT_MINMAX,
YAW_RATE_PID_KP, YAW_RATE_PID_KI, YAW_RATE_PID_KD);

void setup_pid() {
PID_roll_angle.setTimeStep(TIMESTEP);
PID_roll_angle.setBangBang(BANGBANG);
PID_pitch_angle.setTimeStep(TIMESTEP);
PID_pitch_angle.setBangBang(BANGBANG);

PID_roll_rate.setTimeStep(TIMESTEP);
PID_roll_rate.setBangBang(BANGBANG);
PID_pitch_rate.setTimeStep(TIMESTEP);
PID_pitch_rate.setBangBang(BANGBANG);
PID_yaw_rate.setTimeStep(TIMESTEP);
PID_yaw_rate.setBangBang(BANGBANG);
}

void get_radio_input() {
sample_throttle();
throttle = map(get_rx_throttle(), MIN_PULSE_LENGTH, MAX_PULSE_LENGTH, RC_MIN_THROTTLE, RC_MAX_THROTTLE);
rc_throttle = map(get_rx_throttle(), MIN_PULSE_LENGTH, MAX_PULSE_LENGTH, RC_MIN_THROTTLE, RC_MAX_THROTTLE);
angles_setpoint.roll = map(get_rx_roll(), MIN_PULSE_LENGTH, MAX_PULSE_LENGTH, -RC_ROLL_BOUND, RC_ROLL_BOUND);
angles_setpoint.pitch = map(get_rx_pitch(), MIN_PULSE_LENGTH, MAX_PULSE_LENGTH, RC_PITCH_BOUND, -RC_PITCH_BOUND);
rates_setpoint.yaw = map(get_rx_yaw(), MIN_PULSE_LENGTH, MAX_PULSE_LENGTH, -RC_YAW_BOUND, RC_YAW_BOUND);
Expand All @@ -76,23 +74,38 @@ void control_loop() {
get_linear_acc(acc, angles);
get_rates(rates);
low_pass_filter(rates, filtered_rates);
get_range(ground_dist);
get_range(ground_dist, angles);

// Compute PID
PID_altitude_control.run();
PID_roll_angle.run();
PID_pitch_angle.run();
PID_roll_rate.run();
PID_pitch_rate.run();
PID_yaw_rate.run();

// Altitude control
const bool is_flying = (ground_dist > GROUND_DIST_FLYING_THRESHOLD && ground_dist < RANGEFINDER_MAX_DIST);
const bool maintain_altitude = (rc_throttle > START_ALTITUDE_CONTROL_THROTTLE && rc_throttle < STOP_ALTITUDE_CONTROL_THROTTLE);
if (maintain_altitude && is_flying) {
output_throttle = rc_throttle - acc_correction;
} else {
output_throttle = rc_throttle;
}

// Prevent free fall from more than 10cm
if (ground_dist > SMOOTH_FALL_DIST && ground_dist < RANGEFINDER_MAX_DIST)
throttle = max(throttle, SMOOTH_FALL_PWM);
// Thrust correction by attitude
throttle = MIN_PULSE_LENGTH + ((float)(throttle - MIN_PULSE_LENGTH)) / (cos(angles.roll * DEG_TO_RAD) * cos(angles.pitch * DEG_TO_RAD));
if (is_flying)
output_throttle = max(rc_throttle, SMOOTH_FALL_PWM);

// Thrust correction depending on attitude
output_throttle = MIN_PULSE_LENGTH + ((float)(output_throttle - MIN_PULSE_LENGTH)) / (cos(angles.roll * DEG_TO_RAD) * cos(angles.pitch * DEG_TO_RAD));

// Mixer
mixer_output[0] = throttle + (rates_output.roll) + (rates_output.pitch) + rates_output.yaw;
mixer_output[1] = throttle - (rates_output.roll) + (rates_output.pitch) - rates_output.yaw;
mixer_output[2] = throttle + (rates_output.roll) - (rates_output.pitch) - rates_output.yaw;
mixer_output[3] = throttle - (rates_output.roll) - (rates_output.pitch) + rates_output.yaw;
mixer_output[0] = output_throttle + (rates_output.roll) + (rates_output.pitch) + rates_output.yaw;
mixer_output[1] = output_throttle - (rates_output.roll) + (rates_output.pitch) - rates_output.yaw;
mixer_output[2] = output_throttle + (rates_output.roll) - (rates_output.pitch) - rates_output.yaw;
mixer_output[3] = output_throttle - (rates_output.roll) - (rates_output.pitch) + rates_output.yaw;

} else {
// Reset state
acc.reset();
Expand All @@ -102,12 +115,15 @@ void control_loop() {
angles_setpoint.reset();
rates_setpoint.reset();
angles_setpoint.reset();

// Reset PID
PID_altitude_control.stop();
PID_roll_angle.stop();
PID_pitch_angle.stop();
PID_roll_rate.stop();
PID_pitch_rate.stop();
PID_yaw_rate.stop();

// Zero the mixer output
memset(mixer_output, 0, sizeof(int) * 4);
}
Expand All @@ -122,6 +138,23 @@ void control_loop() {
#endif
}

void setup_pid() {
PID_altitude_control.setTimeStep(TIMESTEP);
PID_altitude_control.setBangBang(BANGBANG);

PID_roll_angle.setTimeStep(TIMESTEP);
PID_roll_angle.setBangBang(BANGBANG);
PID_pitch_angle.setTimeStep(TIMESTEP);
PID_pitch_angle.setBangBang(BANGBANG);

PID_roll_rate.setTimeStep(TIMESTEP);
PID_roll_rate.setBangBang(BANGBANG);
PID_pitch_rate.setTimeStep(TIMESTEP);
PID_pitch_rate.setBangBang(BANGBANG);
PID_yaw_rate.setTimeStep(TIMESTEP);
PID_yaw_rate.setBangBang(BANGBANG);
}

void debug_state() {
static uint32_t prev_ms = 0;
static uint32_t ticks = 0;
Expand All @@ -137,14 +170,18 @@ void debug_state() {
acc.print();
Serial.print("Ground distance (cm): ");
Serial.println(ground_dist);
Serial.print("Acc z correction (cm): ");
Serial.println(acc_correction);
Serial.println("Angles (deg)");
angles.print();
Serial.println("Rates (deg/s)");
rates.print();
Serial.println("Filtered rates (deg/s)");
filtered_rates.print();
Serial.print("Throttle (pwm): ");
Serial.println(throttle);
Serial.print("Throttle RC (pwm): ");
Serial.println(rc_throttle);
Serial.print("Throttle output (pwm): ");
Serial.println(output_throttle);
Serial.println("Angles setpoints (deg)");
angles_setpoint.print();
Serial.println("Rates setpoints (deg/s)");
Expand All @@ -159,12 +196,12 @@ void debug_state() {
Serial.print("\n");
}

void low_pass_filter(const Asset& new_asset, Asset& filtered_asset) {
static Asset cum;
filtered_asset.roll = (1 - LPFD) * new_asset.roll + LPFD * cum.roll;
filtered_asset.pitch = (1 - LPFD) * new_asset.pitch + LPFD * cum.pitch;
filtered_asset.yaw = (1 - LPFD) * new_asset.yaw + LPFD * cum.yaw;
cum.roll = filtered_asset.roll;
cum.pitch = filtered_asset.pitch;
cum.yaw = filtered_asset.yaw;
void low_pass_filter(const Attitude& new_attitude, Attitude& filtered_attitude) {
static Attitude cum;
filtered_attitude.roll = (1 - LPFD) * new_attitude.roll + LPFD * cum.roll;
filtered_attitude.pitch = (1 - LPFD) * new_attitude.pitch + LPFD * cum.pitch;
filtered_attitude.yaw = (1 - LPFD) * new_attitude.yaw + LPFD * cum.yaw;
cum.roll = filtered_attitude.roll;
cum.pitch = filtered_attitude.pitch;
cum.yaw = filtered_attitude.yaw;
}
2 changes: 1 addition & 1 deletion flight_control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,6 @@ void control_loop();

void debug_state();

void low_pass_filter(const Asset& new_asset, Asset& filtered_asset);
void low_pass_filter(const Attitude& new_attitude, Attitude& filtered_attitude);

#endif
Binary file added frame/video.gif
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading

0 comments on commit f8e026f

Please sign in to comment.