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

Created a separate Sensor module and Attitude module for revolution

Browse Source
This commit is contained in:
James Cotton 2011-12-12 14:39:38 -06:00
parent 53cb5b67d1
commit a37a17a4fb
8 changed files with 461 additions and 233 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

309
flight/Modules/Attitude/revolution/attitude.c
View File

@ -53,6 +53,7 @@
#include "magnetometer.h" #include "magnetometer.h"
#include "accels.h" #include "accels.h"
#include "gyros.h" #include "gyros.h"
#include "gyrosbias.h"
#include "attitudeactual.h" #include "attitudeactual.h"
#include "attitudesettings.h" #include "attitudesettings.h"
#include "flightstatus.h" #include "flightstatus.h"
@ -61,6 +62,7 @@
// Private constants // Private constants
#define STACK_SIZE_BYTES 1540 #define STACK_SIZE_BYTES 1540
#define TASK_PRIORITY (tskIDLE_PRIORITY+3) #define TASK_PRIORITY (tskIDLE_PRIORITY+3)
#define FAILSAFE_TIMEOUT_MS 10
#define F_PI 3.14159265358979323846f #define F_PI 3.14159265358979323846f
#define PI_MOD(x) (fmod(x + F_PI, F_PI * 2) - F_PI) #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 void AttitudeTask(void *parameters);
static int32_t updateSensors(); static int32_t updateSensors();
static int32_t updateAttitudeComplimentary(); static int32_t updateAttitudeComplimentary(bool first_run);
static void settingsUpdatedCb(UAVObjEvent * objEv); static void settingsUpdatedCb(UAVObjEvent * objEv);
static float accelKi = 0; static float accelKi = 0;
@ -92,9 +94,6 @@ static float R[3][3];
static int8_t rotate = 0; static int8_t rotate = 0;
static bool zero_during_arming = false; 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: * 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 * 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 * \returns 0 on success or -1 if initialisation failed
*/ */
int32_t AttitudeInitialize(void) int32_t AttitudeInitialize(void)
{ {
AttitudeActualInitialize(); AttitudeActualInitialize();
GyrosInitialize();
AccelsInitialize();
MagnetometerInitialize();
AttitudeSettingsInitialize(); AttitudeSettingsInitialize();
// Initialize quaternion // Initialize quaternion
@ -148,9 +123,12 @@ int32_t AttitudeInitialize(void)
AttitudeActualSet(&attitude); AttitudeActualSet(&attitude);
// Cannot trust the values to init right above if BL runs // Cannot trust the values to init right above if BL runs
gyro_bias[0] = 0; GyrosBiasData gyrosBias;
gyro_bias[1] = 0; GyrosBiasGet(&gyrosBias);
gyro_bias[2] = 0; gyrosBias.x = 0;
gyrosBias.y = 0;
gyrosBias.z = 0;
GyrosBiasSet(&gyrosBias);
for(uint8_t i = 0; i < 3; i++) for(uint8_t i = 0; i < 3; i++)
for(uint8_t j = 0; j < 3; j++) for(uint8_t j = 0; j < 3; j++)
@ -161,52 +139,82 @@ int32_t AttitudeInitialize(void)
return 0; 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) 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 * Module thread, should not return.
* stabilization and to the attitude loop
*/ */
static void SensorTask(void *parameters) static void AttitudeTask(void *parameters)
{ {
uint8_t init = 0; AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
portTickType lastSysTime;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); // Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
accel_test = PIOS_BMA180_Test(); bool first_run = true;
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 // Main task loop
lastSysTime = xTaskGetTickCount();
while (1) { while (1) {
// TODO: This initialization stuff should be refactored from here // This function blocks on data queue
updateAttitudeComplimentary(first_run);
if (first_run)
first_run = false;
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
}
float accel_mag;
float qmag;
static int32_t updateAttitudeComplimentary(bool first_run)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
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; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
if(first_run)
init = 0;
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias // For first 7 seconds use accels to get gyro bias
accelKp = 1; accelKp = 1;
accelKi = 0.9; accelKi = 0.9;
yawBiasRate = 0.23; yawBiasRate = 0.23;
init = 0; } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1; accelKp = 1;
accelKi = 0.9; accelKi = 0.9;
yawBiasRate = 0.23; yawBiasRate = 0.23;
@ -219,165 +227,11 @@ static void SensorTask(void *parameters)
init = 1; init = 1;
} }
GyrosGet(&gyrosData);
AccelsGet(&accelsData);
if(updateSensors() != 0) // Compute the dT using the cpu clock
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
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());
// Main task loop
while (1) {
// This function blocks on data queue
updateAttitudeComplimentary();
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()
{
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);
return -1;
}
static int32_t timeval;
float dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw(); timeval = PIOS_DELAY_GetRaw();
float q[4]; float q[4];
@ -405,15 +259,18 @@ static int32_t updateAttitudeComplimentary()
accel_err[2] /= accel_mag; accel_err[2] /= accel_mag;
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s // 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; GyrosBiasData gyrosBias;
gyro_bias[1] += accel_err[1] * accelKi; 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 // Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * accelKp / dT; gyrosData.x += accel_err[0] * accelKp / dT;
gyrosData.y += accel_err[1] * accelKp / dT; gyrosData.y += accel_err[1] * accelKp / dT;
gyrosData.z += accel_err[2] * accelKp / dT; gyrosData.z += accel_err[2] * accelKp / dT;
// Work out time derivative from INSAlgo writeup // Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s // Also accounts for the fact that gyros are in deg/s
float qdot[4]; float qdot[4];
@ -474,15 +331,17 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
gyroGain = attitudeSettings.GyroGain; gyroGain = attitudeSettings.GyroGain;
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
gyro_bias[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f; GyrosBiasData gyrosBias;
gyro_bias[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f; GyrosBiasGet(&gyrosBias);
gyro_bias[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f; 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 // Indicates not to expend cycles on rotation
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&

37
flight/Modules/Sensors/inc/sensors.h Normal file
View File

@ -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

297
flight/Modules/Sensors/sensors.c Normal file
View File

@ -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;
}
}
/**
* @}
* @}
*/

20
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -53,6 +53,8 @@
650D8ED212DFE17500D05CC9 /* uavtalk.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavtalk.c; sourceTree = "<group>"; }; 650D8ED212DFE17500D05CC9 /* uavtalk.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavtalk.c; sourceTree = "<group>"; };
6512D60512ED4CA2008175E5 /* pios_flash_w25x.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_flash_w25x.h; sourceTree = "<group>"; }; 6512D60512ED4CA2008175E5 /* pios_flash_w25x.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_flash_w25x.h; sourceTree = "<group>"; };
6512D60712ED4CB8008175E5 /* pios_flash_w25x.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_flash_w25x.c; sourceTree = "<group>"; }; 6512D60712ED4CB8008175E5 /* pios_flash_w25x.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_flash_w25x.c; sourceTree = "<group>"; };
65140DFA1496927D00E01D11 /* sensors.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = sensors.h; sourceTree = "<group>"; };
65140DFB1496927D00E01D11 /* sensors.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = sensors.c; sourceTree = "<group>"; };
65173C9F12EBFD1700D6A7CB /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../../Makefile; sourceTree = SOURCE_ROOT; }; 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 = "<group>"; }; 651913371256C5240039C0A3 /* ahrs_comm_objects.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_comm_objects.c; sourceTree = "<group>"; };
651913381256C5240039C0A3 /* ahrs_spi_comm.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_spi_comm.c; sourceTree = "<group>"; }; 651913381256C5240039C0A3 /* ahrs_spi_comm.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = ahrs_spi_comm.c; sourceTree = "<group>"; };
@ -3434,6 +3436,7 @@
650D8E5112DFE16400D05CC9 /* ManualControl */, 650D8E5112DFE16400D05CC9 /* ManualControl */,
650D8E5512DFE16400D05CC9 /* MK */, 650D8E5512DFE16400D05CC9 /* MK */,
650D8E5A12DFE16400D05CC9 /* Osd */, 650D8E5A12DFE16400D05CC9 /* Osd */,
65140DF81496927D00E01D11 /* Sensors */,
650D8E5D12DFE16400D05CC9 /* Stabilization */, 650D8E5D12DFE16400D05CC9 /* Stabilization */,
650D8E6112DFE16400D05CC9 /* System */, 650D8E6112DFE16400D05CC9 /* System */,
650D8E6512DFE16400D05CC9 /* Telemetry */, 650D8E6512DFE16400D05CC9 /* Telemetry */,
@ -3763,6 +3766,23 @@
path = inc; path = inc;
sourceTree = "<group>"; sourceTree = "<group>";
}; };
65140DF81496927D00E01D11 /* Sensors */ = {
isa = PBXGroup;
children = (
65140DF91496927D00E01D11 /* inc */,
65140DFB1496927D00E01D11 /* sensors.c */,
);
path = Sensors;
sourceTree = "<group>";
};
65140DF91496927D00E01D11 /* inc */ = {
isa = PBXGroup;
children = (
65140DFA1496927D00E01D11 /* sensors.h */,
);
path = inc;
sourceTree = "<group>";
};
65632DF41251650300469B77 /* Boards */ = { 65632DF41251650300469B77 /* Boards */ = {
isa = PBXGroup; isa = PBXGroup;
children = ( children = (

2
flight/Revolution/Makefile
View File

@ -49,7 +49,7 @@ endif
FLASH_TOOL = OPENOCD FLASH_TOOL = OPENOCD
# List of modules to include # 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 = PYMODULES =
#FlightPlan #FlightPlan

1
flight/Revolution/UAVObjects.inc
View File

@ -33,6 +33,7 @@ UAVOBJSRCFILENAMES += inssettings
UAVOBJSRCFILENAMES += insstatus UAVOBJSRCFILENAMES += insstatus
UAVOBJSRCFILENAMES += attitudeactual UAVOBJSRCFILENAMES += attitudeactual
UAVOBJSRCFILENAMES += gyros UAVOBJSRCFILENAMES += gyros
UAVOBJSRCFILENAMES += gyrosbias
UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += accels
UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += magnetometer
UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += baroaltitude

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

@ -30,6 +30,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/inssettings.h \ $$UAVOBJECT_SYNTHETICS/inssettings.h \
$$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \
$$UAVOBJECT_SYNTHETICS/gyros.h \ $$UAVOBJECT_SYNTHETICS/gyros.h \
$$UAVOBJECT_SYNTHETICS/gyrosbias.h \
$$UAVOBJECT_SYNTHETICS/accels.h \ $$UAVOBJECT_SYNTHETICS/accels.h \
$$UAVOBJECT_SYNTHETICS/magnetometer.h \ $$UAVOBJECT_SYNTHETICS/magnetometer.h \
$$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \
@ -83,6 +84,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \
$$UAVOBJECT_SYNTHETICS/accels.cpp \ $$UAVOBJECT_SYNTHETICS/accels.cpp \
$$UAVOBJECT_SYNTHETICS/gyros.cpp \ $$UAVOBJECT_SYNTHETICS/gyros.cpp \
$$UAVOBJECT_SYNTHETICS/gyrosbias.cpp \
$$UAVOBJECT_SYNTHETICS/magnetometer.cpp \ $$UAVOBJECT_SYNTHETICS/magnetometer.cpp \
$$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \
$$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \

12
shared/uavobjectdefinition/gyrosbias.xml Normal file
View File

@ -0,0 +1,12 @@
<xml>
<object name="GyrosBias" singleinstance="true" settings="false">
<description>The gyro data.</description>
<field name="x" units="deg/s" type="float" elements="1"/>
<field name="y" units="deg/s" type="float" elements="1"/>
<field name="z" units="deg/s" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="never" period="0"/>
</object>
</xml>