mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
2621d07574
- Added FlighSituation Module (development module for sensor fusion, mostly stub, possibly renamed later) - Added Navigation Module (development module for navigating towards a point in space - DEVELOPMENT CODE, NOT STABLE YET (I am testing around with this)) - Changed Stabilization Module (uses local reference frame now. Stable except for code cleanup/review. Tested in simulator and outperforms old code.) git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1154 ebee16cc-31ac-478f-84a7-5cbb03baadba
531 lines
17 KiB
C
531 lines
17 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup NavigationModule Navigation Module
|
|
* @brief Feeds Stabilization with input to fly to a given coordinate in space
|
|
* @note This object updates the @ref AttitudeDesired "Attitude Desired" based on the
|
|
* comparison on the @ref NavigationDesired "Navigation Desired", @ref AttitudeActual
|
|
* "Attitude Actual" and @ref FlightSituationActual "FlightSituation Actual"
|
|
* @{
|
|
*
|
|
* @file navigation.h
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief Position stabilization module.
|
|
*
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
#include "openpilot.h"
|
|
#include "navigation.h"
|
|
#include "navigationsettings.h"
|
|
#include "navigationdesired.h"
|
|
#include "attitudeactual.h"
|
|
#include "attitudedesired.h"
|
|
#include "flightsituationactual.h"
|
|
#include "manualcontrolcommand.h"
|
|
#include "systemsettings.h"
|
|
|
|
|
|
// Private constants
|
|
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
|
|
#define DEG2RAD ( M_PI / 180.0 )
|
|
#define RAD2DEG ( 180.0 / M_PI )
|
|
#define MIN(a,b) ((a<b)?a:b)
|
|
#define MAX(a,b) ((a>b)?a:b)
|
|
#define SSQRT(x) ((x)>=0?sqrt(x):-sqrt(-x)) // signed square root
|
|
#define EARTHRAD 6378137
|
|
#define GRAVITY 9.81
|
|
|
|
// Private types
|
|
|
|
// Private variables
|
|
static xTaskHandle taskHandle;
|
|
|
|
// Private functions
|
|
static void navigationTask(void* parameters);
|
|
static float bound(float val, float min, float max);
|
|
static float angleDifference(float val);
|
|
static float fixAngle(float val,float min, float max);
|
|
static float sphereDistance(float lat1,float long1,float lat2,float long2);
|
|
static float sphereCourse(float lat1,float long1,float lat2,float long2);
|
|
|
|
/**
|
|
* Module initialization
|
|
*/
|
|
int32_t NavigationInitialize()
|
|
{
|
|
// Initialize variables
|
|
|
|
// Start main task
|
|
xTaskCreate(navigationTask, (signed char*)"Navigation", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Module task
|
|
*/
|
|
static void navigationTask(void* parameters)
|
|
{
|
|
NavigationSettingsData navSettings;
|
|
NavigationDesiredData navigationDesired;
|
|
AttitudeActualData attitudeActual;
|
|
AttitudeDesiredData attitudeDesired;
|
|
FlightSituationActualData situationActual;
|
|
ManualControlCommandData manualControl;
|
|
SystemSettingsData systemSettings;
|
|
portTickType lastSysTime;
|
|
|
|
// flight safety values
|
|
float maxPitch;
|
|
float minPitch;
|
|
float maxAccel;
|
|
|
|
// helper variables
|
|
float safeAccel;
|
|
float safeDistance;
|
|
float aoa,saoa;
|
|
|
|
// intended flight direction
|
|
float targetDistance;
|
|
float targetVspeed;
|
|
float targetTruePitch;
|
|
float targetHeading;
|
|
float targetPitch;
|
|
float targetYaw;
|
|
|
|
// turn direction
|
|
float turnDirection;
|
|
float turnSpeed;
|
|
float turnAccel;
|
|
|
|
|
|
// Main task loop
|
|
lastSysTime = xTaskGetTickCount();
|
|
while (1)
|
|
{
|
|
// Read settings and other objects
|
|
NavigationSettingsGet(&navSettings);
|
|
SystemSettingsGet(&systemSettings);
|
|
ManualControlCommandGet(&manualControl);
|
|
FlightSituationActualGet(&situationActual);
|
|
NavigationDesiredGet(&navigationDesired);
|
|
AttitudeActualGet(&attitudeActual);
|
|
|
|
// horizontal distance to waypoint
|
|
// (not 100% exact since the earth is not a perfect sphere, but
|
|
// close enough for our purpose...)
|
|
targetDistance = DEG2RAD * EARTHRAD * sphereDistance(
|
|
situationActual.Latitude,situationActual.Longitude,
|
|
navigationDesired.Latitude,navigationDesired.Longitude
|
|
);
|
|
|
|
// pitch to climb/sink to target altitude (attempts to reach
|
|
// target altitude withing SettleTime seconds
|
|
targetVspeed= (navigationDesired.Altitude - situationActual.Altitude)/navSettings.SettleTime;
|
|
if ( targetVspeed >= situationActual.Airspeed )
|
|
{
|
|
targetTruePitch = 90.0;
|
|
}
|
|
else if ( targetVspeed <= -situationActual.Airspeed )
|
|
{
|
|
targetTruePitch = -90.0;
|
|
}
|
|
else
|
|
{
|
|
targetTruePitch = RAD2DEG * asin( targetVspeed / situationActual.Airspeed );
|
|
}
|
|
|
|
// course to target coordinate
|
|
targetHeading = sphereCourse(
|
|
situationActual.Latitude,situationActual.Longitude,
|
|
navigationDesired.Latitude,navigationDesired.Longitude
|
|
);
|
|
|
|
//printf("\n\n\nTarget: Heading: %2f Distance: %2f climbing %2f°\n",targetHeading,targetDistance,targetTruePitch);
|
|
//printf("distance: %f\n",sphereDistance(situationActual.Latitude,situationActual.Longitude,navigationDesired.Latitude,navigationDesired.Longitude));
|
|
/**
|
|
* navigation for fixed wing planes
|
|
*/
|
|
if (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL)
|
|
{
|
|
|
|
/**
|
|
* Fixed wing planes create lift through their airspeed
|
|
* therefore most maneuver limits depend on the current
|
|
* speed and situation.
|
|
*/
|
|
|
|
// acceleration:
|
|
maxAccel=navSettings.AccelerationMax;
|
|
/**
|
|
* Idea: at minSpeed, the plane can JUST compensate 1g
|
|
* safely. The StallSpeed at any load factor is
|
|
* Vstall=Vs(0) / sqrt(load) so the maximum load for
|
|
* the current speed is maxLoad = (Vs(0)/V)²
|
|
*/
|
|
maxAccel = MIN( maxAccel ,
|
|
GRAVITY * (situationActual.Airspeed*situationActual.Airspeed)
|
|
/ (navSettings.SpeedMin * navSettings.SpeedMin)
|
|
);
|
|
|
|
// maximum pitch:
|
|
maxPitch=85.0;
|
|
|
|
/**
|
|
* Idea: any climb decreases speed. Aside from drag
|
|
* speed decrease is vertical speed times g (9.81m/s²)
|
|
* make sure speed stays above min speed within
|
|
* SettleTime
|
|
*/
|
|
safeAccel = ( navSettings.SpeedSafe-situationActual.Airspeed ) / navSettings.SettleTime;
|
|
if (safeAccel>=0)
|
|
{
|
|
// speed is below minimum speed. Do not allow any climbs!
|
|
maxPitch = MIN(maxPitch,0);
|
|
}
|
|
else if (safeAccel>-GRAVITY)
|
|
{
|
|
// speed is getting low. Limit maximum climb!
|
|
maxPitch = MIN(maxPitch,RAD2DEG*asin(-safeAccel/GRAVITY));
|
|
}
|
|
|
|
// minimum pitch:
|
|
minPitch=-85.0;
|
|
|
|
/**
|
|
* Idea: any dive increases speed. Aside from drag
|
|
* speed increase is vertical speed times g (9.81m/s²)
|
|
* make sure speed stays below max speed within
|
|
* SettleTime
|
|
*/
|
|
safeAccel = ( navSettings.SpeedMax-situationActual.Airspeed ) / navSettings.SettleTime;
|
|
if (safeAccel<=0)
|
|
{
|
|
// speed is above maximum speed. Do not allow any dives!
|
|
minPitch = MAX(minPitch,0);
|
|
}
|
|
else if (safeAccel<GRAVITY)
|
|
{
|
|
// speed is getting high. Limit maximum dive!
|
|
minPitch = MAX(minPitch,-1.*RAD2DEG*asin(safeAccel/GRAVITY));
|
|
}
|
|
|
|
/**
|
|
* Idea: To change pitch, acceleration must be
|
|
* applied. A pull up at maximum allowed pitch has a certain radius.
|
|
* If we are lower than that we are not allowed to
|
|
*/
|
|
safeDistance = situationActual.Airspeed * situationActual.Airspeed / (maxAccel+GRAVITY);
|
|
//printf("safe distance: %f\n",safeDistance);
|
|
if (safeDistance <= 0 || situationActual.Altitude<=navigationDesired.Altitude)
|
|
{
|
|
// altitude is below wanted altitude. Do not allow any dive at all.
|
|
minPitch = MAX(minPitch,0);
|
|
}
|
|
else if ( safeDistance >= situationActual.Altitude - navigationDesired.Altitude)
|
|
{
|
|
// altitude is close to target altitude, limit negative pitch
|
|
minPitch = MAX(minPitch,-1 * RAD2DEG * asin( (situationActual.Altitude - navigationDesired.Altitude) / safeDistance ));
|
|
}
|
|
|
|
//printf("Safety: Pitch: %2f %2f Accel: %2fm/s²\n",minPitch,maxPitch,maxAccel);
|
|
|
|
/**
|
|
* Base anted yaw and pitch on calculated angle of
|
|
* attack and side slip effects
|
|
*
|
|
* Note: This can lead to an oszillation between a
|
|
* flight with positive and negative angle of attack.
|
|
* Without this correction however, the plane will
|
|
* always "lag behind" it's desired altitude.
|
|
*/
|
|
|
|
// angle of attack in X and Y
|
|
aoa = attitudeActual.Pitch - (RAD2DEG * atan2( situationActual.Climbrate, situationActual.Groundspeed));
|
|
saoa = attitudeActual.Yaw-situationActual.Course;
|
|
|
|
targetPitch = bound(targetTruePitch+aoa,minPitch,maxPitch);
|
|
targetYaw = fixAngle(targetHeading+saoa,0.,360.);
|
|
//printf("Target Pitch: %2f Yaw: %2f \n",targetPitch,targetYaw);
|
|
//printf("Current Roll: %2f Pitch: %2f Yaw: %2f \n",attitudeActual.Roll,attitudeActual.Pitch,attitudeActual.Yaw);
|
|
|
|
/**
|
|
* Now we have the current Pitch Yaw and Roll in
|
|
* AttitudeActual and the wanted course in targetPitch
|
|
* and targetYaw. We also have the safety limits. Now
|
|
* make a smooth and save transition.
|
|
* including all safety limits! Make a nice transition.
|
|
*/
|
|
|
|
// turn vector (Euler)
|
|
/**
|
|
* The naive approach wants to fly along an Orthodrome - the quickest way to get from one angle to another
|
|
*/
|
|
turnDirection = sphereCourse(attitudeActual.Pitch,attitudeActual.Yaw,targetPitch,targetYaw);
|
|
|
|
/**
|
|
* However if the target yaw is more than 90° away, this orthodrome would lead through a minimum or maximum point that
|
|
* could be beyond safe min/max pitch values!
|
|
*/
|
|
if (fabs(angleDifference(attitudeActual.Yaw-targetYaw))>90)
|
|
{
|
|
if (fabs(turnDirection)<90)
|
|
{
|
|
/**
|
|
* Positive turns (upward) can conflict with maxPitch
|
|
*/
|
|
if (RAD2DEG*acos(sin(fabs(DEG2RAD*turnDirection))*cos(DEG2RAD*attitudeActual.Pitch))>maxPitch)
|
|
{
|
|
/**
|
|
* In this case the turn has to be adjusted to a circle that won't conflict with maxPitch
|
|
*/
|
|
//printf("maximum is %2f which is higher than %2f\n",RAD2DEG*acos(sin(DEG2RAD*turnDirection)*cos(DEG2RAD*attitudeActual.Pitch)),maxPitch);
|
|
if (cos(DEG2RAD*attitudeActual.Pitch)>cos(DEG2RAD*maxPitch))
|
|
{
|
|
if (turnDirection>0)
|
|
{
|
|
turnDirection = RAD2DEG * asin( cos(DEG2RAD*maxPitch)/cos(DEG2RAD*attitudeActual.Pitch));
|
|
}
|
|
else
|
|
{
|
|
turnDirection = -RAD2DEG * asin( cos(DEG2RAD*maxPitch)/cos(DEG2RAD*attitudeActual.Pitch));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/**
|
|
* something went wrong (attitude outside safe bounds?)- compensate safely
|
|
*/
|
|
turnDirection=(turnDirection>0?110:-110);
|
|
}
|
|
//printf("limiting because of max!\n");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/**
|
|
* Downward turns can conflict with minPitch
|
|
*/
|
|
if (RAD2DEG*acos(sin(fabs(DEG2RAD*turnDirection))*cos(DEG2RAD*attitudeActual.Pitch))>-minPitch)
|
|
{
|
|
//printf("minimum is %2f which is less than %2f\n",-1*RAD2DEG*acos(sin(fabs(DEG2RAD*turnDirection))*cos(DEG2RAD*attitudeActual.Pitch)),minPitch);
|
|
/**
|
|
* In this case the turn has to be adjusted to a circle that won't conflict with minPitch
|
|
*/
|
|
if (cos(DEG2RAD*attitudeActual.Pitch)>cos(DEG2RAD*minPitch))
|
|
{
|
|
if (turnDirection>0) {
|
|
turnDirection = 180 - RAD2DEG * asin( cos(DEG2RAD*minPitch)/cos(DEG2RAD*attitudeActual.Pitch));
|
|
}
|
|
else
|
|
{
|
|
turnDirection = -180 + RAD2DEG * asin( cos(DEG2RAD*minPitch)/cos(DEG2RAD*attitudeActual.Pitch));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/**
|
|
* something went wrong (attitude outside safe bounds?)- compensate safely
|
|
*/
|
|
turnDirection=(turnDirection>0?45:-45);
|
|
}
|
|
//printf("limiting because of min!\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Turn speed in °/s is the minimum of
|
|
* TurnSpeedFactor * degrees to turn
|
|
* and
|
|
* the maximum angular velocity achievable with
|
|
* less than maxAccel centripetal acceleration
|
|
* (taking gravity effect into account)
|
|
* but not less than 0
|
|
* Formula: acceleration = (DEG2RAD*turnSpeed)
|
|
* * airspeed
|
|
*/
|
|
turnSpeed = MAX(0,MIN(
|
|
navSettings.TurnSpeedFactor * sphereDistance(
|
|
attitudeActual.Pitch,attitudeActual.Yaw,
|
|
targetPitch,targetYaw)
|
|
,
|
|
RAD2DEG * (maxAccel-( MIN(0,cos(DEG2RAD*turnDirection))*GRAVITY )) / situationActual.Airspeed
|
|
));
|
|
//printf("turn distance is %2f °\n", sphereDistance(attitudeActual.Pitch,attitudeActual.Yaw,targetPitch,targetYaw));
|
|
//printf("safe speed is %2f °/s\n",RAD2DEG * (maxAccel-( MIN(0,cos(DEG2RAD*turnDirection))*GRAVITY )) / situationActual.Airspeed);
|
|
//printf("turn speed is %2f °/s\n",turnSpeed);
|
|
//printf("Airspeed is %2f °/s\n",situationActual.Airspeed);
|
|
|
|
/**
|
|
* Compute wanted centripetal acceleration
|
|
*/
|
|
turnAccel = DEG2RAD* turnSpeed * situationActual.Airspeed;
|
|
//printf("Turn with: %2f at %2fm/s²\n",turnDirection,turnAccel);
|
|
|
|
|
|
// Desired Attitude
|
|
/**
|
|
* Now that we know which way the plane is supposed to
|
|
* go and at what acceleration, calculate the required
|
|
* roll angle to also compensate gravity during the
|
|
* turn
|
|
*/
|
|
attitudeDesired.Roll = RAD2DEG*atan2(
|
|
turnAccel * sin(DEG2RAD*turnDirection),
|
|
turnAccel * cos(DEG2RAD*turnDirection) + cos(DEG2RAD*attitudeActual.Pitch)*GRAVITY
|
|
);
|
|
/**
|
|
* However this can also result in an upright flight
|
|
* with a load of less than 1g
|
|
* In this case, the effect on yaw is reversed.
|
|
* (Imagine rolling 45° right, then push the stick
|
|
* slightly. You will go left and down!)
|
|
* In this case we reverse the roll to descent into the
|
|
* right direction!
|
|
*/
|
|
if (fabs(angleDifference(attitudeDesired.Roll-turnDirection))>90) {
|
|
attitudeDesired.Roll=-attitudeDesired.Roll;
|
|
}
|
|
//printf("FINALS: roll %2f ",attitudeDesired.Roll);
|
|
|
|
/**
|
|
* The desired Yaw and Pitch values are tricky.
|
|
* The current Stabilization module doesn't allow us to
|
|
* specify an angular speed or even centripetal
|
|
* acceleration instead we have to specify an
|
|
* orientation.
|
|
* According to tests, a formula of
|
|
* sqrt(turnAccel) * StabilizationForceFactor
|
|
* degrees lookahead gives reasonable results.
|
|
*/
|
|
attitudeDesired.Pitch = bound(
|
|
attitudeActual.Pitch + navSettings.StabilizationForceFactor * SSQRT(turnAccel * cos(DEG2RAD*turnDirection)),
|
|
minPitch,
|
|
maxPitch);
|
|
attitudeDesired.Yaw = attitudeActual.Yaw + navSettings.StabilizationForceFactor * SSQRT(turnAccel * sin(DEG2RAD*turnDirection)) ;
|
|
//printf("pitch %2f yaw %2f\n",attitudeDesired.Pitch-attitudeActual.Pitch,attitudeDesired.Yaw-attitudeActual.Yaw);
|
|
|
|
|
|
/**
|
|
* TODO: Make a real PID loop for the throttle.
|
|
*/
|
|
attitudeDesired.Throttle=0.8;
|
|
|
|
|
|
}
|
|
|
|
// Write actuator desired (if not in manual mode)
|
|
if ( manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
|
|
{
|
|
AttitudeDesiredSet(&attitudeDesired);
|
|
}
|
|
|
|
// Wait until next update
|
|
vTaskDelayUntil(&lastSysTime, navSettings.UpdatePeriod / portTICK_RATE_MS );
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Bound input value between limits
|
|
*/
|
|
static float bound(float val, float min, float max)
|
|
{
|
|
if ( val < min )
|
|
{
|
|
val = min;
|
|
}
|
|
else if ( val > max )
|
|
{
|
|
val = max;
|
|
}
|
|
return val;
|
|
}
|
|
|
|
/**
|
|
* Fix result of angular differences
|
|
*/
|
|
static float angleDifference(float val)
|
|
{
|
|
while ( val < -180.0 )
|
|
{
|
|
val += 360.0;
|
|
}
|
|
while ( val > 180.0 )
|
|
{
|
|
val -= 360.0;
|
|
}
|
|
return val;
|
|
}
|
|
|
|
/**
|
|
* Make sure an angular value stays between correct bounds
|
|
*/
|
|
static float fixAngle(float val,float min, float max)
|
|
{
|
|
while ( val < min )
|
|
{
|
|
val += 360.0;
|
|
}
|
|
while ( val >= max )
|
|
{
|
|
val -= 360.0;
|
|
}
|
|
return val;
|
|
}
|
|
|
|
/**
|
|
* calculate spherical distance and course between two coordinate pairs
|
|
* see http://de.wikipedia.org/wiki/Orthodrome for details
|
|
*/
|
|
static float sphereDistance(float lat1, float long1, float lat2, float long2)
|
|
{
|
|
float zeta=(RAD2DEG * acos (
|
|
sin(DEG2RAD*lat1) * sin(DEG2RAD*lat2)
|
|
+ cos(DEG2RAD*lat1) * cos(DEG2RAD*lat2) * cos(DEG2RAD*(long2-long1))
|
|
));
|
|
if (isnan(zeta)) {
|
|
zeta=0;
|
|
}
|
|
return zeta;
|
|
|
|
}
|
|
static float sphereCourse(float lat1, float long1, float lat2, float long2)
|
|
{
|
|
float zeta = sphereDistance(lat1,long1,lat2,long2);
|
|
float angle = RAD2DEG * acos(
|
|
( sin(DEG2RAD*lat2) - sin(DEG2RAD*lat1) * cos(DEG2RAD*zeta) )
|
|
/ ( cos(DEG2RAD*lat1) * sin(DEG2RAD*zeta) )
|
|
);
|
|
if (isnan(angle)) angle=0;
|
|
if (angleDifference(long2-long1)>=0) {
|
|
return angle;
|
|
} else {
|
|
return -angle;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|