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.
185 lines
4.0 KiB
185 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; |
|
} |
|
|
|
|