From 84040d25b4635f89a8c9fb49cb315f9a4a4dcd5a Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Wed, 14 Mar 2012 18:00:56 -0500
Subject: [PATCH] Add attitudesimulated object to capture the simulated state

---
 flight/Modules/Sensors/simulated/sensors.c    | 23 ++++++++++++++++---
 .../FreeRTOS/Source/portable/GCC/Posix/port.c |  1 +
 flight/Revolution/Makefile.osx                |  5 ++++
 .../src/plugins/uavobjects/uavobjects.pro     |  2 ++
 4 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/flight/Modules/Sensors/simulated/sensors.c b/flight/Modules/Sensors/simulated/sensors.c
index d2384d9af..0e42cdc19 100644
--- a/flight/Modules/Sensors/simulated/sensors.c
+++ b/flight/Modules/Sensors/simulated/sensors.c
@@ -52,6 +52,7 @@
 #include "accels.h"
 #include "actuatordesired.h"
 #include "attitudeactual.h"
+#include "attitudesimulated.h"
 #include "attitudesettings.h"
 #include "baroaltitude.h"
 #include "gyros.h"
@@ -94,6 +95,7 @@ enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER} sensor_sim_typ
 int32_t SensorsInitialize(void)
 {
 	AccelsInitialize();
+	AttitudeSimulatedInitialize();
 	BaroAltitudeInitialize();
 	GyrosInitialize();
 	GyrosBiasInitialize();
@@ -328,9 +330,9 @@ static void simulateModelQuadcopter()
 	RateDesiredData rateDesired;
 	RateDesiredGet(&rateDesired);
 	
-	rpy[0] = thrust / MAX_THRUST * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
-	rpy[1] = thrust / MAX_THRUST * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
-	rpy[2] = thrust / MAX_THRUST * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
+	rpy[0] = rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
+	rpy[1] = rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
+	rpy[2] = rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
 	
 	GyrosData gyrosData; // Skip get as we set all the fields
 	gyrosData.x = rpy[0] + rand_gauss();
@@ -434,6 +436,21 @@ static void simulateModelQuadcopter()
 	mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
 	mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
 	MagnetometerSet(&mag);
+	
+	AttitudeSimulatedData attitudeSimulated;
+	AttitudeSimulatedGet(&attitudeSimulated);
+	attitudeSimulated.q1 = q[0];
+	attitudeSimulated.q2 = q[1];
+	attitudeSimulated.q3 = q[2];
+	attitudeSimulated.q4 = q[3];
+	Quaternion2RPY(q,&attitudeSimulated.Roll);
+	attitudeSimulated.Position[0] = pos[0];
+	attitudeSimulated.Position[1] = pos[1];
+	attitudeSimulated.Position[2] = pos[2];
+	attitudeSimulated.Velocity[0] = vel[0];
+	attitudeSimulated.Velocity[1] = vel[1];
+	attitudeSimulated.Velocity[2] = vel[2];
+	AttitudeSimulatedSet(&attitudeSimulated);
 }
 
 
diff --git a/flight/PiOS.osx/osx/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c b/flight/PiOS.osx/osx/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c
index 06be34407..2321403ca 100644
--- a/flight/PiOS.osx/osx/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c
+++ b/flight/PiOS.osx/osx/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c
@@ -879,6 +879,7 @@ void prvSuspendSignalHandler(int sig)
 		//fprintf(stderr, "Caught suspend signal (%d): %s\r\n", sig, threadToName(pthread_self()));
 		storeSelf();
 		pauseSelf();
+		assert(prvGetTaskHandle(pthread_self()) == xTaskGetCurrentTaskHandle());
 		claimRunningSemaphore(1);
 		unmaskSuspend();
 	}
diff --git a/flight/Revolution/Makefile.osx b/flight/Revolution/Makefile.osx
index 3f0a9d6ba..177583368 100644
--- a/flight/Revolution/Makefile.osx
+++ b/flight/Revolution/Makefile.osx
@@ -154,6 +154,11 @@ endif
 ifndef TESTAPP
 #include $(UAVOBJSYNTHDIR)/Makefile.inc
 include ./UAVObjects.inc
+
+UAVOBJSRCFILENAMES += attitudesimulated
+UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
+UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
+
 SRC += $(UAVOBJSRC)
 CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
 endif
diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
index 28a84329e..95115b248 100644
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
@@ -26,6 +26,7 @@ OTHER_FILES += UAVObjects.pluginspec
 HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/baroaltitude.h \
     $$UAVOBJECT_SYNTHETICS/attitudeactual.h \
+    $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
     $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
     $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
     $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
@@ -88,6 +89,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
 SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \
     $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \
+    $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
     $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \
     $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
     $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \