From d22e0e66ddaccb5e3291d177a8ba90a6fb20eaeb Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 12 Dec 2011 01:34:26 -0600 Subject: [PATCH] Create separate UAVOs for the sensors. --- flight/Modules/Attitude/revolution/attitude.c | 36 ++++++++----------- flight/Revolution/UAVObjects.inc | 4 ++- .../src/plugins/uavobjects/uavobjects.pro | 8 +++-- shared/uavobjectdefinition/accels.xml | 13 +++++++ shared/uavobjectdefinition/attituderaw.xml | 13 ------- shared/uavobjectdefinition/gyros.xml | 13 +++++++ shared/uavobjectdefinition/magnetometer.xml | 12 +++++++ 7 files changed, 61 insertions(+), 38 deletions(-) create mode 100644 shared/uavobjectdefinition/accels.xml delete mode 100644 shared/uavobjectdefinition/attituderaw.xml create mode 100644 shared/uavobjectdefinition/gyros.xml create mode 100644 shared/uavobjectdefinition/magnetometer.xml diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c index 4d8c09993..031798143 100644 --- a/flight/Modules/Attitude/revolution/attitude.c +++ b/flight/Modules/Attitude/revolution/attitude.c @@ -50,7 +50,9 @@ #include "pios.h" #include "attitude.h" -#include "attituderaw.h" +#include "magnetometer.h" +#include "accels.h" +#include "gyros.h" #include "attitudeactual.h" #include "attitudesettings.h" #include "baroaltitude.h" @@ -94,23 +96,13 @@ static int8_t rotate = 0; static bool zero_during_arming = false; static bool bias_correct_gyro = true; -struct gyro_data { - float x; - float y; - float z; -}; - -struct accel_data { - float x; - float y; - float z; -}; - -struct mag_data { - float x; - float y; - float z; -}; +/** + * API for sensor fusion algorithms: + * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro) + * Stores all the queues the algorithm will pull data from + * FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias) + * Update() -- queries queues and updates the attitude estiamte + */ /** * Initialise the module, called on startup @@ -119,10 +111,10 @@ struct mag_data { int32_t AttitudeStart(void) { // Create the queues for the sensors - gyroQueue = xQueueCreate(SENSOR_QUEUE_SIZE, sizeof(struct gyro_data)); - accelQueue = xQueueCreate(SENSOR_QUEUE_SIZE, sizeof(struct accel_data)); - magQueue = xQueueCreate(SENSOR_QUEUE_SIZE, sizeof(struct mag_data)); - + gyroQueue = xQueueCreate(SENSOR_QUEUE_SIZE, sizeof(GyrosData)); + accelQueue = xQueueCreate(SENSOR_QUEUE_SIZE, sizeof(AccelsData)); + magQueue = xQueueCreate(SENSOR_QUEUE_SIZE, sizeof(MagnetometerData)); + // Start main task xTaskCreate(SensorTask, (signed char *)"Sensors", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &sensorTaskHandle); xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle); diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index f2e54b92f..b3b6e93f0 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -32,7 +32,9 @@ UAVOBJSRCFILENAMES += attitudesettings UAVOBJSRCFILENAMES += inssettings UAVOBJSRCFILENAMES += insstatus UAVOBJSRCFILENAMES += attitudeactual -UAVOBJSRCFILENAMES += attituderaw +UAVOBJSRCFILENAMES += gyros +UAVOBJSRCFILENAMES += accels +UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index bf291c9ec..5ad5f7591 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -29,7 +29,9 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/attitudeactual.h \ $$UAVOBJECT_SYNTHETICS/inssettings.h \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \ - $$UAVOBJECT_SYNTHETICS/attituderaw.h \ + $$UAVOBJECT_SYNTHETICS/gyros.h \ + $$UAVOBJECT_SYNTHETICS/accels.h \ + $$UAVOBJECT_SYNTHETICS/magnetometer.h \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/systemstats.h \ @@ -79,7 +81,9 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \ $$UAVOBJECT_SYNTHETICS/inssettings.cpp \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \ - $$UAVOBJECT_SYNTHETICS/attituderaw.cpp \ + $$UAVOBJECT_SYNTHETICS/accels.cpp \ + $$UAVOBJECT_SYNTHETICS/gyros.cpp \ + $$UAVOBJECT_SYNTHETICS/magnetometer.cpp \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/systemstats.cpp \ diff --git a/shared/uavobjectdefinition/accels.xml b/shared/uavobjectdefinition/accels.xml new file mode 100644 index 000000000..2eb2a1b5e --- /dev/null +++ b/shared/uavobjectdefinition/accels.xml @@ -0,0 +1,13 @@ + + + The accel data. + + + + + + + + + + diff --git a/shared/uavobjectdefinition/attituderaw.xml b/shared/uavobjectdefinition/attituderaw.xml deleted file mode 100644 index b3d11229c..000000000 --- a/shared/uavobjectdefinition/attituderaw.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - The raw attitude sensor data from @ref AHRSCommsModule. Not always updated. - - - - - - - - - - diff --git a/shared/uavobjectdefinition/gyros.xml b/shared/uavobjectdefinition/gyros.xml new file mode 100644 index 000000000..ec7fbf8ba --- /dev/null +++ b/shared/uavobjectdefinition/gyros.xml @@ -0,0 +1,13 @@ + + + The gyro data. + + + + + + + + + + diff --git a/shared/uavobjectdefinition/magnetometer.xml b/shared/uavobjectdefinition/magnetometer.xml new file mode 100644 index 000000000..c6bc90662 --- /dev/null +++ b/shared/uavobjectdefinition/magnetometer.xml @@ -0,0 +1,12 @@ + + + The mag data. + + + + + + + + +