rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Add a RTH flight mode and remove it from the waypoint actions as that is

Browse Source 
redundant with flying to (0,0,0)
This commit is contained in:
James Cotton 2012-06-03 21:18:20 -05:00
parent 9707c6152a
commit 21eb48c58c
7 changed files with 41 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/Modules/ManualControl/inc/manualcontrol.h
View File

@ -42,7 +42,8 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_RTH) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
)
int32_t ManualControlInitialize();

29
flight/Modules/ManualControl/manualcontrol.c
View File

@ -85,6 +85,7 @@ static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed);
static void updatePathDesired(ManualControlCommandData * cmd, bool changed);
static void setRTH(ManualControlCommandData * cmd, bool changed);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void setArmedIfChanged(uint8_t val);
@ -397,6 +398,9 @@ static void manualControlTask(void *parameters)
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_RTH:
setRTH(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
}
@ -614,7 +618,24 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
}
#if defined(REVOLUTION)
// TODO: Need compile flag to exclude this from copter control
static void setRTH(ManualControlCommandData * cmd, bool changed)
{
if(changed) {
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
// Attempt to fly to home 10 m above the current location
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
}
}
/**
* @brief Update the position desired to current location when
* enabled and allow the waypoint to be moved by transmitter
@ -699,6 +720,12 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
// TODO: These functions should never be accessible on CC. Any configuration that
// could allow them to be called sholud already throw an error to prevent this happening
// in flight
static void setRTH(ManualControlCommandData * cmd, bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);

10
flight/Modules/PathPlanner/pathplanner.c
View File

@ -230,16 +230,6 @@ static void activateWaypoint(int idx)
PathDesiredData pathDesired;
switch(waypoint_mode) {
case WAYPOINT_ACTION_RTH:
{
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = -50; // TODO: Get alt from somewhere?
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
}
break;
case WAYPOINT_ACTION_ENDPOINTTONEXT:
{
WaypointInstGet(idx, &waypoint);

8
flight/Modules/VtolPathFollower/vtolpathfollower.c
View File

@ -178,6 +178,14 @@ static void vtolPathFollowerTask(void *parameters)
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RTH:
if (pathDesired.Mode == PATHDESIRED_MODE_ENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (pathDesired.Mode == PATHDESIRED_MODE_ENDPOINT) {
updateEndpointVelocity();

2
shared/uavobjectdefinition/flightstatus.xml
View File

@ -4,7 +4,7 @@
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner"/>
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner,RTH"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

2
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -23,7 +23,7 @@
<field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling" defaultvalue="Attitude,Attitude,Rate"/>
<!-- Note these options values should be identical to those defined in FlightMode -->
<field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner" defaultvalue="Manual,Stabilized1,Stabilized2"/>
<field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner,RTH,Land" defaultvalue="Manual,Stabilized1,Stabilized2"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<access gcs="readwrite" flight="readwrite"/>

2
shared/uavobjectdefinition/waypoint.xml
View File

@ -4,7 +4,7 @@
<field name="Position" units="m" type="float" elementnames="North, East, Down"/>
<field name="Velocity" units="m/s" type="float" elementnames="North, East, Down"/>
<field name="YawDesired" units="deg" type="float" elements="1"/>
<field name="Action" units="" type="enum" elements="1" options="PathToNext,EndpointToNext,RTH,Loiter10s,Land,Stop"/>
<field name="Action" units="" type="enum" elements="1" options="PathToNext,EndpointToNext,Land,Stop"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="4000"/>