The unified diff between revisions [23a3e9a5..] and [dc88787e..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'motor.c'
# # old_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd] # new_revision [dc88787ecd1d574feba045763baed2a7651ff33d] # # add_file "motor.c" # content [9f90df05166bf056393d18108f1b25d0fd34da52] # ============================================================ --- /dev/null +++ motor.c 9f90df05166bf056393d18108f1b25d0fd34da52 @@ -0,0 +1,185 @@ +/* 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; +} +