From 95eab1b8938e0033a0aa0d10d6ec53d7d2782907 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 11 Mar 2018 15:31:28 -0600 Subject: Added callback for lighthouse found; example in test.c --- test.c | 8 ++++++++ 1 file changed, 8 insertions(+) (limited to 'test.c') diff --git a/test.c b/test.c index bc9677a..d4b3d6f 100644 --- a/test.c +++ b/test.c @@ -84,6 +84,13 @@ void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t butt } } +void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose); + + printf("Lighthouse: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pose->Pos[0], + pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); +} + void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { survive_default_raw_pose_process(so, lighthouse, pose); @@ -139,6 +146,7 @@ int main() survive_install_button_fn(ctx, testprog_button_process); survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); survive_install_imu_fn(ctx, testprog_imu_process); + survive_install_lighthouse_pose_fn(ctx, testprog_lighthouse_process); if( !ctx ) { -- cgit v1.2.3