mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Add a fixed wing simulator mode
This commit is contained in:
James Cotton
parent
463ad550e2
commit
493864bdc4
@ -84,13 +84,15 @@ static void SensorsTask(void *parameters);
|
||||
static void simulateConstant();
|
||||
static void simulateModelAgnostic();
|
||||
static void simulateModelQuadcopter();
|
||||
static void simulateModelAirplane();
|
||||
|
||||
static float accel_bias[3];
|
||||
|
||||
static float rand_gauss();
|
||||
|
||||
enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER} sensor_sim_type;
|
||||
enum sensor_sim_type {CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER, MODEL_AIRPLANE} sensor_sim_type;
|
||||
|
||||
#define GRAV 9.81
|
||||
/**
|
||||
* Initialise the module. Called before the start function
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
@ -152,7 +154,7 @@ static void SensorsTask(void *parameters)
|
||||
// homeLocation.Set = HOMELOCATION_SET_TRUE;
|
||||
// HomeLocationSet(&homeLocation);
|
||||
|
||||
sensor_sim_type = MODEL_QUADCOPTER;
|
||||
sensor_sim_type = MODEL_AIRPLANE;
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
@ -180,6 +182,8 @@ static void SensorsTask(void *parameters)
|
||||
case MODEL_QUADCOPTER:
|
||||
simulateModelQuadcopter();
|
||||
break;
|
||||
case MODEL_AIRPLANE:
|
||||
simulateModelAirplane();
|
||||
}
|
||||
|
||||
vTaskDelay(2 / portTICK_RATE_MS);
|
||||
@ -192,7 +196,7 @@ static void simulateConstant()
|
||||
AccelsData accelsData; // Skip get as we set all the fields
|
||||
accelsData.x = 0;
|
||||
accelsData.y = 0;
|
||||
accelsData.z = -9.81;
|
||||
accelsData.z = -GRAV;
|
||||
accelsData.temperature = 0;
|
||||
AccelsSet(&accelsData);
|
||||
|
||||
@ -246,9 +250,9 @@ static void simulateModelAgnostic()
|
||||
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.x = -GRAV * Rbe[0][2];
|
||||
accelsData.y = -GRAV * Rbe[1][2];
|
||||
accelsData.z = -GRAV * Rbe[2][2];
|
||||
accelsData.temperature = 30;
|
||||
AccelsSet(&accelsData);
|
||||
|
||||
@ -303,7 +307,7 @@ static void simulateModelQuadcopter()
|
||||
float Rbe[3][3];
|
||||
|
||||
const float ACTUATOR_ALPHA = 0.8;
|
||||
const float MAX_THRUST = 9.81 * 2;
|
||||
const float MAX_THRUST = GRAV * 2;
|
||||
const float K_FRICTION = 1;
|
||||
const float GPS_PERIOD = 0.1;
|
||||
const float MAG_PERIOD = 1.0 / 75.0;
|
||||
@ -391,7 +395,7 @@ static void simulateModelQuadcopter()
|
||||
ned_accel[0] = -thrust * Rbe[2][0];
|
||||
ned_accel[1] = -thrust * Rbe[2][1];
|
||||
// Gravity causes acceleration of 9.81 in the down direction
|
||||
ned_accel[2] = -thrust * Rbe[2][2] + 9.81;
|
||||
ned_accel[2] = -thrust * Rbe[2][2] + GRAV;
|
||||
|
||||
// Apply acceleration based on velocity
|
||||
ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]);
|
||||
@ -517,6 +521,251 @@ static void simulateModelQuadcopter()
|
||||
AttitudeSimulatedSet(&attitudeSimulated);
|
||||
}
|
||||
|
||||
/**
|
||||
* This method performs a simple simulation of a quadcopter
|
||||
*
|
||||
* It takes in the ActuatorDesired command to rotate the aircraft and performs
|
||||
* a simple kinetic model where the throttle increases the energy and drag decreases
|
||||
* it. Changing altitude moves energy from kinetic to potential.
|
||||
*
|
||||
* 1. Update attitude based on ActuatorDesired
|
||||
* 2. Update position based on velocity
|
||||
*/
|
||||
static void simulateModelAirplane()
|
||||
{
|
||||
static double pos[3] = {0,0,0};
|
||||
static double vel[3] = {0,0,0};
|
||||
static double ned_accel[3] = {0,0,0};
|
||||
static float q[4] = {1,0,0,0};
|
||||
static float rpy[3] = {0,0,0}; // Low pass filtered actuator
|
||||
static float baro_offset = 0.0f;
|
||||
float Rbe[3][3];
|
||||
|
||||
const float ACTUATOR_ALPHA = 0.8;
|
||||
const float MAX_THRUST = 9.81 * 2;
|
||||
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;
|
||||
|
||||
float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6);
|
||||
if(dT < 1e-3)
|
||||
dT = 2e-3;
|
||||
last_time = PIOS_DELAY_GetRaw();
|
||||
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
ActuatorDesiredData actuatorDesired;
|
||||
ActuatorDesiredGet(&actuatorDesired);
|
||||
|
||||
float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0;
|
||||
if (thrust < 0)
|
||||
thrust = 0;
|
||||
|
||||
if (thrust != thrust)
|
||||
thrust = 0;
|
||||
|
||||
// float control_scaling = thrust * thrustToDegs;
|
||||
// // In rad/s
|
||||
// rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
|
||||
// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
|
||||
// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
|
||||
//
|
||||
// GyrosData gyrosData; // Skip get as we set all the fields
|
||||
// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
|
||||
// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
|
||||
// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
|
||||
|
||||
/**** 1. Update attitude ****/
|
||||
RateDesiredData rateDesired;
|
||||
RateDesiredGet(&rateDesired);
|
||||
|
||||
rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
|
||||
rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * 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.x = rpy[0] + rand_gauss();
|
||||
gyrosData.y = rpy[1] + rand_gauss();
|
||||
gyrosData.z = rpy[2] + rand_gauss();
|
||||
GyrosSet(&gyrosData);
|
||||
|
||||
// Predict the attitude forward in time
|
||||
float qdot[4];
|
||||
qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * M_PI / 180 / 2;
|
||||
qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * M_PI / 180 / 2;
|
||||
|
||||
// Take a time step
|
||||
q[0] = q[0] + qdot[0];
|
||||
q[1] = q[1] + qdot[1];
|
||||
q[2] = q[2] + qdot[2];
|
||||
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){
|
||||
AttitudeActualData attitudeActual;
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
attitudeActual.q1 = q[0];
|
||||
attitudeActual.q2 = q[1];
|
||||
attitudeActual.q3 = q[2];
|
||||
attitudeActual.q4 = q[3];
|
||||
AttitudeActualSet(&attitudeActual);
|
||||
}
|
||||
|
||||
/**** 2. Update position based on velocity ****/
|
||||
static float wind[3] = {0,0,0};
|
||||
wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0;
|
||||
wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0;
|
||||
wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0;
|
||||
|
||||
/* Compute aerodynamic forces in body referenced frame. Later use more sophisticated equations */
|
||||
/* TODO: This should become more accurate. Use the force equations to calculate lift from the */
|
||||
/* various surfaces based on AoA and airspeed. From that compute torques and forces. For later */
|
||||
double forces[3]; // X, Y, Z
|
||||
forces[0] = thrust; // Friction is applied in all directions in NED
|
||||
forces[1] = 0; // No side slip
|
||||
forces[2] = GRAV; // Stupidly simple, always have gravity lift when straight and level
|
||||
|
||||
Quaternion2R(q,Rbe);
|
||||
// Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?)
|
||||
ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0];
|
||||
ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1];
|
||||
ned_accel[2] = forces[0] * Rbe[0][2] + forces[1] * Rbe[1][2] - forces[2] * Rbe[2][2];
|
||||
// Gravity causes acceleration of 9.81 in the down direction
|
||||
ned_accel[2] += 9.81;
|
||||
|
||||
// Apply acceleration based on velocity
|
||||
ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]);
|
||||
ned_accel[1] -= K_FRICTION * (vel[1] - wind[0]);
|
||||
ned_accel[2] -= K_FRICTION * (vel[2] - wind[0]);
|
||||
|
||||
// Predict the velocity forward in time
|
||||
vel[0] = vel[0] + ned_accel[0] * dT;
|
||||
vel[1] = vel[1] + ned_accel[1] * dT;
|
||||
vel[2] = vel[2] + ned_accel[2] * dT;
|
||||
|
||||
// Predict the position forward in time
|
||||
pos[0] = pos[0] + vel[0] * dT;
|
||||
pos[1] = pos[1] + vel[1] * dT;
|
||||
pos[2] = pos[2] + vel[2] * dT;
|
||||
|
||||
// Simulate hitting ground
|
||||
if(pos[2] > 0) {
|
||||
pos[2] = 0;
|
||||
vel[2] = 0;
|
||||
ned_accel[2] = 0;
|
||||
}
|
||||
|
||||
// Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
|
||||
ned_accel[2] -= GRAV;
|
||||
|
||||
// Transform the accels back in to body frame
|
||||
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] + accel_bias[0];
|
||||
accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
|
||||
accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
|
||||
accelsData.temperature = 30;
|
||||
AccelsSet(&accelsData);
|
||||
|
||||
if(baro_offset == 0) {
|
||||
// Hacky initialization
|
||||
baro_offset = 50;// * rand_gauss();
|
||||
} else {
|
||||
// Very small drift process
|
||||
baro_offset += rand_gauss() / 100;
|
||||
}
|
||||
// Update baro periodically
|
||||
static uint32_t last_baro_time = 0;
|
||||
if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
|
||||
BaroAltitudeData baroAltitude;
|
||||
BaroAltitudeGet(&baroAltitude);
|
||||
baroAltitude.Altitude = -pos[2] + baro_offset;
|
||||
BaroAltitudeSet(&baroAltitude);
|
||||
last_baro_time = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
|
||||
HomeLocationData homeLocation;
|
||||
HomeLocationGet(&homeLocation);
|
||||
|
||||
static float gps_vel_drift[3] = {0,0,0};
|
||||
gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
|
||||
gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
|
||||
gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
|
||||
|
||||
// Update GPS periodically
|
||||
static uint32_t last_gps_time = 0;
|
||||
if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) {
|
||||
// Use double precision here as simulating what GPS produces
|
||||
double T[3];
|
||||
T[0] = homeLocation.Altitude+6.378137E6f * M_PI / 180.0;
|
||||
T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f)*(homeLocation.Altitude+6.378137E6) * M_PI / 180.0;
|
||||
T[2] = -1.0;
|
||||
|
||||
static float gps_drift[3] = {0,0,0};
|
||||
gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
|
||||
gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
|
||||
gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
|
||||
|
||||
GPSPositionData gpsPosition;
|
||||
GPSPositionGet(&gpsPosition);
|
||||
gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
|
||||
gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
|
||||
gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
|
||||
gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
|
||||
gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
|
||||
gpsPosition.Satellites = 7;
|
||||
gpsPosition.PDOP = 1;
|
||||
GPSPositionSet(&gpsPosition);
|
||||
last_gps_time = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
|
||||
// Update GPS Velocity measurements
|
||||
static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
|
||||
if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
|
||||
GPSVelocityData gpsVelocity;
|
||||
GPSVelocityGet(&gpsVelocity);
|
||||
gpsVelocity.North = vel[0] + gps_vel_drift[0];
|
||||
gpsVelocity.East = vel[1] + gps_vel_drift[1];
|
||||
gpsVelocity.Down = vel[2] + gps_vel_drift[2];
|
||||
GPSVelocitySet(&gpsVelocity);
|
||||
last_gps_vel_time = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
|
||||
// Update mag periodically
|
||||
static uint32_t last_mag_time = 0;
|
||||
if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
|
||||
MagnetometerData mag;
|
||||
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.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
|
||||
MagnetometerSet(&mag);
|
||||
last_mag_time = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
|
||||
AttitudeSimulatedData attitudeSimulated;
|
||||
AttitudeSimulatedGet(&attitudeSimulated);
|
||||
attitudeSimulated.q1 = q[0];
|
||||
attitudeSimulated.q2 = q[1];
|
||||
attitudeSimulated.q3 = q[2];
|
||||
attitudeSimulated.q4 = q[3];
|
||||
Quaternion2RPY(q,&attitudeSimulated.Roll);
|
||||
attitudeSimulated.Position[0] = pos[0];
|
||||
attitudeSimulated.Position[1] = pos[1];
|
||||
attitudeSimulated.Position[2] = pos[2];
|
||||
attitudeSimulated.Velocity[0] = vel[0];
|
||||
attitudeSimulated.Velocity[1] = vel[1];
|
||||
attitudeSimulated.Velocity[2] = vel[2];
|
||||
AttitudeSimulatedSet(&attitudeSimulated);
|
||||
}
|
||||
|
||||
static float rand_gauss (void) {
|
||||
float v1,v2,s;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user