#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16))
#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16))
#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16))
#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16))
#define ao_xmemcmp(d,s,c) memcmp(d,s,c)
#define AO_NEED_ALTITUDE_TO_PRES 1
#define ao_xmemcmp(d,s,c) memcmp(d,s,c)
#define AO_NEED_ALTITUDE_TO_PRES 1
"accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f "
#endif
"state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
time,
height,
accel,
"accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f "
#endif
"state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
time,
height,
accel,
ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2;
q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3;
q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2;
q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3;
printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt);
printf ("q_dot (%g) %g %g %g\n",
q_dot.q0,
q_dot.q1,
q_dot.q2,
q_dot.q3);
printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt);
printf ("q_dot (%g) %g %g %g\n",
q_dot.q0,
q_dot.q1,
q_dot.q2,
q_dot.q3);
double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y);
double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z);
double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y);
double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z);
- update_orientation(rate_x, rate_z, rate_y, tick);
+ update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick);
type = words[0][0];
tick = strtoul(words[1], NULL, 16);
a = strtoul(words[2], NULL, 16);
type = words[0][0];
tick = strtoul(words[1], NULL, 16);
a = strtoul(words[2], NULL, 16);