The unified diff between revisions [d0420ebd..] and [3dc5e7ac..] 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 [d0420ebd87c820e33a32b29727989516e15980a8] # new_revision [3dc5e7ac4bcb952cc267892653dd78ed095d8778] # # patch "motor.c" # from [500d9a9173fe983ae7c50cf5fac1d37efa96d8ce] # to [38218e6c4dee3f736912d3f16e65339741b2e36b] # ============================================================ --- motor.c 500d9a9173fe983ae7c50cf5fac1d37efa96d8ce +++ motor.c 38218e6c4dee3f736912d3f16e65339741b2e36b @@ -1,9 +1,11 @@ /* motor.c */ #include "stick.h" -#include "timer.h" +#include "thrust.h" #include "dcm.h" #include "uart.h" +#include "status.h" +#include "log.h" float integral[3] = {0.0f, 0.0f, 0.0f}; float last[3]; @@ -25,29 +27,31 @@ float throttle = 0.0f; * 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(float troll, float mroll, - float tpitch, float mpitch, - float tyaw, float myaw) +void motor_pid_update(vec3f target, vec3f measured) { float derivative[3]; float out[3]; - float motor[3]; + float motor[4]; float roll, pitch, yaw; float error, max_error; float min_motor; int i; - roll = troll - mroll; - pitch = tpitch - mpitch; - yaw = tyaw - myaw; + 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)(tyaw * 10000)); + putint_s((int)(target.z * 10000)); putstr(", "); - putint_s((int)(myaw * 10000)); + putint_s((int)(measured.z * 10000)); putstr("}\r\n"); } #endif @@ -56,20 +60,28 @@ void motor_pid_update(float troll, float 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] = (-mroll - last[0]) / delta_t; - derivative[1] = (-mpitch - last[1]) / delta_t; - derivative[2] = (-myaw - last[2]) / delta_t; + 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] = -mroll; - last[1] = -mpitch; - last[2] = -myaw; + 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 (armed) { + if (status_armed()) { /* Front right */ motor[0] = throttle + out[0] + out[1] + out[2]; /* Front left */ @@ -132,22 +144,27 @@ void motor_pid_update(float troll, float } } - timer_set_pwm_value(0, (int)(motor[0] * PWM_MAX)); - timer_set_pwm_value(1, (int)(motor[1] * PWM_MAX)); - timer_set_pwm_value(2, (int)(motor[2] * PWM_MAX)); - timer_set_pwm_value(3, (int)(motor[3] * PWM_MAX)); + 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; - timer_set_pwm_value(0, 0); - timer_set_pwm_value(1, 0); - timer_set_pwm_value(2, 0); - timer_set_pwm_value(3, 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 (armed) + if (status_armed()) throttle = t; }