Skip to content

Commit

Permalink
Stop unconditionally forwarding raw measurements
Browse files Browse the repository at this point in the history
This is so the Roll Controll integrator doesn't wind up before launch.
  • Loading branch information
PSAS FC committed Jul 19, 2015
1 parent cbeff90 commit 2cb9e67
Showing 1 changed file with 3 additions and 6 deletions.
9 changes: 3 additions & 6 deletions src/state.c
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 2cb9e67

Please sign in to comment.