The unified diff between revisions [64de686d..] and [9142f333..] 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 [64de686d701acb9539dc52fe0bff299405612ab0] # new_revision [9142f3330490a5aa00c1686475633b620c2ef5e7] # # patch "motor.c" # from [b9920bb4788532a73cb93301787e4d0540cc3d56] # to [743afb567c758bd114cb1dd29f25081f3454108c] # ============================================================ --- motor.c b9920bb4788532a73cb93301787e4d0540cc3d56 +++ motor.c 743afb567c758bd114cb1dd29f25081f3454108c @@ -1,10 +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]; @@ -33,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; @@ -57,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; @@ -133,18 +142,23 @@ 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) {