mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-30 08:24:11 +01:00
Merge branch 'next' into laurent/OP-1337_French_translations_updates
This commit is contained in:
Laurent Lalanne
commit
94150fc2d3
@ -75,5 +75,27 @@ void quat_inverse(float q[4]);
|
||||
void quat_copy(const float q[4], float qnew[4]);
|
||||
void quat_mult(const float q1[4], const float q2[4], float qout[4]);
|
||||
void rot_mult(float R[3][3], const float vec[3], float vec_out[3]);
|
||||
/**
|
||||
* matrix_mult_3x3f - perform a multiplication between two 3x3 float matrices
|
||||
* result = a*b
|
||||
* @param a
|
||||
* @param b
|
||||
* @param result
|
||||
*/
|
||||
inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
|
||||
{
|
||||
result[0][0] = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2];
|
||||
result[0][1] = a[0][1] * b[0][0] + a[1][1] * b[0][1] + a[2][1] * b[0][2];
|
||||
result[0][2] = a[0][2] * b[0][0] + a[1][2] * b[0][1] + a[2][2] * b[0][2];
|
||||
|
||||
result[1][0] = a[0][0] * b[1][0] + a[1][0] * b[1][1] + a[2][0] * b[1][2];
|
||||
result[1][1] = a[0][1] * b[1][0] + a[1][1] * b[1][1] + a[2][1] * b[1][2];
|
||||
result[1][2] = a[0][2] * b[1][0] + a[1][2] * b[1][1] + a[2][2] * b[1][2];
|
||||
|
||||
result[2][0] = a[0][0] * b[2][0] + a[1][0] * b[2][1] + a[2][0] * b[2][2];
|
||||
result[2][1] = a[0][1] * b[2][0] + a[1][1] * b[2][1] + a[2][1] * b[2][2];
|
||||
result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
|
||||
}
|
||||
|
||||
|
||||
#endif // COORDINATECONVERSIONS_H_
|
||||
|
||||
69
flight/libraries/inc/plans.h
Normal file
69
flight/libraries/inc/plans.h
Normal file
@ -0,0 +1,69 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotLibraries OpenPilot Libraries
|
||||
* @{
|
||||
* @addtogroup Navigation
|
||||
* @brief setups RTH/PH and other pathfollower/pathplanner status
|
||||
* @{
|
||||
*
|
||||
* @file plan.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
*
|
||||
* @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
|
||||
*/
|
||||
|
||||
#ifndef PLANS_H_
|
||||
#define PLANS_H_
|
||||
#include <pios_math.h>
|
||||
|
||||
/** \page standard Plans
|
||||
How to use this library
|
||||
\li Call plan_initialize() to init all needed struct and uavos at startup.<br>
|
||||
It may be safely called more than once.<br>
|
||||
|
||||
\li Functions called plan_setup_* needs to be called once, every time the requested function is engaged.<br>
|
||||
|
||||
\li Functions called plan_run_* are to be periodically called while the requested mode is engaged.<br>
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief initialize UAVOs and structs used by this library
|
||||
*/
|
||||
void plan_initialize();
|
||||
|
||||
/**
|
||||
* @brief setup pathplanner/pathfollower for positionhold
|
||||
*/
|
||||
void plan_setup_positionHold();
|
||||
|
||||
/**
|
||||
* @brief setup pathplanner/pathfollower for return to base
|
||||
*/
|
||||
void plan_setup_returnToBase();
|
||||
|
||||
/**
|
||||
* @brief setup pathplanner/pathfollower for land
|
||||
*/
|
||||
void plan_setup_land();
|
||||
|
||||
/**
|
||||
* @brief execute land
|
||||
*/
|
||||
void plan_run_land();
|
||||
#endif /* PLANS_H_ */
|
||||
130
flight/libraries/plans.c
Normal file
130
flight/libraries/plans.c
Normal file
@ -0,0 +1,130 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotLibraries OpenPilot Libraries
|
||||
* @{
|
||||
* @addtogroup Navigation
|
||||
* @brief setups RTH/PH and other pathfollower/pathplanner status
|
||||
* @{
|
||||
*
|
||||
* @file plan.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
*
|
||||
* @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 <plans.h>
|
||||
#include <openpilot.h>
|
||||
#include <attitudesettings.h>
|
||||
#include <takeofflocation.h>
|
||||
#include <pathdesired.h>
|
||||
#include <positionstate.h>
|
||||
#include <flightmodesettings.h>
|
||||
|
||||
/**
|
||||
* @brief initialize UAVOs and structs used by this library
|
||||
*/
|
||||
void plan_initialize()
|
||||
{
|
||||
TakeOffLocationInitialize();
|
||||
PositionStateInitialize();
|
||||
PathDesiredInitialize();
|
||||
FlightModeSettingsInitialize();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief setup pathplanner/pathfollower for positionhold
|
||||
*/
|
||||
void plan_setup_positionHold()
|
||||
{
|
||||
PositionStateData positionState;
|
||||
|
||||
PositionStateGet(&positionState);
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
|
||||
pathDesired.Start.North = positionState.North;
|
||||
pathDesired.Start.East = positionState.East;
|
||||
pathDesired.Start.Down = positionState.Down;
|
||||
pathDesired.End.North = positionState.North;
|
||||
pathDesired.End.East = positionState.East;
|
||||
pathDesired.End.Down = positionState.Down;
|
||||
pathDesired.StartingVelocity = 1;
|
||||
pathDesired.EndingVelocity = 0;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief setup pathplanner/pathfollower for return to base
|
||||
*/
|
||||
void plan_setup_returnToBase()
|
||||
{
|
||||
// Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
|
||||
float positionStateDown;
|
||||
|
||||
PositionStateDownGet(&positionStateDown);
|
||||
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
|
||||
TakeOffLocationData takeoffLocation;
|
||||
TakeOffLocationGet(&takeoffLocation);
|
||||
|
||||
// TODO: right now VTOLPF does fly straight to destination altitude.
|
||||
// For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
|
||||
|
||||
float destDown;
|
||||
FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
|
||||
destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
|
||||
|
||||
pathDesired.Start.North = takeoffLocation.North;
|
||||
pathDesired.Start.East = takeoffLocation.East;
|
||||
pathDesired.Start.Down = destDown;
|
||||
|
||||
pathDesired.End.North = takeoffLocation.North;
|
||||
pathDesired.End.East = takeoffLocation.East;
|
||||
pathDesired.End.Down = destDown;
|
||||
|
||||
pathDesired.StartingVelocity = 1;
|
||||
pathDesired.EndingVelocity = 0;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
|
||||
void plan_setup_land()
|
||||
{
|
||||
plan_setup_positionHold();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief execute land
|
||||
*/
|
||||
void plan_run_land()
|
||||
{
|
||||
PathDesiredEndData pathDesiredEnd;
|
||||
|
||||
PathDesiredEndGet(&pathDesiredEnd);
|
||||
|
||||
PositionStateDownGet(&pathDesiredEnd.Down);
|
||||
pathDesiredEnd.Down += 5;
|
||||
|
||||
PathDesiredEndSet(&pathDesiredEnd);
|
||||
}
|
||||
@ -63,6 +63,7 @@
|
||||
#include "taskinfo.h"
|
||||
#include <pios_struct_helper.h>
|
||||
|
||||
#include "sin_lookup.h"
|
||||
#include "paths.h"
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
@ -85,6 +86,7 @@ static void updatePathVelocity();
|
||||
static uint8_t updateFixedDesiredAttitude();
|
||||
static void updateFixedAttitude();
|
||||
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
|
||||
static bool correctCourse(float *C, float *V, float *F, float s);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
@ -125,17 +127,17 @@ int32_t FixedWingPathFollowerInitialize()
|
||||
}
|
||||
MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart);
|
||||
|
||||
static float northVelIntegral = 0;
|
||||
static float eastVelIntegral = 0;
|
||||
static float downVelIntegral = 0;
|
||||
static float northVelIntegral = 0.0f;
|
||||
static float eastVelIntegral = 0.0f;
|
||||
static float downVelIntegral = 0.0f;
|
||||
|
||||
static float courseIntegral = 0;
|
||||
static float speedIntegral = 0;
|
||||
static float powerIntegral = 0;
|
||||
static float airspeedErrorInt = 0;
|
||||
static float courseIntegral = 0.0f;
|
||||
static float speedIntegral = 0.0f;
|
||||
static float powerIntegral = 0.0f;
|
||||
static float airspeedErrorInt = 0.0f;
|
||||
|
||||
// correct speed by measured airspeed
|
||||
static float indicatedAirspeedStateBias = 0;
|
||||
static float indicatedAirspeedStateBias = 0.0f;
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
@ -225,12 +227,12 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
|
||||
}
|
||||
} else {
|
||||
// Be cleaner and get rid of global variables
|
||||
northVelIntegral = 0;
|
||||
eastVelIntegral = 0;
|
||||
downVelIntegral = 0;
|
||||
courseIntegral = 0;
|
||||
speedIntegral = 0;
|
||||
powerIntegral = 0;
|
||||
northVelIntegral = 0.0f;
|
||||
eastVelIntegral = 0.0f;
|
||||
downVelIntegral = 0.0f;
|
||||
courseIntegral = 0.0f;
|
||||
speedIntegral = 0.0f;
|
||||
powerIntegral = 0.0f;
|
||||
}
|
||||
PathStatusSet(&pathStatus);
|
||||
}
|
||||
@ -276,14 +278,14 @@ static void updatePathVelocity()
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
default:
|
||||
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
|
||||
boundf(progress.fractional_progress, 0, 1);
|
||||
boundf(progress.fractional_progress, 0.0f, 1.0f);
|
||||
altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
|
||||
boundf(progress.fractional_progress, 0, 1);
|
||||
boundf(progress.fractional_progress, 0.0f, 1.0f);
|
||||
break;
|
||||
}
|
||||
// make sure groundspeed is not zero
|
||||
if (groundspeed < 1e-2f) {
|
||||
groundspeed = 1e-2f;
|
||||
if (groundspeed < 1e-6f) {
|
||||
groundspeed = 1e-6f;
|
||||
}
|
||||
|
||||
// calculate velocity - can be zero if waypoints are too close
|
||||
@ -302,24 +304,12 @@ static void updatePathVelocity()
|
||||
// in this case the plane effectively needs to be turned around
|
||||
// indicators:
|
||||
// difference between correction_direction and velocitystate >90 degrees and
|
||||
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from eerything )
|
||||
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything )
|
||||
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
|
||||
float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityState.East, velocityState.North));
|
||||
float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityState.East, velocityState.North));
|
||||
if (angle1 < -180.0f) {
|
||||
angle1 += 360.0f;
|
||||
}
|
||||
if (angle1 > 180.0f) {
|
||||
angle1 -= 360.0f;
|
||||
}
|
||||
if (angle2 < -180.0f) {
|
||||
angle2 += 360.0f;
|
||||
}
|
||||
if (angle2 > 180.0f) {
|
||||
angle2 -= 360.0f;
|
||||
}
|
||||
if (fabsf(angle1) >= 90.0f && fabsf(angle2) >= 90.0f) {
|
||||
error_speed = 0;
|
||||
if ( // calculating angles < 90 degrees through dot products
|
||||
((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East) < 0.0f) &&
|
||||
((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East) < 0.0f)) {
|
||||
error_speed = 0.0f;
|
||||
}
|
||||
|
||||
// calculate correction - can also be zero if correction vector is 0 or no error present
|
||||
@ -328,14 +318,16 @@ static void updatePathVelocity()
|
||||
|
||||
// scale to correct length
|
||||
float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
|
||||
velocityDesired.North *= groundspeed / l;
|
||||
velocityDesired.East *= groundspeed / l;
|
||||
if (l > 1e-9f) {
|
||||
velocityDesired.North *= groundspeed / l;
|
||||
velocityDesired.East *= groundspeed / l;
|
||||
}
|
||||
|
||||
float downError = altitudeSetpoint - positionState.Down;
|
||||
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
|
||||
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
|
||||
|
||||
// update pathstatus
|
||||
pathStatus.error = progress.error;
|
||||
pathStatus.error = progress.error;
|
||||
pathStatus.fractional_progress = progress.fractional_progress;
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
@ -384,8 +376,7 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
AirspeedStateData airspeedState;
|
||||
SystemSettingsData systemSettings;
|
||||
|
||||
float groundspeedState;
|
||||
float groundspeedDesired;
|
||||
float groundspeedProjection;
|
||||
float indicatedAirspeedState;
|
||||
float indicatedAirspeedDesired;
|
||||
float airspeedError;
|
||||
@ -396,10 +387,9 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
float descentspeedError;
|
||||
float powerCommand;
|
||||
|
||||
float bearing;
|
||||
float heading;
|
||||
float headingError;
|
||||
float course;
|
||||
float airspeedVector[2];
|
||||
float fluidMovement[2];
|
||||
float courseComponent[2];
|
||||
float courseError;
|
||||
float courseCommand;
|
||||
|
||||
@ -413,24 +403,54 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
AirspeedStateGet(&airspeedState);
|
||||
SystemSettingsGet(&systemSettings);
|
||||
|
||||
|
||||
/**
|
||||
* Compute speed error (required for thrust and pitch)
|
||||
* Compute speed error and course
|
||||
*/
|
||||
// missing sensors for airspeed-direction we have to assume within
|
||||
// reasonable error that measured airspeed is actually the airspeed
|
||||
// component in forward pointing direction
|
||||
// airspeedVector is normalized
|
||||
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
|
||||
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
|
||||
|
||||
// Current ground speed
|
||||
groundspeedState = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
|
||||
// note that airspeedStateBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
|
||||
// but thanks to accelerometers, groundspeed reacts faster to changes in direction
|
||||
// current ground speed projected in forward direction
|
||||
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
|
||||
|
||||
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
|
||||
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
|
||||
// than airspeed and gps sensors alone
|
||||
indicatedAirspeedState = groundspeedState + indicatedAirspeedStateBias;
|
||||
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
|
||||
|
||||
// Desired ground speed
|
||||
groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
|
||||
indicatedAirspeedDesired = boundf(groundspeedDesired + indicatedAirspeedStateBias,
|
||||
// fluidMovement is a vector describing the aproximate movement vector of
|
||||
// the surrounding fluid in 2d space (aka wind vector)
|
||||
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
|
||||
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
|
||||
|
||||
// calculate the movement vector we need to fly to reach velocityDesired -
|
||||
// taking fluidMovement into account
|
||||
courseComponent[0] = velocityDesired.North - fluidMovement[0];
|
||||
courseComponent[1] = velocityDesired.East - fluidMovement[1];
|
||||
|
||||
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
|
||||
fixedwingpathfollowerSettings.HorizontalVelMin,
|
||||
fixedwingpathfollowerSettings.HorizontalVelMax);
|
||||
|
||||
// if we could fly at arbitrary speeds, we'd just have to move towards the
|
||||
// courseComponent vector as previously calculated and we'd be fine
|
||||
// unfortunately however we are bound by min and max air speed limits, so
|
||||
// we need to recalculate the correct course to meet at least the
|
||||
// velocityDesired vector direction at our current speed
|
||||
// this overwrites courseComponent
|
||||
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
|
||||
|
||||
// Error condition: wind speed too high, we can't go where we want anymore
|
||||
fixedwingpathfollowerStatus.Errors.Wind = 0;
|
||||
if ((!valid) &&
|
||||
fixedwingpathfollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
|
||||
fixedwingpathfollowerStatus.Errors.Wind = 1;
|
||||
result = 0;
|
||||
}
|
||||
|
||||
// Airspeed error
|
||||
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
|
||||
|
||||
@ -461,23 +481,18 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
result = 0;
|
||||
}
|
||||
|
||||
if (indicatedAirspeedState < 1e-6f) {
|
||||
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
|
||||
// also we cannot handle planes flying backwards, lets just wait until the nose drops
|
||||
fixedwingpathfollowerStatus.Errors.Lowspeed = 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute desired thrust command
|
||||
*/
|
||||
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
|
||||
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
|
||||
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0.0f) {
|
||||
powerIntegral = boundf(powerIntegral + -descentspeedError * dT,
|
||||
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
|
||||
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
|
||||
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
|
||||
} else { powerIntegral = 0; }
|
||||
} else {
|
||||
powerIntegral = 0.0f;
|
||||
}
|
||||
|
||||
// Compute the cross feed from vertical speed to pitch, with saturation
|
||||
float speedErrorToPowerCommandComponent = boundf(
|
||||
@ -504,9 +519,9 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
// Error condition: plane cannot hold altitude at current speed.
|
||||
fixedwingpathfollowerStatus.Errors.Lowpower = 0;
|
||||
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
|
||||
velocityState.Down > 0 && // we ARE going down
|
||||
descentspeedDesired < 0 && // we WANT to go up
|
||||
airspeedError > 0 && // we are too slow already
|
||||
velocityState.Down > 0.0f && // we ARE going down
|
||||
descentspeedDesired < 0.0f && // we WANT to go up
|
||||
airspeedError > 0.0f && // we are too slow already
|
||||
fixedwingpathfollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
|
||||
fixedwingpathfollowerStatus.Errors.Lowpower = 1;
|
||||
result = 0;
|
||||
@ -514,9 +529,9 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
|
||||
fixedwingpathfollowerStatus.Errors.Highpower = 0;
|
||||
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
|
||||
velocityState.Down < 0 && // we ARE going up
|
||||
descentspeedDesired > 0 && // we WANT to go down
|
||||
airspeedError < 0 && // we are too fast already
|
||||
velocityState.Down < 0.0f && // we ARE going up
|
||||
descentspeedDesired > 0.0f && // we WANT to go down
|
||||
airspeedError < 0.0f && // we are too fast already
|
||||
fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
|
||||
fixedwingpathfollowerStatus.Errors.Highpower = 1;
|
||||
result = 0;
|
||||
@ -526,7 +541,6 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
/**
|
||||
* Compute desired pitch command
|
||||
*/
|
||||
|
||||
if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
|
||||
// Integrate with saturation
|
||||
airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT,
|
||||
@ -556,48 +570,18 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
// Error condition: high speed dive
|
||||
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
|
||||
if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
|
||||
velocityState.Down > 0 && // we ARE going down
|
||||
descentspeedDesired < 0 && // we WANT to go up
|
||||
airspeedError < 0 && // we are too fast already
|
||||
velocityState.Down > 0.0f && // we ARE going down
|
||||
descentspeedDesired < 0.0f && // we WANT to go up
|
||||
airspeedError < 0.0f && // we are too fast already
|
||||
fixedwingpathfollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
|
||||
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 1;
|
||||
result = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate where we are heading and why (wind issues)
|
||||
*/
|
||||
bearing = attitudeState.Yaw;
|
||||
heading = RAD2DEG(atan2f(velocityState.East, velocityState.North));
|
||||
headingError = heading - bearing;
|
||||
if (headingError < -180.0f) {
|
||||
headingError += 360.0f;
|
||||
}
|
||||
if (headingError > 180.0f) {
|
||||
headingError -= 360.0f;
|
||||
}
|
||||
// Error condition: wind speed is higher than airspeed. We are forced backwards!
|
||||
fixedwingpathfollowerStatus.Errors.Wind = 0;
|
||||
if ((headingError > fixedwingpathfollowerSettings.Safetymargins.Wind ||
|
||||
headingError < -fixedwingpathfollowerSettings.Safetymargins.Wind) &&
|
||||
fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
|
||||
// we are flying backwards
|
||||
fixedwingpathfollowerStatus.Errors.Wind = 1;
|
||||
result = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute desired roll command
|
||||
*/
|
||||
if (groundspeedDesired > 1e-6f) {
|
||||
course = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North));
|
||||
|
||||
courseError = course - heading;
|
||||
} else {
|
||||
// if we are not supposed to move, run in a circle
|
||||
courseError = -90.0f;
|
||||
result = 0;
|
||||
}
|
||||
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
|
||||
|
||||
if (courseError < -180.0f) {
|
||||
courseError += 360.0f;
|
||||
@ -606,6 +590,19 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
courseError -= 360.0f;
|
||||
}
|
||||
|
||||
// overlap calculation. Theres a dead zone behind the craft where the
|
||||
// counter-yawing of some craft while rolling could render a desired right
|
||||
// turn into a desired left turn. Making the turn direction based on
|
||||
// current roll angle keeps the plane committed to a direction once chosen
|
||||
if (courseError < -180.0f + (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
|
||||
&& attitudeState.Roll > 0.0f) {
|
||||
courseError += 360.0f;
|
||||
}
|
||||
if (courseError > 180.0f - (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
|
||||
&& attitudeState.Roll < 0.0f) {
|
||||
courseError -= 360.0f;
|
||||
}
|
||||
|
||||
courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
|
||||
-fixedwingpathfollowerSettings.CoursePI.ILimit,
|
||||
fixedwingpathfollowerSettings.CoursePI.ILimit);
|
||||
@ -628,7 +625,7 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
* Compute desired yaw command
|
||||
*/
|
||||
// TODO implement raw control mode for yaw and base on Accels.Y
|
||||
stabDesired.Yaw = 0;
|
||||
stabDesired.Yaw = 0.0f;
|
||||
|
||||
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
@ -656,10 +653,116 @@ static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
|
||||
AirspeedStateGet(&airspeedState);
|
||||
VelocityStateGet(&velocityState);
|
||||
float groundspeed = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
|
||||
float airspeedVector[2];
|
||||
float yaw;
|
||||
AttitudeStateYawGet(&yaw);
|
||||
airspeedVector[0] = cos_lookup_deg(yaw);
|
||||
airspeedVector[1] = sin_lookup_deg(yaw);
|
||||
// vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
|
||||
float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
|
||||
|
||||
|
||||
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeed;
|
||||
// note - we do fly by Indicated Airspeed (== calibrated airspeed)
|
||||
// however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement.
|
||||
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
|
||||
// note - we do fly by Indicated Airspeed (== calibrated airspeed) however
|
||||
// since airspeed is updated less often than groundspeed, we use sudden
|
||||
// changes to groundspeed to offset the airspeed by the same measurement.
|
||||
// This has a side effect that in the absence of any airspeed updates, the
|
||||
// pathfollower will fly using groundspeed.
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Function to calculate course vector C based on airspeed s, fluid movement F
|
||||
* and desired movement vector V
|
||||
* parameters in: V,F,s
|
||||
* parameters out: C
|
||||
* returns true if a valid solution could be found for V,F,s, false if not
|
||||
* C will be set to a best effort attempt either way
|
||||
*/
|
||||
static bool correctCourse(float *C, float *V, float *F, float s)
|
||||
{
|
||||
// Approach:
|
||||
// Let Sc be a circle around origin marking possible movement vectors
|
||||
// of the craft with airspeed s (all possible options for C)
|
||||
// Let Vl be a line through the origin along movement vector V where fr any
|
||||
// point v on line Vl v = k * (V / |V|) = k' * V
|
||||
// Let Wl be a line parallel to Vl where for any point v on line Vl exists
|
||||
// a point w on WL with w = v - F
|
||||
// Then any intersection between circle Sc and line Wl represents course
|
||||
// vector which would result in a movement vector
|
||||
// V' = k * ( V / |V|) = k' * V
|
||||
// If there is no intersection point, S is insufficient to compensate
|
||||
// for F and we can only try to fly in direction of V (thus having wind drift
|
||||
// but at least making progress orthogonal to wind)
|
||||
|
||||
s = fabsf(s);
|
||||
float f = sqrtf(F[0] * F[0] + F[1] * F[1]);
|
||||
|
||||
// normalize Cn=V/|V|, |V| must be >0
|
||||
float v = sqrtf(V[0] * V[0] + V[1] * V[1]);
|
||||
if (v < 1e-6f) {
|
||||
// if |V|=0, we aren't supposed to move, turn into the wind
|
||||
// (this allows hovering)
|
||||
C[0] = -F[0];
|
||||
C[1] = -F[1];
|
||||
// if desired airspeed matches fluidmovement a hover is actually
|
||||
// intended so return true
|
||||
return fabsf(f - s) < 1e-3f;
|
||||
}
|
||||
float Vn[2] = { V[0] / v, V[1] / v };
|
||||
|
||||
// project F on V
|
||||
float fp = F[0] * Vn[0] + F[1] * Vn[1];
|
||||
|
||||
// find component Fo of F that is orthogonal to V
|
||||
// (which is exactly the distance between Vl and Wl)
|
||||
float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
|
||||
float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1];
|
||||
|
||||
// find k where k * Vn = C - Fo
|
||||
// |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
|
||||
// so k^2 + fo^2 = s^2 (since |Vn|=1)
|
||||
float k2 = s * s - fo2;
|
||||
if (k2 <= -1e-3f) {
|
||||
// there is no solution, we will be drifted off either way
|
||||
// fallback: fly stupidly in direction of V and hope for the best
|
||||
C[0] = V[0];
|
||||
C[1] = V[1];
|
||||
return false;
|
||||
} else if (k2 <= 1e-3f) {
|
||||
// there is exactly one solution: -Fo
|
||||
C[0] = -Fo[0];
|
||||
C[1] = -Fo[1];
|
||||
return true;
|
||||
}
|
||||
// we have two possible solutions k positive and k negative as there are
|
||||
// two intersection points between Wl and Sc
|
||||
// which one is better? two criteria:
|
||||
// 1. we MUST move in the right direction, if any k leads to -v its invalid
|
||||
// 2. we should minimize the speed error
|
||||
float k = sqrt(k2);
|
||||
float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
|
||||
float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
|
||||
// project C+F on Vn to find signed resulting movement vector length
|
||||
float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
|
||||
float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
|
||||
if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
|
||||
// in this case the angle between course and resulting movement vector
|
||||
// is greater than 90 degrees - so we actually fly backwards
|
||||
C[0] = C1[0];
|
||||
C[1] = C1[1];
|
||||
return true;
|
||||
}
|
||||
C[0] = C2[0];
|
||||
C[1] = C2[1];
|
||||
if (vp2 >= 0.0f) {
|
||||
// in this case the angle between course and movement vector is less than
|
||||
// 90 degrees, but we do move in the right direction
|
||||
return true;
|
||||
} else {
|
||||
// in this case we actually get driven in the opposite direction of V
|
||||
// with both solutions for C
|
||||
// this might be reached in headwind stronger than maximum allowed
|
||||
// airspeed.
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -75,6 +75,18 @@ void pathFollowerHandler(bool newinit);
|
||||
*/
|
||||
void pathPlannerHandler(bool newinit);
|
||||
|
||||
/**
|
||||
* @brief Handler to setup takeofflocation on arming. it is set up during Arming
|
||||
* @input: NONE:
|
||||
* @output: NONE
|
||||
*/
|
||||
void takeOffLocationHandler();
|
||||
|
||||
/**
|
||||
* @brief Initialize TakeoffLocationHanlder
|
||||
*/
|
||||
void takeOffLocationHandlerInit();
|
||||
|
||||
/*
|
||||
* These are assumptions we make in the flight code about the order of settings and their consistency between
|
||||
* objects. Please keep this synchronized to the UAVObjects
|
||||
|
||||
@ -134,6 +134,9 @@ int32_t ManualControlStart()
|
||||
// Make sure unarmed on power up
|
||||
armHandler(true);
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
takeOffLocationHandlerInit();
|
||||
#endif
|
||||
// Start main task
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
|
||||
@ -167,7 +170,9 @@ static void manualControlTask(void)
|
||||
{
|
||||
// Process Arming
|
||||
armHandler(false);
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
takeOffLocationHandler();
|
||||
#endif
|
||||
// Process flight mode
|
||||
FlightStatusData flightStatus;
|
||||
|
||||
|
||||
@ -35,8 +35,9 @@
|
||||
#include <positionstate.h>
|
||||
#include <flightmodesettings.h>
|
||||
|
||||
|
||||
#if defined(REVOLUTION)
|
||||
#include <plans.h>
|
||||
|
||||
// Private constants
|
||||
|
||||
// Private types
|
||||
@ -51,47 +52,30 @@
|
||||
void pathFollowerHandler(bool newinit)
|
||||
{
|
||||
if (newinit) {
|
||||
PathDesiredInitialize();
|
||||
PositionStateInitialize();
|
||||
plan_initialize();
|
||||
}
|
||||
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
uint8_t flightMode;
|
||||
FlightStatusFlightModeGet(&flightMode);
|
||||
|
||||
if (newinit) {
|
||||
// After not being in this mode for a while init at current height
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
FlightModeSettingsData settings;
|
||||
FlightModeSettingsGet(&settings);
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
switch (flightStatus.FlightMode) {
|
||||
switch (flightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
|
||||
// Simple Return To Base mode - keep altitude the same, fly to home position
|
||||
|
||||
|
||||
pathDesired.Start.North = 0;
|
||||
pathDesired.Start.East = 0;
|
||||
pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
|
||||
pathDesired.End.North = 0;
|
||||
pathDesired.End.East = 0;
|
||||
pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
|
||||
pathDesired.StartingVelocity = 1;
|
||||
pathDesired.EndingVelocity = 0;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
plan_setup_returnToBase();
|
||||
break;
|
||||
default:
|
||||
|
||||
pathDesired.Start.North = positionState.North;
|
||||
pathDesired.Start.East = positionState.East;
|
||||
pathDesired.Start.Down = positionState.Down;
|
||||
pathDesired.End.North = positionState.North;
|
||||
pathDesired.End.East = positionState.East;
|
||||
pathDesired.End.Down = positionState.Down;
|
||||
pathDesired.StartingVelocity = 1;
|
||||
pathDesired.EndingVelocity = 0;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
plan_setup_positionHold();
|
||||
break;
|
||||
|
||||
case FLIGHTSTATUS_FLIGHTMODE_LAND:
|
||||
plan_setup_land();
|
||||
break;
|
||||
|
||||
default:
|
||||
plan_setup_positionHold();
|
||||
|
||||
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
|
||||
} else {
|
||||
PathDesiredData pathDesired;
|
||||
@ -103,17 +87,15 @@ void pathFollowerHandler(bool newinit)
|
||||
*/
|
||||
break;
|
||||
}
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
|
||||
switch (flightMode) {
|
||||
// special handling of autoland - behaves like positon hold but with slow altitude decrease
|
||||
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
pathDesired.End.Down = positionState.Down + 5;
|
||||
PathDesiredSet(&pathDesired);
|
||||
case FLIGHTSTATUS_FLIGHTMODE_LAND:
|
||||
plan_run_land();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
125
flight/modules/ManualControl/takeofflocationhandler.c
Normal file
125
flight/modules/ManualControl/takeofflocationhandler.c
Normal file
@ -0,0 +1,125 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file takeofflocationhandler.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief handles TakeOffLocation
|
||||
* --
|
||||
* @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 "inc/manualcontrol.h"
|
||||
#include <stdint.h>
|
||||
#include <flightstatus.h>
|
||||
#include <takeofflocation.h>
|
||||
#include <positionstate.h>
|
||||
|
||||
// Private constants
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static bool locationSet;
|
||||
// Private functions
|
||||
static void SetTakeOffLocation();
|
||||
|
||||
void takeOffLocationHandlerInit()
|
||||
{
|
||||
TakeOffLocationInitialize();
|
||||
// check whether there is a preset/valid takeoff location
|
||||
uint8_t mode;
|
||||
uint8_t status;
|
||||
TakeOffLocationModeGet(&mode);
|
||||
TakeOffLocationStatusGet(&status);
|
||||
// preset with invalid location will actually behave like FirstTakeoff
|
||||
if (mode == TAKEOFFLOCATION_MODE_PRESET && status == TAKEOFFLOCATION_STATUS_VALID) {
|
||||
locationSet = true;
|
||||
} else {
|
||||
locationSet = false;
|
||||
status = TAKEOFFLOCATION_STATUS_INVALID;
|
||||
TakeOffLocationStatusSet(&status);
|
||||
}
|
||||
}
|
||||
/**
|
||||
* Handles TakeOffPosition location setup
|
||||
* @param newinit
|
||||
*/
|
||||
void takeOffLocationHandler()
|
||||
{
|
||||
uint8_t armed;
|
||||
uint8_t status;
|
||||
|
||||
FlightStatusArmedGet(&armed);
|
||||
|
||||
// Location already acquired/preset
|
||||
if (armed == FLIGHTSTATUS_ARMED_ARMED && locationSet) {
|
||||
return;
|
||||
}
|
||||
|
||||
TakeOffLocationStatusGet(&status);
|
||||
|
||||
switch (armed) {
|
||||
case FLIGHTSTATUS_ARMED_ARMING:
|
||||
case FLIGHTSTATUS_ARMED_ARMED:
|
||||
if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) {
|
||||
uint8_t mode;
|
||||
TakeOffLocationModeGet(&mode);
|
||||
|
||||
if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) {
|
||||
SetTakeOffLocation();
|
||||
} else {
|
||||
locationSet = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case FLIGHTSTATUS_ARMED_DISARMED:
|
||||
// unset if location is to be acquired at each arming
|
||||
if (locationSet) {
|
||||
uint8_t mode;
|
||||
TakeOffLocationModeGet(&mode);
|
||||
if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
|
||||
locationSet = false;
|
||||
status = TAKEOFFLOCATION_STATUS_INVALID;
|
||||
TakeOffLocationStatusSet(&status);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieve TakeOffLocation from current PositionStatus
|
||||
*/
|
||||
void SetTakeOffLocation()
|
||||
{
|
||||
TakeOffLocationData takeOffLocation;
|
||||
|
||||
TakeOffLocationGet(&takeOffLocation);
|
||||
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
|
||||
takeOffLocation.North = positionState.North;
|
||||
takeOffLocation.East = positionState.East;
|
||||
takeOffLocation.Down = positionState.Down;
|
||||
takeOffLocation.Status = TAKEOFFLOCATION_STATUS_VALID;
|
||||
|
||||
TakeOffLocationSet(&takeOffLocation);
|
||||
locationSet = true;
|
||||
}
|
||||
@ -45,6 +45,7 @@
|
||||
#include "flightmodesettings.h"
|
||||
#include <pios_struct_helper.h>
|
||||
#include "paths.h"
|
||||
#include "plans.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 1024
|
||||
@ -89,6 +90,7 @@ static bool pathplanner_active = false;
|
||||
*/
|
||||
int32_t PathPlannerStart()
|
||||
{
|
||||
plan_initialize();
|
||||
// when the active waypoint changes, update pathDesired
|
||||
WaypointConnectCallback(commandUpdated);
|
||||
WaypointActiveConnectCallback(commandUpdated);
|
||||
@ -169,21 +171,7 @@ static void pathPlannerTask()
|
||||
if (!failsafeRTHset) {
|
||||
failsafeRTHset = 1;
|
||||
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
FlightModeSettingsData settings;
|
||||
FlightModeSettingsGet(&settings);
|
||||
|
||||
pathDesired.Start.North = 0;
|
||||
pathDesired.Start.East = 0;
|
||||
pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
|
||||
pathDesired.End.North = 0;
|
||||
pathDesired.End.East = 0;
|
||||
pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
|
||||
pathDesired.StartingVelocity = 1;
|
||||
pathDesired.EndingVelocity = 0;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
PathDesiredSet(&pathDesired);
|
||||
plan_setup_positionHold();
|
||||
}
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
|
||||
|
||||
|
||||
@ -62,6 +62,7 @@
|
||||
#include <CoordinateConversions.h>
|
||||
|
||||
#include <pios_board_info.h>
|
||||
#include <pios_struct_helper.h>
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 1000
|
||||
@ -474,35 +475,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
|
||||
RPY2Quaternion(rpy, rotationQuat);
|
||||
Quaternion2R(rotationQuat, R);
|
||||
|
||||
mag_transform[0][0] = R[0][0] * cal.mag_transform.r0c0 +
|
||||
R[1][0] * cal.mag_transform.r0c1 +
|
||||
R[2][0] * cal.mag_transform.r0c2;
|
||||
mag_transform[0][1] = R[0][1] * cal.mag_transform.r0c0 +
|
||||
R[1][1] * cal.mag_transform.r0c1 +
|
||||
R[2][1] * cal.mag_transform.r0c2;
|
||||
mag_transform[0][2] = R[0][2] * cal.mag_transform.r0c0 +
|
||||
R[1][2] * cal.mag_transform.r0c1 +
|
||||
R[2][2] * cal.mag_transform.r0c2;
|
||||
|
||||
mag_transform[1][0] = R[0][0] * cal.mag_transform.r1c0 +
|
||||
R[1][0] * cal.mag_transform.r1c1 +
|
||||
R[2][0] * cal.mag_transform.r1c2;
|
||||
mag_transform[1][1] = R[0][1] * cal.mag_transform.r1c0 +
|
||||
R[1][1] * cal.mag_transform.r1c1 +
|
||||
R[2][1] * cal.mag_transform.r1c2;
|
||||
mag_transform[1][2] = R[0][2] * cal.mag_transform.r1c0 +
|
||||
R[1][2] * cal.mag_transform.r1c1 +
|
||||
R[2][2] * cal.mag_transform.r1c2;
|
||||
|
||||
mag_transform[1][0] = R[0][0] * cal.mag_transform.r2c0 +
|
||||
R[1][0] * cal.mag_transform.r2c1 +
|
||||
R[2][0] * cal.mag_transform.r2c2;
|
||||
mag_transform[2][1] = R[0][1] * cal.mag_transform.r2c0 +
|
||||
R[1][1] * cal.mag_transform.r2c1 +
|
||||
R[2][1] * cal.mag_transform.r2c2;
|
||||
mag_transform[2][2] = R[0][2] * cal.mag_transform.r2c0 +
|
||||
R[1][2] * cal.mag_transform.r2c1 +
|
||||
R[2][2] * cal.mag_transform.r2c2;
|
||||
matrix_mult_3x3f((float(*)[3])cast_struct_to_array(cal.mag_transform, cal.mag_transform.r0c0), R, mag_transform);
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
|
||||
@ -82,6 +82,7 @@ ifndef TESTAPP
|
||||
|
||||
## Misc library functions
|
||||
SRC += $(FLIGHTLIB)/paths.c
|
||||
SRC += $(FLIGHTLIB)/plans.c
|
||||
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
||||
SRC += $(FLIGHTLIB)/insgps13state.c
|
||||
|
||||
|
||||
@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation
|
||||
UAVOBJSRCFILENAMES += poilearnsettings
|
||||
UAVOBJSRCFILENAMES += mpu6000settings
|
||||
UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -84,6 +84,7 @@ ifndef TESTAPP
|
||||
|
||||
## Misc library functions
|
||||
SRC += $(FLIGHTLIB)/paths.c
|
||||
SRC += $(FLIGHTLIB)/plans.c
|
||||
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
||||
SRC += $(FLIGHTLIB)/insgps13state.c
|
||||
|
||||
|
||||
@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation
|
||||
UAVOBJSRCFILENAMES += poilearnsettings
|
||||
UAVOBJSRCFILENAMES += mpu6000settings
|
||||
UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -83,6 +83,7 @@ ifndef TESTAPP
|
||||
|
||||
## Misc library functions
|
||||
SRC += $(FLIGHTLIB)/paths.c
|
||||
SRC += $(FLIGHTLIB)/plans.c
|
||||
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
||||
SRC += $(FLIGHTLIB)/insgps13state.c
|
||||
|
||||
|
||||
@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation
|
||||
UAVOBJSRCFILENAMES += poilearnsettings
|
||||
UAVOBJSRCFILENAMES += mpu6000settings
|
||||
UAVOBJSRCFILENAMES += txpidsettings
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -94,6 +94,7 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c
|
||||
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
||||
SRC += $(FLIGHTLIB)/insgps13state.c
|
||||
SRC += $(FLIGHTLIB)/paths.c
|
||||
SRC += $(FLIGHTLIB)/plans.c
|
||||
SRC += $(FLIGHTLIB)/sanitycheck.c
|
||||
|
||||
SRC += $(MATHLIB)/sin_lookup.c
|
||||
|
||||
@ -111,6 +111,7 @@ UAVOBJSRCFILENAMES += revosettings
|
||||
UAVOBJSRCFILENAMES += altitudeholdstatus
|
||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||
UAVOBJSRCFILENAMES += ekfstatevariance
|
||||
UAVOBJSRCFILENAMES += takeofflocation
|
||||
|
||||
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
|
||||
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
|
||||
|
||||
@ -197,7 +197,6 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
|
||||
QString sec2 = ""; // after &zoom=...
|
||||
GetSecGoogleWords(pos, sec1, sec2);
|
||||
TryCorrectGoogleVersions();
|
||||
QString VersionGoogleSatellite = "132";
|
||||
return QString("https://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatellite).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2);
|
||||
}
|
||||
break;
|
||||
|
||||
@ -44,7 +44,7 @@ CoordinateConversions::CoordinateConversions()
|
||||
* @param[in] LLA Longitude latitude altitude for this location
|
||||
* @param[out] Rne[3][3] Rotation matrix
|
||||
*/
|
||||
void CoordinateConversions::RneFromLLA(double LLA[3], double Rne[3][3])
|
||||
void CoordinateConversions::RneFromLLA(double LLA[3], float Rne[3][3])
|
||||
{
|
||||
float sinLat, sinLon, cosLat, cosLon;
|
||||
|
||||
@ -134,7 +134,7 @@ int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3],
|
||||
// stored value is in cm, convert to m
|
||||
double BaseLLA[3];
|
||||
double ECEF[3];
|
||||
double Rne[3][3];
|
||||
float Rne[3][3];
|
||||
|
||||
// Get LLA address to compute conversion matrix
|
||||
ECEF2LLA(BaseECEFm, BaseLLA);
|
||||
|
||||
@ -41,7 +41,7 @@ public:
|
||||
CoordinateConversions();
|
||||
int NED2LLA_HomeECEF(double BaseECEFcm[3], double NED[3], double position[3]);
|
||||
int NED2LLA_HomeLLA(double LLA[3], double NED[3], double position[3]);
|
||||
void RneFromLLA(double LLA[3], double Rne[3][3]);
|
||||
void RneFromLLA(double LLA[3], float Rne[3][3]);
|
||||
void LLA2ECEF(double LLA[3], double ECEF[3]);
|
||||
int ECEF2LLA(double ECEF[3], double LLA[3]);
|
||||
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
|
||||
|
||||
@ -268,12 +268,12 @@ void FGSimulator::processUpdate(const QByteArray & inp)
|
||||
float temperature = fields[19].toFloat();
|
||||
// Get pressure (kpa)
|
||||
float pressure = fields[20].toFloat() * INHG2KPA;
|
||||
// Get VelocityState Down (cm/s)
|
||||
float velocityStateDown = -fields[21].toFloat() * FPS2CMPS;
|
||||
// Get VelocityState East (cm/s)
|
||||
float velocityStateEast = fields[22].toFloat() * FPS2CMPS;
|
||||
// Get VelocityState Down (cm/s)
|
||||
float velocityStateNorth = fields[23].toFloat() * FPS2CMPS;
|
||||
// Get VelocityState Down (m/s)
|
||||
float velocityStateDown = -fields[21].toFloat() * FPS2CMPS * 1e-2f;
|
||||
// Get VelocityState East (m/s)
|
||||
float velocityStateEast = fields[22].toFloat() * FPS2CMPS * 1e-2f;
|
||||
// Get VelocityState Down (m/s)
|
||||
float velocityStateNorth = fields[23].toFloat() * FPS2CMPS * 1e-2f;
|
||||
|
||||
// Get UDP packets received by FG
|
||||
int n = fields[24].toInt();
|
||||
@ -286,16 +286,15 @@ void FGSimulator::processUpdate(const QByteArray & inp)
|
||||
Output2Hardware out;
|
||||
memset(&out, 0, sizeof(Output2Hardware));
|
||||
|
||||
HomeLocation::DataFields homeData = posHome->getData();
|
||||
double HomeLLA[3] = { (double)homeData.Latitude * 1e-7, (double)homeData.Longitude * 1e-7, homeData.Altitude };
|
||||
double HomeECEF[3];
|
||||
float HomeRNE[3][3];
|
||||
double LLA[3] = { latitude, longitude, altitude_msl };
|
||||
float NED[3];
|
||||
// convert from cm back to meters
|
||||
|
||||
double LLA[3] = { latitude, longitude, altitude_msl };
|
||||
double ECEF[3];
|
||||
double RNE[9];
|
||||
Utils::CoordinateConversions().RneFromLLA(LLA, (double(*)[3])RNE);
|
||||
Utils::CoordinateConversions().LLA2ECEF(LLA, ECEF);
|
||||
Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float(*)[3])RNE, NED);
|
||||
|
||||
Utils::CoordinateConversions().RneFromLLA(HomeLLA, HomeRNE);
|
||||
Utils::CoordinateConversions().LLA2ECEF(HomeLLA, HomeECEF);
|
||||
Utils::CoordinateConversions().LLA2Base(LLA, HomeECEF, HomeRNE, NED);
|
||||
|
||||
// Update GPS Position objects
|
||||
out.latitude = latitude * 1e7;
|
||||
|
||||
@ -125,7 +125,8 @@ HEADERS += \
|
||||
$$UAVOBJECT_SYNTHETICS/osdsettings.h \
|
||||
$$UAVOBJECT_SYNTHETICS/waypoint.h \
|
||||
$$UAVOBJECT_SYNTHETICS/waypointactive.h \
|
||||
$$UAVOBJECT_SYNTHETICS/mpu6000settings.h
|
||||
$$UAVOBJECT_SYNTHETICS/mpu6000settings.h \
|
||||
$$UAVOBJECT_SYNTHETICS/takeofflocation.h
|
||||
|
||||
SOURCES += \
|
||||
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
|
||||
@ -227,4 +228,5 @@ SOURCES += \
|
||||
$$UAVOBJECT_SYNTHETICS/osdsettings.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/waypoint.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/waypointactive.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp
|
||||
$$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/takeofflocation.cpp
|
||||
|
||||
@ -12,6 +12,8 @@
|
||||
|
||||
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
|
||||
<!-- how many seconds to plan the flight vector ahead when initiating necessary heading changes - increase for planes with sluggish response -->
|
||||
<field name="ReverseCourseOverlap" units="deg" type="float" elements="1" defaultvalue="20.0"/>
|
||||
<!-- how big the overlapping area behind the plane is, where, if the desired course lies behind, the current bank angle will determine if the plane goes left or right -->
|
||||
|
||||
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.05"/>
|
||||
<!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->
|
||||
|
||||
@ -108,7 +108,7 @@
|
||||
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
|
||||
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
|
||||
<field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
|
||||
<field name="ReturnToHomeAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
|
||||
<field name="ReturnToBaseAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
@ -45,8 +45,8 @@
|
||||
</elementnames>
|
||||
</field>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="onchange" period="0"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
|
||||
14
shared/uavobjectdefinition/takeofflocation.xml
Normal file
14
shared/uavobjectdefinition/takeofflocation.xml
Normal file
@ -0,0 +1,14 @@
|
||||
<xml>
|
||||
<object name="TakeOffLocation" singleinstance="true" settings="true" category="Navigation">
|
||||
<description>TakeOffLocation setting which contains the destination of a ReturnToBase operation</description>
|
||||
<field name="North" units="m" type="float" elements="1" defaultvalue="0" />
|
||||
<field name="East" units="m" type="float" elements="1" defaultvalue="0" />
|
||||
<field name="Down" units="m" type="float" elements="1" defaultvalue="0" />
|
||||
<field name="Mode" units="" type="enum" elements="1" options="ArmingLocation,FirstArmingLocation,Preset" defaultvalue="ArmingLocation"/>
|
||||
<field name="Status" units="" type="enum" elements="1" options="Valid,Invalid" defaultvalue="Invalid"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
Loading…
Reference in New Issue
Block a user