From 2cb9e67eac5058627b812d9a209a05ddede1e3f6 Mon Sep 17 00:00:00 2001 From: PSAS FC Date: Sun, 19 Jul 2015 10:12:52 -0700 Subject: [PATCH] Stop unconditionally forwarding raw measurements This is so the Roll Controll integrator doesn't wind up before launch. --- src/state.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/state.c b/src/state.c index 29d7891..7f3ac21 100644 --- a/src/state.c +++ b/src/state.c @@ -40,20 +40,17 @@ void state_receive_imu(const char *ID, uint8_t *timestamp, uint16_t len, void *b launch_time = now; } - /* Unconditionally forward raw measurements because they're just - * as accurate at any phase of the launch. */ - current_state.acc_up = accel - 9.81; // Subtract gravity (Remember, IMU is upside down) - current_state.roll_rate = -roll_rate; //Sensor coordinate frame is rotated 180 - if (last_time == 0) last_time = now; if (has_launched) { const double dt = (now - last_time) / 1.0e9; // Integrate sensors current_state.time = (now - launch_time) / 1.0e9; + current_state.acc_up = accel - 9.81; // Subtract gravity (Remember, IMU is upside down) current_state.vel_up += current_state.acc_up*dt; current_state.altitude += current_state.vel_up*dt; - current_state.roll_angle += roll_rate*dt; + current_state.roll_rate = -roll_rate; //Sensor coordinate frame is rotated 180 + current_state.roll_angle += roll_rate*dt; //This is wrong (negative) but we don't care, nothing uses it. } // Send data