From df6bc4deff94f1f869605a89d141678c746a06ab Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 14 Mar 2012 21:52:31 -0500 Subject: [PATCH] Enable guidance on simulation and add wind model --- flight/Modules/Sensors/simulated/sensors.c | 11 ++++++++--- flight/Revolution/Makefile.osx | 1 + 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/flight/Modules/Sensors/simulated/sensors.c b/flight/Modules/Sensors/simulated/sensors.c index c10a4b1cd..60411d7f1 100644 --- a/flight/Modules/Sensors/simulated/sensors.c +++ b/flight/Modules/Sensors/simulated/sensors.c @@ -380,6 +380,11 @@ static void simulateModelQuadcopter() AttitudeActualSet(&attitudeActual); } + static float wind[3] = {0,0,0}; + wind[0] = wind[0] * 0.95 + rand_gauss(); + wind[1] = wind[1] * 0.95 + rand_gauss(); + wind[2] = wind[2] * 0.95 + rand_gauss(); + Quaternion2R(q,Rbe); // Make thrust negative as down is positive ned_accel[0] = -thrust * Rbe[2][0]; @@ -388,9 +393,9 @@ static void simulateModelQuadcopter() ned_accel[2] = -thrust * Rbe[2][2] + 9.81; // Apply acceleration based on velocity - ned_accel[0] -= K_FRICTION * vel[0]; - ned_accel[1] -= K_FRICTION * vel[1]; - ned_accel[2] -= K_FRICTION * vel[2]; + 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; diff --git a/flight/Revolution/Makefile.osx b/flight/Revolution/Makefile.osx index 177583368..a013d6fbc 100644 --- a/flight/Revolution/Makefile.osx +++ b/flight/Revolution/Makefile.osx @@ -55,6 +55,7 @@ USE_THUMB_MODE = YES # List of modules to include MODULES = Telemetry MODULES += Actuator ManualControl Stabilization +MODULES += AltitudeHold Guidance MODULES += Attitude/revolution #MODULES += SimulatedAttitude