rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Merge branch 'amorale/OP-1350_takeoff_location_handling' into next

Browse Source
This commit is contained in:
Corvus Corax 2014-05-26 17:17:04 +02:00
commit 26a660c79d
18 changed files with 396 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
flight/libraries/inc/plans.h Normal file
View 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
View 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);
}

12
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -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

7
flight/modules/ManualControl/manualcontrol.c
View File

@ -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;

66
flight/modules/ManualControl/pathfollowerhandler.c
View File

@ -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
View 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;
}

18
flight/modules/PathPlanner/pathplanner.c
View File

@ -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);

1
flight/targets/boards/discoveryf4bare/firmware/Makefile
View File

@ -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

1
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -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) )

1
flight/targets/boards/revolution/firmware/Makefile
View File

@ -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

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -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) )

1
flight/targets/boards/revoproto/firmware/Makefile
View File

@ -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

1
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -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) )

1
flight/targets/boards/simposix/firmware/Makefile
View File

@ -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

1
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -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) )

6
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -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

2
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -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"/>

14
shared/uavobjectdefinition/takeofflocation.xml Normal file
View 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>