From 910b465ac6213beccb0e58cdc68ea532ac5465c2 Mon Sep 17 00:00:00 2001
From: Oleg Semyonov <os@sevsoft.net>
Date: Mon, 7 May 2012 17:35:37 +0300
Subject: [PATCH] AeroSimRC: do not update sensor and/or attitude data if
 read-only flag is set

---
 flight/Modules/Attitude/attitude.c | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c
index 9c2b53106..02ba45744 100644
--- a/flight/Modules/Attitude/attitude.c
+++ b/flight/Modules/Attitude/attitude.c
@@ -234,17 +234,21 @@ static void AttitudeTask(void *parameters)
 
 		AccelsData accels;
 		GyrosData gyros;
-		int32_t retval;
-		if(cc3d) 
+		int32_t retval = 0;
+
+		if (cc3d)
 			retval = updateSensorsCC3D(&accels, &gyros);
 		else
 			retval = updateSensors(&accels, &gyros);
 
-		if(retval != 0)
+		// Only update attitude when sensor data is good
+		if (retval != 0)
 			AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
 		else {
-			// Only update attitude when sensor data is good
-			updateAttitude(&accels, &gyros);
+			// Do not update attitude data in simulation mode
+			if (!AttitudeActualReadOnly())
+				updateAttitude(&accels, &gyros);
+
 			AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 		}
 	}
@@ -268,8 +272,8 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros)
 		return -1;
 	}
 
-	// for simulation
-	if (AttitudeRawReadOnly() == ACCESS_READONLY)
+	// Do not read raw sensor data in simulation mode
+	if (GyrosReadOnly() || AccelsReadOnly())
 		return 0;
 
 	// No accel data available
@@ -420,10 +424,6 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
 
 static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
 {
-	// for simulation
-	if (AttitudeActualReadOnly() == ACCESS_READONLY)
-		return;
-
 	float dT;
 	portTickType thisSysTime = xTaskGetTickCount();
 	static portTickType lastSysTime = 0;