rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

Add a new sensor simulation mode that essentially has a gyro rate that is rate

Browse Source 
desired and the accel based on the current attitude.
This commit is contained in:
James Cotton 2012-03-11 22:41:06 -05:00
parent ce54c4205d
commit f1aa3b658e
1 changed files with 122 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -59,6 +59,7 @@
#include "gpsposition.h" #include "gpsposition.h"
#include "homelocation.h" #include "homelocation.h"
#include "magnetometer.h" #include "magnetometer.h"
#include "ratedesired.h"
#include "revocalibration.h" #include "revocalibration.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
@ -77,6 +78,10 @@ static xTaskHandle sensorsTaskHandle;
// Private functions // Private functions
static void SensorsTask(void *parameters); static void SensorsTask(void *parameters);
static void simulateConstant();
static void simulateModelAgnostic();
enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC} sensor_sim_type;
/** /**
* Initialise the module. Called before the start function * Initialise the module. Called before the start function
@ -130,23 +135,40 @@ static void SensorsTask(void *parameters)
homeLocation.Set = HOMELOCATION_SET_TRUE; homeLocation.Set = HOMELOCATION_SET_TRUE;
HomeLocationSet(&homeLocation); HomeLocationSet(&homeLocation);
sensor_sim_type = MODEL_AGNOSTIC;
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
while (1) { while (1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
switch(sensor_sim_type) {
case CONSTANT:
simulateConstant();
break;
case MODEL_AGNOSTIC:
simulateModelAgnostic();
break;
}
vTaskDelay(2 / portTICK_RATE_MS);
}
}
static void simulateConstant()
{
AccelsData accelsData; // Skip get as we set all the fields AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = 0; accelsData.x = 0;
accelsData.y = -1; accelsData.y = 0;
accelsData.z = -8; accelsData.z = -9.81;
accelsData.temperature = 0; accelsData.temperature = 0;
AccelsSet(&accelsData); AccelsSet(&accelsData);
GyrosData gyrosData; // Skip get as we set all the fields GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = 2; gyrosData.x = 0;
gyrosData.y = 0; gyrosData.y = 0;
gyrosData.z = 1; gyrosData.z = 0;
// Apply bias correction to the gyros // Apply bias correction to the gyros
GyrosBiasData gyrosBias; GyrosBiasData gyrosBias;
@ -176,13 +198,68 @@ static void SensorsTask(void *parameters)
mag.y = 0; mag.y = 0;
mag.z = 800; mag.z = 800;
MagnetometerSet(&mag); MagnetometerSet(&mag);
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
vTaskDelay(20 / portTICK_RATE_MS);
} }
static void simulateModelAgnostic()
{
float Rbe[3][3];
float q[4];
// Simulate accels based on current attitude
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
Quaternion2R(q,Rbe);
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = -9.81 * Rbe[0][2];
accelsData.y = -9.81 * Rbe[1][2];
accelsData.z = -9.81 * Rbe[2][2];
accelsData.temperature = 30;
AccelsSet(&accelsData);
RateDesiredData rateDesired;
RateDesiredGet(&rateDesired);
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rateDesired.Roll;
gyrosData.y = rateDesired.Pitch;
gyrosData.z = rateDesired.Yaw;
// Apply bias correction to the gyros
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x += gyrosBias.x;
gyrosData.y += gyrosBias.y;
gyrosData.z += gyrosBias.z;
GyrosSet(&gyrosData);
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = 1;
BaroAltitudeSet(&baroAltitude);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
gpsPosition.Latitude = 0;
gpsPosition.Longitude = 0;
gpsPosition.Altitude = 0;
GPSPositionSet(&gpsPosition);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
MagnetometerData mag;
mag.x = 400;
mag.y = 0;
mag.z = 800;
MagnetometerSet(&mag);
} }
/** /**
* @} * @}
* @} * @}