1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/flight/modules/AltitudeHold/altitudehold.c

204 lines
6.6 KiB
C
Raw Normal View History

2011-09-13 07:25:03 +02:00
/**
******************************************************************************
*
* @file guidance.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
2011-09-13 07:25:03 +02:00
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @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: ActiveWaypoint
* Input object: PositionState
2011-09-13 07:25:03 +02:00
* Input object: ManualControlCommand
* Output object: AttitudeDesired
*
* This module will periodically update the value of the AttitudeDesired object.
*
* 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 <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <altitudeholdsettings.h>
#include <altitudeholddesired.h> // object that will be updated by the module
#include <altitudeholdstatus.h>
#include <flightstatus.h>
#include <stabilizationdesired.h>
#include <accelstate.h>
#include <pios_constants.h>
#include <velocitystate.h>
#include <positionstate.h>
2011-09-13 07:25:03 +02:00
// Private constants
2013-12-07 23:31:26 +01:00
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define STACK_SIZE_BYTES 1024
#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
2011-09-13 07:25:03 +02:00
// Private types
// Private variables
static DelayedCallbackInfo *altitudeHoldCBInfo;
2012-02-08 16:42:10 +01:00
static AltitudeHoldSettingsData altitudeHoldSettings;
2013-12-27 18:37:27 +01:00
static struct pid pid0, pid1;
2011-09-13 07:25:03 +02:00
// Private functions
static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
2011-09-13 07:25:03 +02:00
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeHoldStart()
{
// Start main task
SettingsUpdatedCb(NULL);
DelayedCallbackDispatch(altitudeHoldCBInfo);
2011-09-13 07:25:03 +02:00
return 0;
2011-09-13 07:25:03 +02:00
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeHoldInitialize()
{
AltitudeHoldSettingsInitialize();
AltitudeHoldDesiredInitialize();
AltitudeHoldStatusInitialize();
2011-09-13 07:25:03 +02:00
// Create object queue
2012-02-08 16:42:10 +01:00
altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
2012-02-08 16:42:10 +01:00
return 0;
2011-09-13 07:25:03 +02:00
}
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
2011-09-13 07:25:03 +02:00
2011-09-13 07:25:03 +02:00
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void)
2011-09-13 07:25:03 +02:00
{
static float startThrust = 0.5f;
2013-12-07 23:31:26 +01:00
// make sure we run only when we are supposed to run
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
break;
default:
2013-12-27 18:37:27 +01:00
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrustGet(&startThrust);
2013-12-07 23:31:26 +01:00
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
break;
}
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
2013-12-07 23:31:26 +01:00
// do the actual control loop(s)
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
// altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
break;
case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
break;
}
2013-12-07 23:31:26 +01:00
2013-12-29 19:09:54 +01:00
AltitudeHoldStatusSet(&altitudeHoldStatus);
float thrust;
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST:
thrust = altitudeHoldDesired.SetPoint;
break;
default:
// velocity control loop
thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
if (thrust >= 1.0f) {
thrust = 1.0f;
}
if (thrust <= 0.0f) {
thrust = 0.0f;
}
break;
2013-12-29 19:09:54 +01:00
}
2013-12-07 23:31:26 +01:00
StabilizationDesiredData stab;
StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Thrust = thrust;
2013-12-07 23:31:26 +01:00
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stab);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
2012-01-03 00:19:14 +01:00
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
2013-12-27 18:37:27 +01:00
pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
pid_zero(&pid0);
pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
pid_zero(&pid1);
2012-01-03 00:19:14 +01:00
}