The unified diff between revisions [d0420ebd..] and [24d5b9f4..] 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 [24d5b9f4dff9135787b198fe1127d9c1e3326b9c] # # patch "motor.c" # from [500d9a9173fe983ae7c50cf5fac1d37efa96d8ce] # to [0a5cff753a89545c1d3f23027cfb4f651db669cd] # ============================================================ --- motor.c 500d9a9173fe983ae7c50cf5fac1d37efa96d8ce +++ motor.c 0a5cff753a89545c1d3f23027cfb4f651db669cd @@ -1,17 +1,18 @@ /* motor.c */ #include "stick.h" -#include "timer.h" +#include "thrust.h" #include "dcm.h" #include "uart.h" +#include "status.h" float integral[3] = {0.0f, 0.0f, 0.0f}; float last[3]; float throttle = 0.0f; -#define Kp 0.2 -#define Ki 0.04 +#define Kp 0.3 +#define Ki 0.02 #define Kd 0.08 #define Ka 0.0 @@ -56,6 +57,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 +78,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 +141,22 @@ 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]); } 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; }