The unified diff between revisions [d0420ebd..] and [bfc9e27f..] 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 [bfc9e27f5c40da31ae4269394aaf0545e5856a70] # # patch "motor.c" # from [500d9a9173fe983ae7c50cf5fac1d37efa96d8ce] # to [743afb567c758bd114cb1dd29f25081f3454108c] # ============================================================ --- motor.c 500d9a9173fe983ae7c50cf5fac1d37efa96d8ce +++ motor.c 743afb567c758bd114cb1dd29f25081f3454108c @@ -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]; @@ -32,7 +34,7 @@ void motor_pid_update(float troll, float { float derivative[3]; float out[3]; - float motor[3]; + float motor[4]; float roll, pitch, yaw; float error, max_error; float min_motor; @@ -56,6 +58,14 @@ 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; @@ -69,7 +79,7 @@ void motor_pid_update(float troll, float 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 +142,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; }