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

@@ -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;
 	}