diff options
Diffstat (limited to 'main.cpp')
| -rw-r--r-- | main.cpp | 144 |
1 files changed, 133 insertions, 11 deletions
@@ -38,6 +38,44 @@ static Pin& cs_gyro = PB5; static Pin& esc_power = PA1; static Pin pwm_outputs[] = {PC6, PC7, PC8, PC9}; +volatile float g_throttle = 0; + +class PID { + float i_acc; + float last_error; + + const float dt = (1.0 / 400); + + public: + float Kp; + float Ki; + float Kd; + + PID() : i_acc(0), last_error(0), Kp(0), Ki(0), Kd(0) {} + + float update(float error) { + float corr = Kp * error + (i_acc += Ki * error * dt) + Kd * (error - last_error) / dt; + + last_error = error; + + return corr; + } + + void reset() { + i_acc = 0; + last_error = 0; + } +}; + +PID pid_x; +PID pid_y; +PID pid_z; + +volatile uint32_t gyro_calib = 0; +float gyro_calib_x = 0; +float gyro_calib_y = 0; +float gyro_calib_z = 0; + auto dev_desc = device_desc(0x200, 0, 0, 0, 64, 0x1234, 0x5678, 0, 0, 0, 0, 1); auto conf_desc = configuration_desc(3, 1, 0, 0xc0, 0, interface_desc(0, 0, 3, 0xff, 0x00, 0x00, 0, @@ -165,7 +203,43 @@ class USB_TM : public USB_class_driver { switch(bufp[0]) { case 2: - TIM8.CCR1 = TIM8.CCR2 = TIM8.CCR3 = TIM8.CCR4 = 1000 + bufp[1] * 2; + g_throttle = float(bufp[1]) / 128.0f * 0.7; + break; + + case 3: + pid_x.Kp = float(bufp[1]) / 128.0f * 0.4; + pid_y.Kp = float(bufp[1]) / 128.0f * 0.4; + break; + + case 4: + pid_x.Ki = float(bufp[1]) / 128.0f * 0.4; + pid_y.Ki = float(bufp[1]) / 128.0f * 0.4; + break; + + case 5: + pid_x.Kd = float(bufp[1]) / 128.0f * 0.4; + pid_y.Kd = float(bufp[1]) / 128.0f * 0.4; + break; + + case 6: + pid_z.Kp = float(bufp[1]) / 128.0f * 0.4; + break; + + case 7: + pid_z.Kp = float(bufp[1]) / 128.0f * 0.4; + break; + + case 8: + pid_z.Kp = float(bufp[1]) / 128.0f * 0.4; + break; + + case 23: + if(bufp[1]) { + gyro_calib = 256; + gyro_calib_x = 0; + gyro_calib_y = 0; + gyro_calib_z = 0; + } break; } } @@ -279,24 +353,70 @@ int main() { TIM8.CCR4 = 1000; TIM8.CR1 = 0x05; + float abst = 0; + while(1) { led_error.toggle(); - Time::sleep(10); + + // Wait for new period. + while(!(TIM8.SR & 1)) { + Thread::yield(); + } + + TIM8.SR = 0; + + float throttle = g_throttle; gyro.update(); - accel.update(); - magn.update(); + //accel.update(); + //magn.update(); + + if(gyro_calib) { + gyro_calib--; + gyro_calib_x += gyro.x / 256; + gyro_calib_y += gyro.y / 256; + gyro_calib_z += gyro.z / 256; + } + + gyro.x -= gyro_calib_x; + gyro.y -= gyro_calib_y; + gyro.z -= gyro_calib_z; + + float stab_x = pid_x.update(-gyro.x); + float stab_y = pid_y.update(-gyro.y); + float stab_z = pid_z.update(-gyro.z); + + if(throttle < 0.05) { + stab_x = 0; + stab_y = 0; + stab_z = 0; + pid_x.reset(); + pid_y.reset(); + pid_z.reset(); + } - int16_t buf[] = { + float motors[] = { + throttle - stab_x - stab_y + stab_z, + throttle - stab_x + stab_y - stab_z, + throttle + stab_x + stab_y + stab_z, + throttle + stab_x - stab_y - stab_z + }; + + TIM8.CCR1 = 1000 + motors[0] * 1000; + TIM8.CCR2 = 1000 + motors[1] * 1000; + TIM8.CCR3 = 1000 + motors[2] * 1000; + TIM8.CCR4 = 1000 + motors[3] * 1000; + + float buf[] = { + abst, gyro.x, gyro.y, gyro.z, - accel.x, - accel.y, - accel.z, - magn.x, - magn.y, - magn.z, + stab_x, + stab_y, + stab_z, + throttle, + 0, }; if(usb.ep_ready(2)) { @@ -304,5 +424,7 @@ int main() { } else { usb_rblog.log("Busy."); } + + abst += 1.0 / 400.0; } } |