rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-22 14:19:42 +01:00
Code Issues Releases Activity

Create stub for waypoint navigation

Browse Source
This commit is contained in:
James Cotton 2012-03-11 06:31:38 -05:00
parent c8ceea72e6
commit 3eac69e953
11 changed files with 214 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

36
flight/Modules/PathPlanner/inc/pathplanner.h Normal file
View File

@ -0,0 +1,36 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup FlightPlan Flight Plan Module
* @brief Executes flight plan scripts in Python
* @{
*
* @file flightplan.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Executes flight plan scripts in Python
*
* @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 FLIGHTPLAN_H
#define FLIGHTPLAN_H
int32_t FlightPlanInitialize();
#endif // FLIGHTPLAN_H

140
flight/Modules/PathPlanner/pathplanner.c Normal file
View File

@ -0,0 +1,140 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathPlanner Path Planner Module
* @brief Executes a series of waypoints
* @{
*
* @file pathplanner.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Executes a series of waypoints
*
* @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 "openpilot.h"
#include "flightstatus.h"
#include "positionactual.h"
#include "positiondesired.h"
#include "waypoint.h"
#include "waypointactive.h"
// Private constants
#define STACK_SIZE_BYTES 1500
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
#define MAX_QUEUE_SIZE 2
// Private types
// Private variables
static xTaskHandle taskHandle;
static xQueueHandle queue;
// Private functions
static void pathPlannerTask(void *parameters);
/**
* Module initialization
*/
int32_t PathPlannerStart()
{
taskHandle = NULL;
// Start VM thread
xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_PATHPLANNER, taskHandle);
return 0;
}
/**
* Module initialization
*/
int32_t PathPlannerInitialize()
{
taskHandle = NULL;
WaypointInitialize();
WaypointActiveInitialize();
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
return 0;
}
MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
/**
* Module task
*/
static void pathPlannerTask(void *parameters)
{
FlightStatusData flightStatus;
PositionActualData positionActual;
WaypointActiveData waypointActive;
WaypointData waypoint;
const float MIN_RADIUS = 2.0f; // Radius to consider at waypoint (m)
// Main thread loop
while (1)
{
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER)
continue;
PositionActualGet(&positionActual);
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) +
powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) +
powf(positionActual.Down - waypoint.Position[WAYPOINT_POSITION_DOWN], 2);
// We hit this waypoint
if (r2 < (MIN_RADIUS * MIN_RADIUS)) {
if (waypoint.Action == WAYPOINT_ACTION_NEXT) {
waypointActive.Index++;
WaypointActiveSet(&waypointActive);
if(WaypointInstGet(waypointActive.Index, &waypoint) != 0) {
// Oh shit, tried to go to non-existant waypoint
continue;
}
PositionDesiredData positionDesired;
PositionDesiredGet(&positionDesired);
positionDesired.North = waypoint.Position[WAYPOINT_POSITION_NORTH];
positionDesired.East = waypoint.Position[WAYPOINT_POSITION_EAST];
positionDesired.Down = waypoint.Position[WAYPOINT_POSITION_DOWN];
PositionDesiredSet(&positionDesired);
}
}
vTaskDelay(10);
}
}
/**
* @}
* @}
*/

3
flight/Revolution/Makefile
View File

@ -49,7 +49,8 @@ endif
FLASH_TOOL = OPENOCD
# List of modules to include
MODULES = Sensors Attitude/revolution ManualControl Stabilization Altitude/revolution Actuator GPS FirmwareIAP AltitudeHold Guidance
MODULES = Sensors Attitude/revolution ManualControl Stabilization Altitude/revolution Actuator GPS FirmwareIAP
MODULES += AltitudeHold Guidance PathPlanner
MODULES += CameraStab
MODULES += OveroSync
MODULES += Telemetry

2
flight/Revolution/UAVObjects.inc
View File

@ -80,6 +80,8 @@ UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

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

@ -81,6 +81,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/attitudesettings.h \
$$UAVOBJECT_SYNTHETICS/txpidsettings.h \
$$UAVOBJECT_SYNTHETICS/cameradesired.h \
$$UAVOBJECT_SYNTHETICS/waypoint.h \
$$UAVOBJECT_SYNTHETICS/waypointactive.h \
$$UAVOBJECT_SYNTHETICS/faultsettings.h
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
@ -142,4 +144,6 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesettings.cpp \
$$UAVOBJECT_SYNTHETICS/txpidsettings.cpp \
$$UAVOBJECT_SYNTHETICS/cameradesired.cpp \
$$UAVOBJECT_SYNTHETICS/waypoint.cpp \
$$UAVOBJECT_SYNTHETICS/waypointactive.cpp \
$$UAVOBJECT_SYNTHETICS/faultsettings.cpp

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"/>
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

2
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -21,7 +21,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" defaultvalue="Manual,Stabilized1,Stabilized2"/>
<field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner" defaultvalue="Manual,Stabilized1,Stabilized2"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<access gcs="readwrite" flight="readwrite"/>

6
shared/uavobjectdefinition/taskinfo.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="TaskInfo" singleinstance="true" settings="false">
<description>Task information</description>
<field name="StackRemaining" units="bytes" type="uint16" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync"/>
<field name="Running" units="bool" type="enum" options="False,True" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync"/>
<field name="RunningTime" units="%" type="uint8" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync"/>
<field name="StackRemaining" units="bytes" type="uint16" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,PathPlanner,Com2UsbBridge,Usb2ComBridge,OveroSync"/>
<field name="Running" units="bool" type="enum" options="False,True" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,PathPlanner,Com2UsbBridge,Usb2ComBridge,OveroSync"/>
<field name="RunningTime" units="%" type="uint8" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,PathPlanner,Com2UsbBridge,Usb2ComBridge,OveroSync"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="periodic" period="10000"/>

13
shared/uavobjectdefinition/waypoint.xml Normal file
View File

@ -0,0 +1,13 @@
<xml>
<object name="Waypoint" singleinstance="false" settings="false">
<description>A waypoint the aircraft can try and hit. Used by the @ref PathPlanner module</description>
<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="Next,RTH,Loiter10s"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="0"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>

10
shared/uavobjectdefinition/waypointactive.xml Normal file
View File

@ -0,0 +1,10 @@
<xml>
<object name="WaypointActive" singleinstance="true" settings="false">
<description>Indicates the currently active waypoint</description>
<field name="Index" units="" type="uint8" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="0"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>