diff --git a/flight/Modules/Sensors/simulated/sensors.c b/flight/Modules/Sensors/simulated/sensors.c index b9e830dbb..e42ce2ce6 100644 --- a/flight/Modules/Sensors/simulated/sensors.c +++ b/flight/Modules/Sensors/simulated/sensors.c @@ -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;