summary |
shortlog |
log |
commit | commitdiff |
tree
raw |
patch |
inline | side by side (from parent 1:
9877f6b)
Only present when HAS_FLIGHT_DEBUG is enabled, this command lets the
user check the orientation tracking code by showing the current
orientation and when the calibration values are reset.
Signed-off-by: Keith Packard <keithp@keithp.com>
ao_flight_state = ao_flight_idle;
}
ao_flight_state = ao_flight_idle;
}
+uint8_t ao_orient_test;
+
+static void
+ao_orient_test_select(void)
+{
+ ao_orient_test = !ao_orient_test;
+}
+
__code struct ao_cmds ao_flight_cmds[] = {
{ ao_flight_dump, "F\0Dump flight status" },
{ ao_gyro_test, "G\0Test gyro code" },
__code struct ao_cmds ao_flight_cmds[] = {
{ ao_flight_dump, "F\0Dump flight status" },
{ ao_gyro_test, "G\0Test gyro code" },
+ { ao_orient_test_select,"O\0Test orientation code" },
static struct ao_quaternion ao_rotation;
#endif
static struct ao_quaternion ao_rotation;
#endif
+#if HAS_FLIGHT_DEBUG
+extern uint8_t ao_orient_test;
+#endif
+
static void
ao_sample_preflight_add(void)
{
static void
ao_sample_preflight_add(void)
{
* that as the current rotation vector
*/
ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient);
* that as the current rotation vector
*/
ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient);
+#if HAS_FLIGHT_DEBUG
+ if (ao_orient_test)
+ printf("\n\treset\n");
rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r;
ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI);
rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r;
ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI);
+
+#if HAS_FLIGHT_DEBUG
+ if (ao_orient_test) {
+ printf ("rot %d %d %d orient %d \r",
+ (int) (x * 1000),
+ (int) (y * 1000),
+ (int) (z * 1000),
+ ao_sample_orient);
+ }
+#endif
+