From a37a17a4fb61b93fb1e948e4eba179f6e7b5efc0 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 12 Dec 2011 14:39:38 -0600 Subject: [PATCH] Created a separate Sensor module and Attitude module for revolution --- flight/Modules/Attitude/revolution/attitude.c | 323 +++++------------- flight/Modules/Sensors/inc/sensors.h | 37 ++ flight/Modules/Sensors/sensors.c | 297 ++++++++++++++++ .../OpenPilotOSX.xcodeproj/project.pbxproj | 20 ++ flight/Revolution/Makefile | 2 +- flight/Revolution/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 + shared/uavobjectdefinition/gyrosbias.xml | 12 + 8 files changed, 461 insertions(+), 233 deletions(-) create mode 100644 flight/Modules/Sensors/inc/sensors.h create mode 100644 flight/Modules/Sensors/sensors.c create mode 100644 shared/uavobjectdefinition/gyrosbias.xml diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c index ac1c71740..aa2ef1ad8 100644 --- a/flight/Modules/Attitude/revolution/attitude.c +++ b/flight/Modules/Attitude/revolution/attitude.c @@ -53,6 +53,7 @@ #include "magnetometer.h" #include "accels.h" #include "gyros.h" +#include "gyrosbias.h" #include "attitudeactual.h" #include "attitudesettings.h" #include "flightstatus.h" @@ -61,6 +62,7 @@ // Private constants #define STACK_SIZE_BYTES 1540 #define TASK_PRIORITY (tskIDLE_PRIORITY+3) +#define FAILSAFE_TIMEOUT_MS 10 #define F_PI 3.14159265358979323846f #define PI_MOD(x) (fmod(x + F_PI, F_PI * 2) - F_PI) @@ -80,7 +82,7 @@ static void SensorTask(void *parameters); static void AttitudeTask(void *parameters); static int32_t updateSensors(); -static int32_t updateAttitudeComplimentary(); +static int32_t updateAttitudeComplimentary(bool first_run); static void settingsUpdatedCb(UAVObjEvent * objEv); static float accelKi = 0; @@ -92,9 +94,6 @@ static float R[3][3]; static int8_t rotate = 0; static bool zero_during_arming = false; -// These values are initialized by settings but can be updated by the attitude algorithm -static bool bias_correct_gyro = true; -static float gyro_bias[3] = {0,0,0}; /** * API for sensor fusion algorithms: @@ -104,38 +103,14 @@ static float gyro_bias[3] = {0,0,0}; * Update() -- queries queues and updates the attitude estiamte */ -/** - * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed - */ -int32_t AttitudeStart(void) -{ - // Create the queues for the sensors - 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); - TaskMonitorAdd(TASKINFO_RUNNING_SENSORS, sensorTaskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); - PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); - - return 0; -} /** - * Initialise the module, called on startup + * Initialise the module. Called before the start function * \returns 0 on success or -1 if initialisation failed */ int32_t AttitudeInitialize(void) { AttitudeActualInitialize(); - GyrosInitialize(); - AccelsInitialize(); - MagnetometerInitialize(); AttitudeSettingsInitialize(); // Initialize quaternion @@ -146,238 +121,117 @@ int32_t AttitudeInitialize(void) attitude.q3 = 0; attitude.q4 = 0; AttitudeActualSet(&attitude); - + // Cannot trust the values to init right above if BL runs - gyro_bias[0] = 0; - gyro_bias[1] = 0; - gyro_bias[2] = 0; - + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosBias.x = 0; + gyrosBias.y = 0; + gyrosBias.z = 0; + GyrosBiasSet(&gyrosBias); + for(uint8_t i = 0; i < 3; i++) for(uint8_t j = 0; j < 3; j++) R[i][j] = 0; - + AttitudeSettingsConnectCallback(&settingsUpdatedCb); + + return 0; +} + +/** + * Start the task. Expects all objects to be initialized by this point. + * \returns 0 on success or -1 if initialisation failed + */ +int32_t AttitudeStart(void) +{ + // Create the queues for the sensors + gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + + // Start main task + xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); + + GyrosConnectQueue(gyroQueue); return 0; } MODULE_INITCALL(AttitudeInitialize, AttitudeStart) -int32_t accel_test; -int32_t gyro_test; -int32_t mag_test; -//int32_t pressure_test; - -/** - * The sensor task. This polls the gyros at 500 Hz and pumps that data to - * stabilization and to the attitude loop - */ -static void SensorTask(void *parameters) -{ - uint8_t init = 0; - portTickType lastSysTime; - - AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); - - accel_test = PIOS_BMA180_Test(); - gyro_test = PIOS_MPU6000_Test(); - mag_test = PIOS_HMC5883_Test(); - - if(accel_test < 0 || gyro_test < 0 || mag_test < 0) { - AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); - while(1) { - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); - vTaskDelay(1); - } - } - - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) { - - // TODO: This initialization stuff should be refactored from here - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { - // For first 7 seconds use accels to get gyro bias - accelKp = 1; - accelKi = 0.9; - yawBiasRate = 0.23; - init = 0; - } - else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { - accelKp = 1; - accelKi = 0.9; - yawBiasRate = 0.23; - init = 0; - } else if (init == 0) { - // Reload settings (all the rates) - AttitudeSettingsAccelKiGet(&accelKi); - AttitudeSettingsAccelKpGet(&accelKp); - AttitudeSettingsYawBiasRateGet(&yawBiasRate); - init = 1; - } - - - if(updateSensors() != 0) - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - else { - // TODO: Push data onto queue - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); - vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS); - } -} - /** * Module thread, should not return. */ static void AttitudeTask(void *parameters) { - uint8_t init = 0; AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); // Force settings update to make sure rotation loaded settingsUpdatedCb(AttitudeSettingsHandle()); + + bool first_run = true; // Main task loop while (1) { // This function blocks on data queue - updateAttitudeComplimentary(); + updateAttitudeComplimentary(first_run); + + if (first_run) + first_run = false; PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); } } - -uint32_t accel_samples; -uint32_t gyro_samples; -struct pios_bma180_data accel; -struct pios_mpu6000_data gyro; -int32_t accel_accum[3] = {0, 0, 0}; -int32_t gyro_accum[3] = {0,0,0}; -float scaling; - -/** - * Get an update from the sensors - * @param[in] attitudeRaw Populate the UAVO instead of saving right here - * @return 0 if successfull, -1 if not - */ -static int32_t updateSensors() -{ - int32_t read_good; - int32_t count; - - for (int i = 0; i < 3; i++) { - accel_accum[i] = 0; - gyro_accum[i] = 0; - } - accel_samples = 0; - gyro_samples = 0; - - // Make sure we get one sample - count = 0; - while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0); - while(read_good == 0) { - count++; - - accel_accum[0] += accel.x; - accel_accum[1] += accel.y; - accel_accum[2] += accel.z; - - read_good = PIOS_BMA180_ReadFifo(&accel); - } - accel_samples = count; - - float accels[3] = {(float) accel_accum[1] / accel_samples, (float) accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples}; - - // Not the swaping of channel orders - scaling = PIOS_BMA180_GetScale(); - AccelsData accelsData; // Skip get as we set all the fields - accelsData.x = (accels[0] - accelbias[0]) * scaling; - accelsData.y = (accels[1] - accelbias[1]) * scaling; - accelsData.z = (accels[2] - accelbias[2]) * scaling; - accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f; - AccelsSet(&accelsData); - - // Push the data onto the queue for attitude to consume - if(xQueueSendToBack(accelQueue, (void *) &accelsData, 0) != pdTRUE) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - } - - // Make sure we get one sample - count = 0; - while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0); - while(read_good == 0) { - count++; - - gyro_accum[0] += gyro.gyro_x; - gyro_accum[1] += gyro.gyro_y; - gyro_accum[2] += gyro.gyro_z; - - read_good = PIOS_MPU6000_ReadFifo(&gyro); - } - gyro_samples = count; - - float gyros[3] = {(float) gyro_accum[1] / gyro_samples, (float) gyro_accum[0] / gyro_samples, -(float) gyro_accum[2] / gyro_samples}; - - scaling = PIOS_MPU6000_GetScale(); - GyrosData gyrosData; // Skip get as we set all the fields - gyrosData.x = gyros[0] * scaling; - gyrosData.y = gyros[1] * scaling; - gyrosData.z = gyros[2] * scaling; - gyrosData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f; - // Don't set yet. We push raw data to queue but then bias correct for other modules - - // Push the data onto the queue for attitude to consume - if(xQueueSendToBack(gyroQueue, (void *) &gyrosData, 0) != pdTRUE) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - } - - // Apply bias correction to the gyros - if(bias_correct_gyro) { - gyrosData.x += gyro_bias[0]; - gyrosData.y += gyro_bias[1]; - gyrosData.z += gyro_bias[2]; - } - GyrosSet(&gyrosData); - - // Because most crafts wont get enough information from gravity to zero yaw gyro, we try - // and make it average zero (weakly) - gyro_bias[2] += - gyrosData.z * yawBiasRate; - - if (PIOS_HMC5883_NewDataAvailable()) { - int16_t values[3]; - PIOS_HMC5883_ReadMag(values); - MagnetometerData mag; // Skip get as we set all the fields - mag.x = -values[0]; - mag.y = -values[1]; - mag.z = -values[2]; - MagnetometerSet(&mag); - } - - return 0; -} - float accel_mag; float qmag; -static int32_t updateAttitudeComplimentary() +static int32_t updateAttitudeComplimentary(bool first_run) { + UAVObjEvent ev; GyrosData gyrosData; AccelsData accelsData; - - if(xQueueReceive(gyroQueue, (void *) &gyrosData, 10 / portTICK_RATE_MS) != pdTRUE || - xQueueReceive(accelQueue, (void *) &accelsData, 10 / portTICK_RATE_MS) != pdTRUE) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + static int32_t timeval; + float dT; + static uint8_t init = 0; + + // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe + if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) + { + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); return -1; } + + // During initialization and + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + if(first_run) + init = 0; - static int32_t timeval; - float dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; + if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { + // For first 7 seconds use accels to get gyro bias + accelKp = 1; + accelKi = 0.9; + yawBiasRate = 0.23; + } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { + accelKp = 1; + accelKi = 0.9; + yawBiasRate = 0.23; + init = 0; + } else if (init == 0) { + // Reload settings (all the rates) + AttitudeSettingsAccelKiGet(&accelKi); + AttitudeSettingsAccelKpGet(&accelKp); + AttitudeSettingsYawBiasRateGet(&yawBiasRate); + init = 1; + } + + GyrosGet(&gyrosData); + AccelsGet(&accelsData); + + // Compute the dT using the cpu clock + dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; timeval = PIOS_DELAY_GetRaw(); float q[4]; @@ -405,15 +259,18 @@ static int32_t updateAttitudeComplimentary() accel_err[2] /= accel_mag; // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - gyro_bias[0] += accel_err[0] * accelKi; - gyro_bias[1] += accel_err[1] * accelKi; - + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosBias.x += accel_err[0] * accelKi; + gyrosBias.y += accel_err[1] * accelKi; + gyrosBias.z += - gyrosData.z * yawBiasRate; + GyrosBiasSet(&gyrosBias); + // Correct rates based on error, integral component dealt with in updateSensors gyrosData.x += accel_err[0] * accelKp / dT; gyrosData.y += accel_err[1] * accelKp / dT; gyrosData.z += accel_err[2] * accelKp / dT; - // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; @@ -474,16 +331,18 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) { gyroGain = attitudeSettings.GyroGain; zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; - bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; - gyro_bias[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f; - gyro_bias[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f; - gyro_bias[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f; - + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosBias.x = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f; + gyrosBias.y = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f; + gyrosBias.z = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f; + GyrosBiasSet(&gyrosBias); + // Indicates not to expend cycles on rotation if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && attitudeSettings.BoardRotation[2] == 0) { diff --git a/flight/Modules/Sensors/inc/sensors.h b/flight/Modules/Sensors/inc/sensors.h new file mode 100644 index 000000000..16e24daab --- /dev/null +++ b/flight/Modules/Sensors/inc/sensors.h @@ -0,0 +1,37 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Sensors Sensors Module + * @{ + * + * @file attitude.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Acquires sensor data and fuses it into attitude estimate for CC + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef SENSORS_H +#define SENSORS_H + +#include "openpilot.h" + +int32_t SensorsInitialize(void); + +#endif // SENSORS_H diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c new file mode 100644 index 000000000..17d3f3970 --- /dev/null +++ b/flight/Modules/Sensors/sensors.c @@ -0,0 +1,297 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Sensors + * @brief Acquires sensor data + * Specifically updates the the @ref Gyros, @ref Accels, and @ref Magnetometer objects + * @{ + * + * @file sensors.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Module to handle all comms to the AHRS on a periodic basis. + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * Input objects: None, takes sensor data via pios + * Output objects: @ref Gyros @ref Accels @ref Magnetometer + * + * The module executes in its own thread. + * + * UAVObjects are automatically generated by the UAVObjectGenerator from + * the object definition XML file. + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +#include "pios.h" +#include "attitude.h" +#include "magnetometer.h" +#include "accels.h" +#include "gyros.h" +#include "gyrosbias.h" +#include "attitudeactual.h" +#include "attitudesettings.h" +#include "flightstatus.h" +#include "CoordinateConversions.h" + +// Private constants +#define STACK_SIZE_BYTES 1540 +#define TASK_PRIORITY (tskIDLE_PRIORITY+3) + +#define F_PI 3.14159265358979323846f +#define PI_MOD(x) (fmod(x + F_PI, F_PI * 2) - F_PI) +// Private types + +// Private variables +static xTaskHandle sensorsTaskHandle; + +// Private functions +static void SensorsTask(void *parameters); +static void settingsUpdatedCb(UAVObjEvent * objEv); + +static float gyroGain = 0.42; +static int16_t accelbias[3]; +static float R[3][3]; +static int8_t rotate = 0; +static bool zero_during_arming = false; + +// These values are initialized by settings but can be updated by the attitude algorithm +static bool bias_correct_gyro = true; +static float gyro_bias[3] = {0,0,0}; + +/** + * 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 before the start function + * \returns 0 on success or -1 if initialisation failed + */ +int32_t SensorsInitialize(void) +{ + GyrosInitialize(); + GyrosBiasInitialize(); + AccelsInitialize(); + MagnetometerInitialize(); + AttitudeSettingsInitialize(); + + for(uint8_t i = 0; i < 3; i++) + for(uint8_t j = 0; j < 3; j++) + R[i][j] = 0; + + AttitudeSettingsConnectCallback(&settingsUpdatedCb); + + return 0; +} + +/** + * Start the task. Expects all objects to be initialized by this point. + * \returns 0 on success or -1 if initialisation failed + */ +int32_t SensorsStart(void) +{ + // Start main task + xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &sensorsTaskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); + + return 0; +} + +MODULE_INITCALL(SensorsInitialize, SensorsStart) + +int32_t accel_test; +int32_t gyro_test; +int32_t mag_test; +//int32_t pressure_test; + + +/** + * The sensor task. This polls the gyros at 500 Hz and pumps that data to + * stabilization and to the attitude loop + */ +static void SensorsTask(void *parameters) +{ + uint8_t init = 0; + portTickType lastSysTime; + uint32_t accel_samples; + uint32_t gyro_samples; + struct pios_bma180_data accel; + struct pios_mpu6000_data gyro; + int32_t accel_accum[3] = {0, 0, 0}; + int32_t gyro_accum[3] = {0,0,0}; + float scaling; + + AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); + + accel_test = PIOS_BMA180_Test(); + gyro_test = PIOS_MPU6000_Test(); + mag_test = PIOS_HMC5883_Test(); + + if(accel_test < 0 || gyro_test < 0 || mag_test < 0) { + AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); + while(1) { + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + vTaskDelay(10); + } + } + + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { + // TODO: add timeouts to the sensor reads and set an error if the fail + + int32_t read_good; + int32_t count; + + for (int i = 0; i < 3; i++) { + accel_accum[i] = 0; + gyro_accum[i] = 0; + } + accel_samples = 0; + gyro_samples = 0; + + // Make sure we get one sample + count = 0; + while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0); + while(read_good == 0) { + count++; + + accel_accum[0] += accel.x; + accel_accum[1] += accel.y; + accel_accum[2] += accel.z; + + read_good = PIOS_BMA180_ReadFifo(&accel); + } + accel_samples = count; + + float accels[3] = {(float) accel_accum[1] / accel_samples, (float) accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples}; + + // Not the swaping of channel orders + scaling = PIOS_BMA180_GetScale(); + AccelsData accelsData; // Skip get as we set all the fields + accelsData.x = (accels[0] - accelbias[0]) * scaling; + accelsData.y = (accels[1] - accelbias[1]) * scaling; + accelsData.z = (accels[2] - accelbias[2]) * scaling; + accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f; + AccelsSet(&accelsData); + + + // Make sure we get one sample + count = 0; + while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0); + while(read_good == 0) { + count++; + + gyro_accum[0] += gyro.gyro_x; + gyro_accum[1] += gyro.gyro_y; + gyro_accum[2] += gyro.gyro_z; + + read_good = PIOS_MPU6000_ReadFifo(&gyro); + } + gyro_samples = count; + + float gyros[3] = {(float) gyro_accum[1] / gyro_samples, (float) gyro_accum[0] / gyro_samples, -(float) gyro_accum[2] / gyro_samples}; + + scaling = PIOS_MPU6000_GetScale(); + GyrosData gyrosData; // Skip get as we set all the fields + gyrosData.x = gyros[0] * scaling; + gyrosData.y = gyros[1] * scaling; + gyrosData.z = gyros[2] * scaling; + gyrosData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f; + + if (bias_correct_gyro) { + // Apply bias correction to the gyros + GyrosBiasData gyrosBias; + GyrosBiasGet(&gyrosBias); + gyrosData.x += gyrosBias.x; + gyrosData.y += gyrosBias.y; + gyrosData.z += gyrosBias.z; + } + + GyrosSet(&gyrosData); + + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + + if (PIOS_HMC5883_NewDataAvailable()) { + int16_t values[3]; + PIOS_HMC5883_ReadMag(values); + MagnetometerData mag; // Skip get as we set all the fields + mag.x = -values[0]; + mag.y = -values[1]; + mag.z = -values[2]; + MagnetometerSet(&mag); + } + + PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); + vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS); + } +} + +/** + * Locally cache some variables from the AtttitudeSettings object + */ +static void settingsUpdatedCb(UAVObjEvent * objEv) { + AttitudeSettingsData attitudeSettings; + AttitudeSettingsGet(&attitudeSettings); + + gyroGain = attitudeSettings.GyroGain; + + zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; + bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; + + accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; + accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; + accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; + + // Indicates not to expend cycles on rotation + if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && + attitudeSettings.BoardRotation[2] == 0) { + rotate = 0; + + // Shouldn't be used but to be safe + float rotationQuat[4] = {1,0,0,0}; + Quaternion2R(rotationQuat, R); + } else { + float rotationQuat[4]; + const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], + attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]}; + RPY2Quaternion(rpy, rotationQuat); + Quaternion2R(rotationQuat, R); + rotate = 1; + } +} +/** + * @} + * @} + */ diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index dbc2ef0ff..ee6cdd6c0 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -53,6 +53,8 @@ 650D8ED212DFE17500D05CC9 /* uavtalk.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavtalk.c; sourceTree = ""; }; 6512D60512ED4CA2008175E5 /* pios_flash_w25x.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_flash_w25x.h; sourceTree = ""; }; 6512D60712ED4CB8008175E5 /* pios_flash_w25x.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_flash_w25x.c; sourceTree = ""; }; + 65140DFA1496927D00E01D11 /* sensors.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = sensors.h; sourceTree = ""; }; + 65140DFB1496927D00E01D11 /* sensors.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = sensors.c; sourceTree = ""; }; 65173C9F12EBFD1700D6A7CB /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../../Makefile; sourceTree = SOURCE_ROOT; }; 651913371256C5240039C0A3 /* ahrs_comm_objects.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_comm_objects.c; sourceTree = ""; }; 651913381256C5240039C0A3 /* ahrs_spi_comm.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_spi_comm.c; sourceTree = ""; }; @@ -3434,6 +3436,7 @@ 650D8E5112DFE16400D05CC9 /* ManualControl */, 650D8E5512DFE16400D05CC9 /* MK */, 650D8E5A12DFE16400D05CC9 /* Osd */, + 65140DF81496927D00E01D11 /* Sensors */, 650D8E5D12DFE16400D05CC9 /* Stabilization */, 650D8E6112DFE16400D05CC9 /* System */, 650D8E6512DFE16400D05CC9 /* Telemetry */, @@ -3763,6 +3766,23 @@ path = inc; sourceTree = ""; }; + 65140DF81496927D00E01D11 /* Sensors */ = { + isa = PBXGroup; + children = ( + 65140DF91496927D00E01D11 /* inc */, + 65140DFB1496927D00E01D11 /* sensors.c */, + ); + path = Sensors; + sourceTree = ""; + }; + 65140DF91496927D00E01D11 /* inc */ = { + isa = PBXGroup; + children = ( + 65140DFA1496927D00E01D11 /* sensors.h */, + ); + path = inc; + sourceTree = ""; + }; 65632DF41251650300469B77 /* Boards */ = { isa = PBXGroup; children = ( diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile index a39ea9a4a..993a31813 100644 --- a/flight/Revolution/Makefile +++ b/flight/Revolution/Makefile @@ -49,7 +49,7 @@ endif FLASH_TOOL = OPENOCD # List of modules to include -MODULES = Telemetry Attitude/revolution ManualControl Stabilization Altitude/revolution Actuator GPS +MODULES = Telemetry Sensors Attitude/revolution ManualControl Stabilization Altitude/revolution Actuator GPS PYMODULES = #FlightPlan diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index b3b6e93f0..593463db0 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -33,6 +33,7 @@ UAVOBJSRCFILENAMES += inssettings UAVOBJSRCFILENAMES += insstatus UAVOBJSRCFILENAMES += attitudeactual UAVOBJSRCFILENAMES += gyros +UAVOBJSRCFILENAMES += gyrosbias UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += baroaltitude diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 5ad5f7591..41420d5c2 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -30,6 +30,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/inssettings.h \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/gyros.h \ + $$UAVOBJECT_SYNTHETICS/gyrosbias.h \ $$UAVOBJECT_SYNTHETICS/accels.h \ $$UAVOBJECT_SYNTHETICS/magnetometer.h \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ @@ -83,6 +84,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/accels.cpp \ $$UAVOBJECT_SYNTHETICS/gyros.cpp \ + $$UAVOBJECT_SYNTHETICS/gyrosbias.cpp \ $$UAVOBJECT_SYNTHETICS/magnetometer.cpp \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \ diff --git a/shared/uavobjectdefinition/gyrosbias.xml b/shared/uavobjectdefinition/gyrosbias.xml new file mode 100644 index 000000000..992610a50 --- /dev/null +++ b/shared/uavobjectdefinition/gyrosbias.xml @@ -0,0 +1,12 @@ + + + The gyro data. + + + + + + + + +