rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

Add attitudesimulated object to capture the simulated state

Browse Source
This commit is contained in:
James Cotton 2012-03-14 18:00:56 -05:00
parent 09ce6e1ab4
commit 84040d25b4
4 changed files with 28 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/Modules/Sensors/simulated/sensors.c
View File

@ -52,6 +52,7 @@
#include "accels.h" #include "accels.h"
#include "actuatordesired.h" #include "actuatordesired.h"
#include "attitudeactual.h" #include "attitudeactual.h"
#include "attitudesimulated.h"
#include "attitudesettings.h" #include "attitudesettings.h"
#include "baroaltitude.h" #include "baroaltitude.h"
#include "gyros.h" #include "gyros.h"
@ -94,6 +95,7 @@ enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER} sensor_sim_typ
int32_t SensorsInitialize(void) int32_t SensorsInitialize(void)
{ {
AccelsInitialize(); AccelsInitialize();
AttitudeSimulatedInitialize();
BaroAltitudeInitialize(); BaroAltitudeInitialize();
GyrosInitialize(); GyrosInitialize();
GyrosBiasInitialize(); GyrosBiasInitialize();
@ -328,9 +330,9 @@ static void simulateModelQuadcopter()
RateDesiredData rateDesired; RateDesiredData rateDesired;
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
rpy[0] = thrust / MAX_THRUST * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; rpy[0] = rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
rpy[1] = thrust / MAX_THRUST * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; rpy[1] = rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
rpy[2] = thrust / MAX_THRUST * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; rpy[2] = rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
GyrosData gyrosData; // Skip get as we set all the fields GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[0] + rand_gauss(); 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.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]; mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
MagnetometerSet(&mag); 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);
} }

1
flight/PiOS.osx/osx/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c
View File

@ -879,6 +879,7 @@ void prvSuspendSignalHandler(int sig)
//fprintf(stderr, "Caught suspend signal (%d): %s\r\n", sig, threadToName(pthread_self())); //fprintf(stderr, "Caught suspend signal (%d): %s\r\n", sig, threadToName(pthread_self()));
storeSelf(); storeSelf();
pauseSelf(); pauseSelf();
assert(prvGetTaskHandle(pthread_self()) == xTaskGetCurrentTaskHandle());
claimRunningSemaphore(1); claimRunningSemaphore(1);
unmaskSuspend(); unmaskSuspend();
} }

5
flight/Revolution/Makefile.osx
View File

@ -154,6 +154,11 @@ endif
ifndef TESTAPP ifndef TESTAPP
#include $(UAVOBJSYNTHDIR)/Makefile.inc #include $(UAVOBJSYNTHDIR)/Makefile.inc
include ./UAVObjects.inc include ./UAVObjects.inc
UAVOBJSRCFILENAMES += attitudesimulated
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
SRC += $(UAVOBJSRC) SRC += $(UAVOBJSRC)
CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE) CFLAGS_UAVOBJECTS = $(UAVOBJDEFINE)
endif endif

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -26,6 +26,7 @@ OTHER_FILES += UAVObjects.pluginspec
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/baroaltitude.h \ $$UAVOBJECT_SYNTHETICS/baroaltitude.h \
$$UAVOBJECT_SYNTHETICS/attitudeactual.h \ $$UAVOBJECT_SYNTHETICS/attitudeactual.h \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
@ -88,6 +89,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \ $$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \
$$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \ $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \