Quadrotor from scratch
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

186 lines
4.0 KiB

/* motor.c */
#include "stick.h"
#include "thrust.h"
#include "dcm.h"
#include "uart.h"
#include "status.h"
#include "log.h"
#include "config.h"
float integral[3] = {0.0f, 0.0f, 0.0f};
float last[3];
float throttle = 0.0f;
#if 0
#define Kp 0.2
#define Ki 0.04
#define Kd 0.08
#define Ka 0.0
#define Kp_y 0.2
#define Ki_y 0.00
#define Kd_y 0.00
#define Ka_y 0.0
#else
#define Kp config.pid.rollpitch.p
#define Ki config.pid.rollpitch.i
#define Kd config.pid.rollpitch.d
#define Ka config.pid.rollpitch.a
#define Kp_y config.pid.yaw.p
#define Ki_y config.pid.yaw.i
#define Kd_y config.pid.yaw.d
#define Ka_y config.pid.yaw.a
#endif
/*
* Perform a PID loop iteration.
* roll and pitch are absolute values
* yaw is, currently, a rate.
* For this function only, coordinate convention is:
* x = roll
* y = pitch
* z = yaw
*/
void motor_pid_update(vec3f target, vec3f measured)
{
float derivative[3];
float out[3];
float motor[4];
float roll, pitch, yaw;
float error, max_error;
float min_motor;
int i;
roll = target.x - measured.x;
pitch = target.y - measured.y;
yaw = target.z - measured.z;
#if 0
if ((stick_counter % 100) == 0) {
putstr("{");
putint_s((int)(target.z * 10000));
putstr(", ");
putint_s((int)(measured.z * 10000));
putstr("}\r\n");
}
#endif
integral[0] += roll * delta_t;
integral[1] += pitch * delta_t;
integral[2] += yaw * delta_t;
#define INTEGRAL_LIMIT 1.0f
for (i = 0; i < 3; i++) {
if (integral[i] > INTEGRAL_LIMIT)
integral[i] = INTEGRAL_LIMIT;
if (integral[i] < -INTEGRAL_LIMIT)
integral[i] = -INTEGRAL_LIMIT;
}
/* The measurements are the opposite sign to the error */
derivative[0] = (-measured.x - last[0]) / delta_t;
derivative[1] = (-measured.y - last[1]) / delta_t;
derivative[2] = (-measured.z - last[2]) / delta_t;
last[0] = -measured.x;
last[1] = -measured.y;
last[2] = -measured.z;
out[0] = roll * Kp + integral[0] * Ki + derivative[0] * Kd;
out[1] = pitch * Kp + integral[1] * Ki + derivative[1] * Kd;
out[2] = yaw * Kp_y + integral[2] * Ki_y + derivative[2] * Kd_y;
if (status_armed()) {
/* Front right */
motor[0] = throttle + out[0] + out[1] + out[2];
/* Front left */
motor[1] = throttle - out[0] + out[1] - out[2];
/* Rear left */
motor[2] = throttle - out[0] - out[1] + out[2];
/* Rear right */
motor[3] = throttle + out[0] - out[1] - out[2];
} else {
motor[0] = 0.0;
motor[1] = 0.0;
motor[2] = 0.0;
motor[3] = 0.0;
}
max_error = 0.0;
min_motor = 1.0;
for (i = 0; i < 3; i++) {
if (motor[i] < 0.0)
motor[i] = 0.0;
if (motor[i] > 1.0f) {
error = motor[i] - 1.0f;
if (error > max_error)
max_error = error;
}
if (motor[i] < min_motor)
min_motor = motor[i];
}
if (max_error > 0.0) {
for (i = 0; i < 3; i++) {
motor[i] -= max_error;
if (motor[i] < 0.0)
motor[i] = 0.0;
}
}
if (throttle <= 0.0) {
motor[0] = 0.0;
motor[1] = 0.0;
motor[2] = 0.0;
motor[3] = 0.0;
integral[0] = 0.0;
integral[1] = 0.0;
integral[2] = 0.0;
}
if (max_error < min_motor) {
float new_throttle2, new_out[3];
new_throttle2 = (motor[0] + motor[1] + motor[2] + motor[3])/2.0;
new_out[0] = (motor[0] + motor[3] - new_throttle2)/2.0;
new_out[1] = (motor[0] + motor[1] - new_throttle2)/2.0;
new_out[2] = (motor[0] + motor[2] - new_throttle2)/2.0;
/* Anti-windup */
for (i = 0; i < 3; i++) {
if (new_out[i] > 1.0)
integral[i] -= (new_out[i]-1.0) * Ka;
if (new_out[i] < 0.0)
integral[i] -= (new_out[i]) * Ka;
}
}
set_thrust(0, motor[0]);
set_thrust(1, motor[1]);
set_thrust(2, motor[2]);
set_thrust(3, motor[3]);
log_put_uint16((unsigned int) (motor[0] * 65535));
log_put_uint16((unsigned int) (motor[1] * 65535));
log_put_uint16((unsigned int) (motor[2] * 65535));
log_put_uint16((unsigned int) (motor[3] * 65535));
}
void motor_kill(void) {
throttle = 0.0;
set_thrust(0, 0.0);
set_thrust(1, 0.0);
set_thrust(2, 0.0);
set_thrust(3, 0.0);
}
void motor_set_throttle(float t) {
if (status_armed())
throttle = t;
}