summaryrefslogtreecommitdiff
path: root/main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'main.cpp')
-rw-r--r--main.cpp144
1 files changed, 133 insertions, 11 deletions
diff --git a/main.cpp b/main.cpp
index aa1df03..aaa3661 100644
--- a/main.cpp
+++ b/main.cpp
@@ -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;
}
}