From 735a8bd11070b0c563e891ff8b70ce297a52a367 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Fri, 5 Jan 2018 03:54:29 -0700
Subject: Add standard output mechanism for posers

Added a raw pose output/ callback that the posers can
call when they have calculated a pose.
---
 src/survive_process.c | 39 ++++++++++++++++++++++++++++++---------
 1 file changed, 30 insertions(+), 9 deletions(-)

(limited to 'src/survive_process.c')

diff --git a/src/survive_process.c b/src/survive_process.c
index 4b86144..42d2897 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -66,20 +66,34 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val)
 {
 	// do nothing.
-	//printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
-	//	eventType,
-	//	buttonId,
-	//	axis1Id,
-	//	axis1Val,
-	//	axis2Id,
-	//	axis2Val);
+	printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
+		eventType,
+		buttonId,
+		axis1Id,
+		axis1Val,
+		axis2Id,
+		axis2Val);
 	if (buttonId == 24 && eventType == 1) // trigger engage
 	{
-		for (int j = 0; j < 40; j++)
+		for (int j = 0; j < 6; j++)
 		{
-			for (int i = 0; i < 0x1; i++)
+			for (int i = 0; i < 0x5; i++)
 			{
 				survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+				//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+				OGUSleep(1000);
+			}
+			OGUSleep(20000);
+		}
+	}
+	if (buttonId == 2 && eventType == 1) // trigger engage
+	{
+		for (int j = 0; j < 6; j++)
+		{
+			for (int i = 0; i < 0x1; i++)
+			{
+				survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100);
+				//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
 				OGUSleep(5000);
 			}
 			OGUSleep(20000);
@@ -87,6 +101,13 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8
 	}
 }
 
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat)
+{
+	// print the pose;
+	printf("Pose: [%2.2x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+
+}
+
 void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
 {
 	if( so->PoserFn )
-- 
cgit v1.2.3