rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +01:00
Code Issues Releases Activity

Fix bugs in simulated sensor data. Now works well with INSGPS.

Browse Source
This commit is contained in:
James Cotton 2012-03-14 20:03:44 -05:00
parent 84040d25b4
commit ca535c2632
1 changed files with 66 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -293,9 +293,13 @@ static void simulateModelQuadcopter()
double T[3]; double T[3];
float Rbe[3][3]; float Rbe[3][3];
const float ACTUATOR_ALPHA = 0.99; const float ACTUATOR_ALPHA = 0.8;
const float MAX_THRUST = 9.81 * 2; const float MAX_THRUST = 9.81 * 2;
const float K_FRICTION = 1; const float K_FRICTION = 1;
const float GPS_PERIOD = 0.1;
const float MAG_PERIOD = 1.0 / 75.0;
const float BARO_PERIOD = 1.0 / 20.0;
static uint32_t last_time; static uint32_t last_time;
@ -330,9 +334,9 @@ static void simulateModelQuadcopter()
RateDesiredData rateDesired; RateDesiredData rateDesired;
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
rpy[0] = rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
rpy[1] = rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
rpy[2] = rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * 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();
@ -360,6 +364,12 @@ static void simulateModelQuadcopter()
q[2] = q[2] + qdot[2]; q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3]; q[3] = q[3] + qdot[3];
float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
if(overideAttitude){ if(overideAttitude){
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
@ -371,20 +381,21 @@ static void simulateModelQuadcopter()
} }
Quaternion2R(q,Rbe); Quaternion2R(q,Rbe);
ned_accel[0] = thrust * Rbe[0][2]; // Make thrust negative as down is positive
ned_accel[1] = thrust * Rbe[1][2]; ned_accel[0] = -thrust * Rbe[2][0];
ned_accel[2] = thrust * Rbe[2][2]; ned_accel[1] = -thrust * Rbe[2][1];
ned_accel[2] -= 9.81; // Gravity causes acceleration of 9.81 in the down direction
ned_accel[2] = -thrust * Rbe[2][2] + 9.81;
// Apply acceleration based on velocity // Apply acceleration based on velocity
ned_accel[0] -= K_FRICTION * vel[0]; ned_accel[0] -= K_FRICTION * vel[0];
ned_accel[1] -= K_FRICTION * vel[1]; ned_accel[1] -= K_FRICTION * vel[1];
ned_accel[2] += K_FRICTION * vel[2]; ned_accel[2] -= K_FRICTION * vel[2];
// Predict the velocity forward in time // Predict the velocity forward in time
vel[0] = vel[0] + ned_accel[0] * dT; vel[0] = vel[0] + ned_accel[0] * dT;
vel[1] = vel[1] + ned_accel[1] * dT; vel[1] = vel[1] + ned_accel[1] * dT;
vel[2] = vel[2] - ned_accel[2] * dT; vel[2] = vel[2] + ned_accel[2] * dT;
// Predict the position forward in time // Predict the position forward in time
pos[0] = pos[0] + vel[0] * dT; pos[0] = pos[0] + vel[0] * dT;
@ -395,9 +406,12 @@ static void simulateModelQuadcopter()
if(pos[2] > 0) { if(pos[2] > 0) {
pos[2] = 0; pos[2] = 0;
vel[2] = 0; vel[2] = 0;
ned_accel[2] = -9.81; ned_accel[2] = 0;
} }
// Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
ned_accel[2] -= 9.81;
// Transform the accels back in to body frame // Transform the accels back in to body frame
AccelsData accelsData; // Skip get as we set all the fields AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2]; accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2];
@ -406,13 +420,22 @@ static void simulateModelQuadcopter()
accelsData.temperature = 30; accelsData.temperature = 30;
AccelsSet(&accelsData); AccelsSet(&accelsData);
// Update baro periodically
static uint32_t last_baro_time = 0;
if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
BaroAltitudeData baroAltitude; BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude); BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = -pos[2]; baroAltitude.Altitude = -pos[2];
BaroAltitudeSet(&baroAltitude); BaroAltitudeSet(&baroAltitude);
last_baro_time = PIOS_DELAY_GetRaw();
}
HomeLocationData homeLocation; HomeLocationData homeLocation;
HomeLocationGet(&homeLocation); HomeLocationGet(&homeLocation);
// Update GPS periodically
static uint32_t last_gps_time = 0;
if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) {
T[0] = homeLocation.Altitude+6.378137E6f * M_PI / 180; T[0] = homeLocation.Altitude+6.378137E6f * M_PI / 180;
T[1] = cosf(homeLocation.Latitude / 10e6)*(homeLocation.Altitude+6.378137E6f) * M_PI / 180; T[1] = cosf(homeLocation.Latitude / 10e6)*(homeLocation.Altitude+6.378137E6f) * M_PI / 180;
T[2] = -1.0f; T[2] = -1.0f;
@ -421,21 +444,25 @@ static void simulateModelQuadcopter()
GPSPositionGet(&gpsPosition); GPSPositionGet(&gpsPosition);
gpsPosition.Latitude = homeLocation.Latitude + (pos[0] / T[0] * 10.0e6); gpsPosition.Latitude = homeLocation.Latitude + (pos[0] / T[0] * 10.0e6);
gpsPosition.Longitude = homeLocation.Longitude + (pos[1] / T[1] * 10.0e6); gpsPosition.Longitude = homeLocation.Longitude + (pos[1] / T[1] * 10.0e6);
gpsPosition.Altitude = homeLocation.Altitude + (pos[2] / T[2] * 10.0e6); gpsPosition.Altitude = homeLocation.Altitude + (pos[2] / T[2]);
gpsPosition.Groundspeed = sqrt(vel[0] * vel[0] + vel[1] * vel[1]); gpsPosition.Groundspeed = sqrt(vel[0] * vel[0] + vel[1] * vel[1]);
gpsPosition.Heading = 180 / M_PI * atan2(vel[0], vel[1]); gpsPosition.Heading = 180 / M_PI * atan2(vel[1],vel[0]);
gpsPosition.Satellites = 7; gpsPosition.Satellites = 7;
gpsPosition.PDOP = 1; gpsPosition.PDOP = 1;
GPSPositionSet(&gpsPosition); GPSPositionSet(&gpsPosition);
last_gps_time = PIOS_DELAY_GetRaw();
}
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try // Update mag periodically
// and make it average zero (weakly) static uint32_t last_mag_time = 0;
if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
MagnetometerData mag; MagnetometerData mag;
mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
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);
last_mag_time = PIOS_DELAY_GetRaw();
}
AttitudeSimulatedData attitudeSimulated; AttitudeSimulatedData attitudeSimulated;
AttitudeSimulatedGet(&attitudeSimulated); AttitudeSimulatedGet(&attitudeSimulated);