From ca1923f76fabec18f1272f4f8a7395831e974c37 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 10 Aug 2014 12:50:38 +0200 Subject: [PATCH] OP-1156: Added new (stub template) pathFollower Module as well as builkd environment changes to compile it instead of old pathfollowers --- flight/libraries/sanitycheck.c | 1 - flight/modules/PathFollower/pathfollower.c | 138 +++++++++++++++++++++ .../boards/discoveryf4bare/firmware/Makefile | 3 +- flight/targets/boards/revolution/firmware/Makefile | 3 +- flight/targets/boards/revoproto/firmware/Makefile | 3 +- flight/targets/boards/simposix/firmware/Makefile | 3 +- shared/uavobjectdefinition/callbackinfo.xml | 3 + shared/uavobjectdefinition/hwsettings.xml | 2 +- shared/uavobjectdefinition/taskinfo.xml | 3 - 9 files changed, 146 insertions(+), 13 deletions(-) create mode 100644 flight/modules/PathFollower/pathfollower.c diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 365e1af7b..0902cbce7 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -149,7 +149,6 @@ int32_t configuration_check() case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE: ADDSEVERITY(!coptercontrol); - ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)); ADDSEVERITY(navCapableFusion); break; default: diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c new file mode 100644 index 000000000..3a719ffd7 --- /dev/null +++ b/flight/modules/PathFollower/pathfollower.c @@ -0,0 +1,138 @@ +/** + ****************************************************************************** + * + * @file pathfollower.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint + * 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: PathDesired + * Input object: PositionState + * Input object: ManualControlCommand + * Output object: StabilizationDesired + * + * This module acts as "autopilot" - it controls the setpoints of stabilization + * based on current flight situation and desired flight path (PathDesired) as + * directed by flightmode selection or pathplanner + * This is a periodic delayed callback module + * + * 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 + +#include + +#include +#include +#include + + +#include +#include +#include +#include + + +// Private constants + +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + +#define PF_IDLE_UPDATE_RATE_MS 100 + +#define STACK_SIZE_BYTES 2048 +// Private types + +// Private variables +static DelayedCallbackInfo *pathFollowerCBInfo; + +static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; + +// Private functions +static void pathFollowerTask(void); +static void SettingsUpdatedCb(UAVObjEvent *ev); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t PathFollowerStart() +{ + // Start main task + SettingsUpdatedCb(NULL); + PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo); + + return 0; +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t PathFollowerInitialize() +{ + // initialize objects + FixedWingPathFollowerSettingsInitialize(); + VtolPathFollowerSettingsInitialize(); + FlightStatusInitialize(); + PathStatusInitialize(); + + // Create object queue + + pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); + FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + + return 0; +} +MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart); + + +/** + * Module thread, should not return. + */ +static void pathFollowerTask(void) +{ + FlightStatusData flightStatus; + + FlightStatusGet(&flightStatus); + if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; + } + + // do pathfollower things here + + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); +} + +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + // read in settings (TODO) +} diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 608d2b6cf..bc8e72959 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -35,7 +35,6 @@ USE_DSP_LIB ?= NO #MODULES += Airspeed #MODULES += AltitudeHold #MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Actuator MODULES += GPS @@ -45,7 +44,7 @@ MODULES += Battery MODULES += FirmwareIAP #MODULES += Radio MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index cb99372f1..52b5f8a64 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -35,7 +35,6 @@ MODULES += Altitude/revolution MODULES += Airspeed #MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Receiver MODULES += Actuator @@ -46,7 +45,7 @@ MODULES += Battery MODULES += FirmwareIAP MODULES += Radio MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 39d2d7f3b..9933124a3 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -35,7 +35,6 @@ MODULES += Altitude/revolution MODULES += Airspeed #MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Receiver MODULES += Actuator @@ -45,7 +44,7 @@ MODULES += CameraStab MODULES += Battery MODULES += FirmwareIAP MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 2c89a81ef..d8af95e4a 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -33,8 +33,7 @@ DEBUG ?= YES # List of modules to include MODULES = ManualControl Stabilization GPS MODULES += PathPlanner -MODULES += FixedWingPathFollower -MODULES += VtolPathFollower +MODULES += PathFollower MODULES += CameraStab MODULES += Telemetry MODULES += Logging diff --git a/shared/uavobjectdefinition/callbackinfo.xml b/shared/uavobjectdefinition/callbackinfo.xml index 8624867c4..598e4988a 100644 --- a/shared/uavobjectdefinition/callbackinfo.xml +++ b/shared/uavobjectdefinition/callbackinfo.xml @@ -8,6 +8,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl @@ -20,6 +21,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl @@ -36,6 +38,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f005a31ab..c41187009 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -22,7 +22,7 @@ - + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index a5fb7ad4b..f8018ab53 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -19,7 +19,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx @@ -52,7 +51,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx @@ -89,7 +87,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx -- 2.11.4.GIT