rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Added ReturnToBase flightmode and correct behaviour in ManualControl

Browse Source
This commit is contained in:
Corvus Corax 2012-05-21 10:53:20 +02:00
parent 6a6eace700
commit 84a0fd731f
4 changed files with 24 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -42,6 +42,7 @@ 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_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
)

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

@ -84,7 +84,7 @@ static portTickType lastSysTime;
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 updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void setArmedIfChanged(uint8_t val);
@ -395,7 +395,10 @@ static void manualControlTask(void *parameters)
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
@ -619,7 +622,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
* @brief Update the position desired to current location when
* enabled and allow the waypoint to be moved by transmitter
*/
static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home)
{
static portTickType lastSysTime;
portTickType thisSysTime;
@ -629,7 +632,19 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
if(changed) {
if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else if(changed) {
// After not being in this mode for a while init at current height
PositionActualData positionActual;
PositionActualGet(&positionActual);
@ -639,14 +654,14 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else {
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
}

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,ReturnToBase,PathPlanner"/>
<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,ReturnToBase,PathPlanner" defaultvalue="Manual,Stabilized1,Stabilized2"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<access gcs="readwrite" flight="readwrite"/>