mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-13 20:48:42 +01:00
730 lines
28 KiB
C
730 lines
28 KiB
C
/**
|
|
******************************************************************************
|
|
*
|
|
* @file vtolpathfollower.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
|
* @brief This module compared @ref PositionState to @ref PathDesired
|
|
* and sets @ref Stabilization. It only does this when the FlightMode field
|
|
* of @ref FlightStatus is PathPlanner or RTH.
|
|
*
|
|
* @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
|
|
*/
|
|
|
|
/**
|
|
* Input object: FlightStatus
|
|
* Input object: PathDesired
|
|
* Input object: PositionState
|
|
* Output object: StabilizationDesired
|
|
*
|
|
* This module will periodically update the value of the @ref StabilizationDesired object based on
|
|
* @ref PathDesired and @PositionState when the Flight Mode selected in @FlightStatus is supported
|
|
* by this module. Otherwise another module (e.g. @ref ManualControlCommand) is expected to be
|
|
* writing to @ref StabilizationDesired.
|
|
*
|
|
* The module executes in its own thread in this example.
|
|
*
|
|
* Modules have no API, all communication to other modules is done through UAVObjects.
|
|
* However modules may use the API exposed by shared libraries.
|
|
* See the OpenPilot wiki for more details.
|
|
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
|
*
|
|
*/
|
|
|
|
#include <openpilot.h>
|
|
#include <pios_struct_helper.h>
|
|
#include "vtolpathfollower.h"
|
|
|
|
#include "accelstate.h"
|
|
#include "attitudestate.h"
|
|
#include "hwsettings.h"
|
|
#include "pathdesired.h" // object that will be updated by the module
|
|
#include "positionstate.h"
|
|
#include "manualcontrolcommand.h"
|
|
#include "flightstatus.h"
|
|
#include "pathstatus.h"
|
|
#include "gpsvelocitysensor.h"
|
|
#include "gpspositionsensor.h"
|
|
#include "homelocation.h"
|
|
#include "vtolpathfollowersettings.h"
|
|
#include "nedaccel.h"
|
|
#include "stabilizationdesired.h"
|
|
#include "stabilizationsettings.h"
|
|
#include "stabilizationbank.h"
|
|
#include "systemsettings.h"
|
|
#include "velocitydesired.h"
|
|
#include "velocitystate.h"
|
|
#include "taskinfo.h"
|
|
|
|
#include "paths.h"
|
|
#include "CoordinateConversions.h"
|
|
#include <sanitycheck.h>
|
|
|
|
#include "cameradesired.h"
|
|
#include "poilearnsettings.h"
|
|
#include "poilocation.h"
|
|
#include "accessorydesired.h"
|
|
|
|
// Private constants
|
|
#define MAX_QUEUE_SIZE 4
|
|
#define STACK_SIZE_BYTES 1548
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
|
|
|
|
// Private types
|
|
|
|
// Private variables
|
|
static xTaskHandle pathfollowerTaskHandle;
|
|
static PathStatusData pathStatus;
|
|
static VtolPathFollowerSettingsData vtolpathfollowerSettings;
|
|
static float poiRadius;
|
|
|
|
// Private functions
|
|
static void vtolPathFollowerTask(void *parameters);
|
|
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
|
static void updateNedAccel();
|
|
static void updatePOIBearing();
|
|
static void updatePathVelocity();
|
|
static void updateEndpointVelocity();
|
|
static void updateFixedAttitude(float *attitude);
|
|
static void updateVtolDesiredAttitude(bool yaw_attitude);
|
|
static bool vtolpathfollower_enabled;
|
|
static void accessoryUpdated(UAVObjEvent *ev);
|
|
|
|
/**
|
|
* Initialise the module, called on startup
|
|
* \returns 0 on success or -1 if initialisation failed
|
|
*/
|
|
int32_t VtolPathFollowerStart()
|
|
{
|
|
if (vtolpathfollower_enabled) {
|
|
// Start main task
|
|
xTaskCreate(vtolPathFollowerTask, "VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
|
|
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Initialise the module, called on startup
|
|
* \returns 0 on success or -1 if initialisation failed
|
|
*/
|
|
int32_t VtolPathFollowerInitialize()
|
|
{
|
|
HwSettingsOptionalModulesData optionalModules;
|
|
|
|
HwSettingsOptionalModulesGet(&optionalModules);
|
|
FrameType_t frameType = GetCurrentFrameType();
|
|
|
|
if ((optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) ||
|
|
(frameType == FRAME_TYPE_MULTIROTOR)) {
|
|
VtolPathFollowerSettingsInitialize();
|
|
NedAccelInitialize();
|
|
PathDesiredInitialize();
|
|
PathStatusInitialize();
|
|
VelocityDesiredInitialize();
|
|
CameraDesiredInitialize();
|
|
AccessoryDesiredInitialize();
|
|
PoiLearnSettingsInitialize();
|
|
PoiLocationInitialize();
|
|
vtolpathfollower_enabled = true;
|
|
} else {
|
|
vtolpathfollower_enabled = false;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart);
|
|
|
|
static float northVelIntegral = 0;
|
|
static float eastVelIntegral = 0;
|
|
static float downVelIntegral = 0;
|
|
|
|
static float northPosIntegral = 0;
|
|
static float eastPosIntegral = 0;
|
|
static float downPosIntegral = 0;
|
|
|
|
static float thrustOffset = 0;
|
|
/**
|
|
* Module thread, should not return.
|
|
*/
|
|
static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
|
|
{
|
|
SystemSettingsData systemSettings;
|
|
FlightStatusData flightStatus;
|
|
|
|
portTickType lastUpdateTime;
|
|
|
|
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
|
|
AccessoryDesiredConnectCallback(accessoryUpdated);
|
|
|
|
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
|
|
|
|
// Main task loop
|
|
lastUpdateTime = xTaskGetTickCount();
|
|
while (1) {
|
|
// Conditions when this runs:
|
|
// 1. Must have VTOL type airframe
|
|
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
|
|
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
|
|
|
|
SystemSettingsGet(&systemSettings);
|
|
if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
|
|
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
|
|
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX)
|
|
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO)
|
|
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP)
|
|
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH)
|
|
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX)) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
|
|
vTaskDelay(1000);
|
|
continue;
|
|
}
|
|
|
|
// Continue collecting data if not enough time
|
|
vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
|
|
|
|
// Convert the accels into the NED frame
|
|
updateNedAccel();
|
|
|
|
FlightStatusGet(&flightStatus);
|
|
PathStatusGet(&pathStatus);
|
|
PathDesiredData pathDesired;
|
|
PathDesiredGet(&pathDesired);
|
|
|
|
// Check the combinations of flightmode and pathdesired mode
|
|
if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
|
if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
|
|
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) {
|
|
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
|
|
updateEndpointVelocity();
|
|
updateVtolDesiredAttitude(true);
|
|
updatePOIBearing();
|
|
} else {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
|
|
}
|
|
} else {
|
|
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
|
|
updateEndpointVelocity();
|
|
updateVtolDesiredAttitude(false);
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
|
|
} else {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
|
|
}
|
|
}
|
|
} else {
|
|
pathStatus.UID = pathDesired.UID;
|
|
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
|
|
switch (pathDesired.Mode) {
|
|
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
|
|
case PATHDESIRED_MODE_FLYENDPOINT:
|
|
case PATHDESIRED_MODE_FLYVECTOR:
|
|
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
|
|
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
|
updatePathVelocity();
|
|
updateVtolDesiredAttitude(false);
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
|
|
break;
|
|
case PATHDESIRED_MODE_FIXEDATTITUDE:
|
|
updateFixedAttitude(pathDesired.ModeParameters);
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
|
|
break;
|
|
case PATHDESIRED_MODE_DISARMALARM:
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
|
|
break;
|
|
default:
|
|
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
|
|
break;
|
|
}
|
|
PathStatusSet(&pathStatus);
|
|
}
|
|
} else {
|
|
// Be cleaner and get rid of global variables
|
|
northVelIntegral = 0;
|
|
eastVelIntegral = 0;
|
|
downVelIntegral = 0;
|
|
northPosIntegral = 0;
|
|
eastPosIntegral = 0;
|
|
downPosIntegral = 0;
|
|
|
|
// Track thrust before engaging this mode. Cheap system ident
|
|
StabilizationDesiredData stabDesired;
|
|
StabilizationDesiredGet(&stabDesired);
|
|
thrustOffset = stabDesired.Thrust;
|
|
}
|
|
|
|
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Compute bearing and elevation between current position and POI
|
|
*/
|
|
static void updatePOIBearing()
|
|
{
|
|
const float DEADBAND_HIGH = 0.10f;
|
|
const float DEADBAND_LOW = -0.10f;
|
|
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
|
|
|
PathDesiredData pathDesired;
|
|
|
|
PathDesiredGet(&pathDesired);
|
|
PositionStateData positionState;
|
|
PositionStateGet(&positionState);
|
|
CameraDesiredData cameraDesired;
|
|
CameraDesiredGet(&cameraDesired);
|
|
StabilizationDesiredData stabDesired;
|
|
StabilizationDesiredGet(&stabDesired);
|
|
PoiLocationData poi;
|
|
PoiLocationGet(&poi);
|
|
|
|
float dLoc[3];
|
|
float yaw = 0;
|
|
/*float elevation = 0;*/
|
|
|
|
dLoc[0] = positionState.North - poi.North;
|
|
dLoc[1] = positionState.East - poi.East;
|
|
dLoc[2] = positionState.Down - poi.Down;
|
|
|
|
if (dLoc[1] < 0) {
|
|
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
|
|
} else {
|
|
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
|
|
}
|
|
|
|
// distance
|
|
float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
|
|
|
|
ManualControlCommandData manualControlData;
|
|
ManualControlCommandGet(&manualControlData);
|
|
|
|
float changeRadius = 0;
|
|
// Move closer or further, radially
|
|
if (manualControlData.Pitch > DEADBAND_HIGH) {
|
|
changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
|
|
} else if (manualControlData.Pitch < DEADBAND_LOW) {
|
|
changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
|
|
}
|
|
|
|
// move along circular path
|
|
float pathAngle = 0;
|
|
if (manualControlData.Roll > DEADBAND_HIGH) {
|
|
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
|
|
} else if (manualControlData.Roll < DEADBAND_LOW) {
|
|
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
|
|
} else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
|
|
// change radius only when not circling
|
|
poiRadius = distance + changeRadius;
|
|
}
|
|
|
|
// don't try to move any closer
|
|
if (poiRadius >= 3.0f || changeRadius > 0) {
|
|
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
|
|
pathDesired.End.North = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
|
|
pathDesired.End.East = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
|
|
pathDesired.StartingVelocity = 1.0f;
|
|
pathDesired.EndingVelocity = 0.0f;
|
|
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
|
PathDesiredSet(&pathDesired);
|
|
}
|
|
}
|
|
// not above
|
|
if (distance >= 3.0f) {
|
|
// You can feed this into camerastabilization
|
|
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
|
|
|
|
stabDesired.Yaw = yaw + (pathAngle / 2.0f);
|
|
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
|
|
|
// cameraDesired.Yaw=yaw;
|
|
// cameraDesired.PitchOrServo2=elevation;
|
|
|
|
CameraDesiredSet(&cameraDesired);
|
|
StabilizationDesiredSet(&stabDesired);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Compute desired velocity from the current position and path
|
|
*
|
|
* Takes in @ref PositionState and compares it to @ref PathDesired
|
|
* and computes @ref VelocityDesired
|
|
*/
|
|
static void updatePathVelocity()
|
|
{
|
|
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
|
float downCommand;
|
|
|
|
PathDesiredData pathDesired;
|
|
|
|
PathDesiredGet(&pathDesired);
|
|
PositionStateData positionState;
|
|
PositionStateGet(&positionState);
|
|
|
|
float cur[3] =
|
|
{ positionState.North, positionState.East, positionState.Down };
|
|
struct path_status progress;
|
|
|
|
path_progress(
|
|
cast_struct_to_array(pathDesired.Start, pathDesired.Start.North),
|
|
cast_struct_to_array(pathDesired.End, pathDesired.End.North),
|
|
cur, &progress, pathDesired.Mode);
|
|
|
|
float groundspeed;
|
|
switch (pathDesired.Mode) {
|
|
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
|
|
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
|
|
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
|
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
|
|
groundspeed = pathDesired.EndingVelocity;
|
|
break;
|
|
case PATHDESIRED_MODE_FLYENDPOINT:
|
|
case PATHDESIRED_MODE_DRIVEENDPOINT:
|
|
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
|
|
if (progress.fractional_progress > 1) {
|
|
groundspeed = 0;
|
|
}
|
|
break;
|
|
case PATHDESIRED_MODE_FLYVECTOR:
|
|
case PATHDESIRED_MODE_DRIVEVECTOR:
|
|
default:
|
|
groundspeed = pathDesired.StartingVelocity
|
|
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
|
|
if (progress.fractional_progress > 1) {
|
|
groundspeed = 0;
|
|
}
|
|
break;
|
|
}
|
|
|
|
VelocityDesiredData velocityDesired;
|
|
velocityDesired.North = progress.path_direction[0] * groundspeed;
|
|
velocityDesired.East = progress.path_direction[1] * groundspeed;
|
|
|
|
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp;
|
|
float correction_velocity[2] =
|
|
{ progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed };
|
|
|
|
float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2));
|
|
float scale = 1;
|
|
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) {
|
|
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
|
|
}
|
|
|
|
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
|
|
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
|
|
|
|
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1);
|
|
|
|
float downError = altitudeSetpoint - positionState.Down;
|
|
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
|
|
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
|
|
vtolpathfollowerSettings.VerticalPosPI.ILimit);
|
|
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
|
|
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
|
|
|
// update pathstatus
|
|
pathStatus.error = progress.error;
|
|
pathStatus.fractional_progress = progress.fractional_progress;
|
|
|
|
VelocityDesiredSet(&velocityDesired);
|
|
}
|
|
|
|
/**
|
|
* Compute desired velocity from the current position
|
|
*
|
|
* Takes in @ref PositionState and compares it to @ref PositionDesired
|
|
* and computes @ref VelocityDesired
|
|
*/
|
|
void updateEndpointVelocity()
|
|
{
|
|
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
|
|
|
PathDesiredData pathDesired;
|
|
|
|
PathDesiredGet(&pathDesired);
|
|
|
|
PositionStateData positionState;
|
|
VelocityDesiredData velocityDesired;
|
|
|
|
PositionStateGet(&positionState);
|
|
VelocityDesiredGet(&velocityDesired);
|
|
|
|
float northError;
|
|
float eastError;
|
|
float downError;
|
|
float northCommand;
|
|
float eastCommand;
|
|
float downCommand;
|
|
|
|
// Compute desired north command
|
|
northError = pathDesired.End.North - positionState.North;
|
|
northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
|
|
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
|
|
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
|
|
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral);
|
|
|
|
eastError = pathDesired.End.East - positionState.East;
|
|
eastPosIntegral = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
|
|
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
|
|
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
|
|
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral);
|
|
|
|
// Limit the maximum velocity
|
|
float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
|
|
float scale = 1;
|
|
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) {
|
|
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
|
|
}
|
|
|
|
velocityDesired.North = northCommand * scale;
|
|
velocityDesired.East = eastCommand * scale;
|
|
|
|
downError = pathDesired.End.Down - positionState.Down;
|
|
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
|
|
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
|
|
vtolpathfollowerSettings.VerticalPosPI.ILimit);
|
|
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
|
|
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
|
|
|
VelocityDesiredSet(&velocityDesired);
|
|
}
|
|
|
|
/**
|
|
* Compute desired attitude from a fixed preset
|
|
*
|
|
*/
|
|
static void updateFixedAttitude(float *attitude)
|
|
{
|
|
StabilizationDesiredData stabDesired;
|
|
|
|
StabilizationDesiredGet(&stabDesired);
|
|
stabDesired.Roll = attitude[0];
|
|
stabDesired.Pitch = attitude[1];
|
|
stabDesired.Yaw = attitude[2];
|
|
stabDesired.Thrust = attitude[3];
|
|
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
|
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
|
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
|
|
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
|
StabilizationDesiredSet(&stabDesired);
|
|
}
|
|
|
|
/**
|
|
* Compute desired attitude from the desired velocity
|
|
*
|
|
* Takes in @ref NedState which has the acceleration in the
|
|
* NED frame as the feedback term and then compares the
|
|
* @ref VelocityState against the @ref VelocityDesired
|
|
*/
|
|
static void updateVtolDesiredAttitude(bool yaw_attitude)
|
|
{
|
|
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
|
|
|
VelocityDesiredData velocityDesired;
|
|
VelocityStateData velocityState;
|
|
StabilizationDesiredData stabDesired;
|
|
AttitudeStateData attitudeState;
|
|
NedAccelData nedAccel;
|
|
StabilizationBankData stabSettings;
|
|
SystemSettingsData systemSettings;
|
|
|
|
float northError;
|
|
float northCommand;
|
|
|
|
float eastError;
|
|
float eastCommand;
|
|
|
|
float downError;
|
|
float downCommand;
|
|
|
|
SystemSettingsGet(&systemSettings);
|
|
VelocityStateGet(&velocityState);
|
|
VelocityDesiredGet(&velocityDesired);
|
|
StabilizationDesiredGet(&stabDesired);
|
|
VelocityDesiredGet(&velocityDesired);
|
|
AttitudeStateGet(&attitudeState);
|
|
StabilizationBankGet(&stabSettings);
|
|
NedAccelGet(&nedAccel);
|
|
|
|
float northVel = 0, eastVel = 0, downVel = 0;
|
|
switch (vtolpathfollowerSettings.VelocitySource) {
|
|
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_STATE_ESTIMATION:
|
|
northVel = velocityState.North;
|
|
eastVel = velocityState.East;
|
|
downVel = velocityState.Down;
|
|
break;
|
|
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_VELNED:
|
|
{
|
|
GPSVelocitySensorData gpsVelocity;
|
|
GPSVelocitySensorGet(&gpsVelocity);
|
|
northVel = gpsVelocity.North;
|
|
eastVel = gpsVelocity.East;
|
|
downVel = gpsVelocity.Down;
|
|
}
|
|
break;
|
|
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_GROUNDSPEED:
|
|
{
|
|
GPSPositionSensorData gpsPosition;
|
|
GPSPositionSensorGet(&gpsPosition);
|
|
northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading));
|
|
eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading));
|
|
downVel = velocityState.Down;
|
|
}
|
|
break;
|
|
default:
|
|
PIOS_Assert(0);
|
|
break;
|
|
}
|
|
|
|
// Testing code - refactor into manual control command
|
|
ManualControlCommandData manualControlData;
|
|
ManualControlCommandGet(&manualControlData);
|
|
|
|
// Compute desired north command
|
|
northError = velocityDesired.North - northVel;
|
|
northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
|
|
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
|
|
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
|
|
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral
|
|
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd
|
|
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
|
|
|
|
// Compute desired east command
|
|
eastError = velocityDesired.East - eastVel;
|
|
eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
|
|
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
|
|
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
|
|
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral
|
|
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd
|
|
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
|
|
|
|
// Compute desired down command
|
|
downError = velocityDesired.Down - downVel;
|
|
// Must flip this sign
|
|
downError = -downError;
|
|
downVelIntegral = boundf(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
|
|
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
|
|
vtolpathfollowerSettings.VerticalVelPID.ILimit);
|
|
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
|
|
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
|
|
|
|
stabDesired.Thrust = boundf(downCommand + thrustOffset, 0, 1);
|
|
|
|
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
|
|
// craft should move similarly for 5 deg roll versus 5 deg pitch
|
|
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
|
|
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
|
|
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
|
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
|
|
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
|
|
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
|
|
|
if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) {
|
|
// For now override thrust with manual control. Disable at your risk, quad goes to China.
|
|
ManualControlCommandData manualControl;
|
|
ManualControlCommandGet(&manualControl);
|
|
stabDesired.Thrust = manualControl.Thrust;
|
|
}
|
|
|
|
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
|
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
|
if (yaw_attitude) {
|
|
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
|
} else {
|
|
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
|
|
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw;
|
|
}
|
|
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
|
StabilizationDesiredSet(&stabDesired);
|
|
}
|
|
|
|
/**
|
|
* Keep a running filtered version of the acceleration in the NED frame
|
|
*/
|
|
static void updateNedAccel()
|
|
{
|
|
float accel[3];
|
|
float q[4];
|
|
float Rbe[3][3];
|
|
float accel_ned[3];
|
|
|
|
// Collect downsampled attitude data
|
|
AccelStateData accelState;
|
|
|
|
AccelStateGet(&accelState);
|
|
accel[0] = accelState.x;
|
|
accel[1] = accelState.y;
|
|
accel[2] = accelState.z;
|
|
|
|
// rotate avg accels into earth frame and store it
|
|
AttitudeStateData attitudeState;
|
|
AttitudeStateGet(&attitudeState);
|
|
q[0] = attitudeState.q1;
|
|
q[1] = attitudeState.q2;
|
|
q[2] = attitudeState.q3;
|
|
q[3] = attitudeState.q4;
|
|
Quaternion2R(q, Rbe);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
accel_ned[i] = 0;
|
|
for (uint8_t j = 0; j < 3; j++) {
|
|
accel_ned[i] += Rbe[j][i] * accel[j];
|
|
}
|
|
}
|
|
accel_ned[2] += 9.81f;
|
|
|
|
NedAccelData accelData;
|
|
NedAccelGet(&accelData);
|
|
accelData.North = accel_ned[0];
|
|
accelData.East = accel_ned[1];
|
|
accelData.Down = accel_ned[2];
|
|
NedAccelSet(&accelData);
|
|
}
|
|
|
|
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|
{
|
|
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
|
|
}
|
|
|
|
static void accessoryUpdated(UAVObjEvent *ev)
|
|
{
|
|
if (ev->obj != AccessoryDesiredHandle()) {
|
|
return;
|
|
}
|
|
|
|
AccessoryDesiredData accessory;
|
|
PoiLearnSettingsData poiLearn;
|
|
PoiLearnSettingsGet(&poiLearn);
|
|
|
|
if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) {
|
|
if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
|
|
if (accessory.AccessoryVal < -0.5f) {
|
|
PositionStateData positionState;
|
|
PositionStateGet(&positionState);
|
|
PoiLocationData poi;
|
|
PoiLocationGet(&poi);
|
|
poi.North = positionState.North;
|
|
poi.East = positionState.East;
|
|
poi.Down = positionState.Down;
|
|
PoiLocationSet(&poi);
|
|
}
|
|
}
|
|
}
|
|
}
|