From cbeff90632973c00bd8ea9dec2aee512654ebca0 Mon Sep 17 00:00:00 2001 From: PSAS FC Date: Sun, 19 Jul 2015 02:13:11 -0700 Subject: [PATCH] Gravity should be subtracted from acceleration Also in the case where the first imu message is in state has_launched, last_time will no longer cause velocity to be wildly off --- src/state.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/state.c b/src/state.c index 51372a5..29d7891 100644 --- a/src/state.c +++ b/src/state.c @@ -42,15 +42,16 @@ void state_receive_imu(const char *ID, uint8_t *timestamp, uint16_t len, void *b /* Unconditionally forward raw measurements because they're just * as accurate at any phase of the launch. */ - current_state.acc_up = accel; - current_state.roll_rate = roll_rate; + 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.vel_up += accel*dt; + current_state.vel_up += current_state.acc_up*dt; current_state.altitude += current_state.vel_up*dt; current_state.roll_angle += roll_rate*dt; }