dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'navigator_new' of github.com:PX4/Firmware into navigator_new

This commit is contained in:
Lorenz Meier 2014-01-05 15:20:11 +01:00
parent 018e42733a f387c0ccc3
commit f00e14f749
10 changed files with 219 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/lib/launchdetection/LaunchDetector.cpp
View File

@ -43,7 +43,8 @@
#include <systemlib/err.h>
LaunchDetector::LaunchDetector() :
launchdetection_on(NULL, "LAUN_ALL_ON", false)
launchdetection_on(NULL, "LAUN_ALL_ON", false),
throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
{
/* init all detectors */
launchMethods[0] = new CatapultLaunchMethod();
@ -83,10 +84,11 @@ bool LaunchDetector::getLaunchDetected()
void LaunchDetector::updateParams() {
warnx(" LaunchDetector::updateParams()");
//launchdetection_on.update();
launchdetection_on.update();
throttlePreTakeoff.update();
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
//launchMethods[i]->updateParams();
launchMethods[i]->updateParams();
warnx("updating component %d", i);
}

3
src/lib/launchdetection/LaunchDetector.h
View File

@ -59,11 +59,14 @@ public:
void updateParams();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
// virtual bool getLaunchDetected();
protected:
private:
LaunchMethod* launchMethods[1];
control::BlockParamInt launchdetection_on;
control::BlockParamFloat throttlePreTakeoff;
};

11
src/lib/launchdetection/launchdetection_params.c
View File

@ -33,11 +33,11 @@
****************************************************************************/
/**
* @file fw_pos_control_l1_params.c
* @file launchdetection_params.c
*
* Parameters defined by the L1 position control task
* Parameters for launchdetection
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@ -65,3 +65,8 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
// @Range > 0, in seconds
PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
// @DisplayName Throttle setting while detecting the launch
// @Description The throttle is set to this value while the system is waiting for the takeoff
// @Range 0 to 1
PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);

14
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -174,6 +174,7 @@ private:
/* takeoff/launch states */
bool launch_detected;
bool usePreTakeoffThrust;
bool launch_detection_message_sent;
/* Landingslope object */
@ -396,6 +397,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
launchDetector(),
launch_detected(false),
usePreTakeoffThrust(false),
launch_detection_message_sent(false)
{
/* safely initialize structs */
@ -1004,6 +1006,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_att_sp.yaw_body = _l1_control.nav_bearing();
if (launch_detected) {
usePreTakeoffThrust = false;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (altitude_error > 15.0f) {
@ -1028,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
} else {
throttle_max = 0.0f;
usePreTakeoffThrust = true;
}
}
@ -1053,6 +1056,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* reset takeoff/launch state */
if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
launch_detected = false;
usePreTakeoffThrust = false;
launch_detection_message_sent = false;
}
@ -1150,8 +1154,14 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
setpoint = false;
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
if (usePreTakeoffThrust) {
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
return setpoint;
}

116
src/modules/navigator/geofence.cpp
View File

@ -57,9 +57,12 @@ static const int ERROR = -1;
Geofence::Geofence() : _fence_pub(-1),
_altitude_min(0),
_altitude_max(0)
_altitude_max(0),
_verticesCount(0),
param_geofence_on(NULL, "GF_ON", false)
{
memset(&_fence, 0, sizeof(_fence));
/* Load initial params */
updateParams();
}
Geofence::~Geofence()
@ -70,57 +73,75 @@ Geofence::~Geofence()
bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
{
double lat = vehicle->lat / 1e7d;
double lon = vehicle->lon / 1e7d;
float alt = vehicle->alt;
return inside(lat, lon, vehicle->alt);
}
bool Geofence::inside(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled */
if (param_geofence_on.get() != 1)
return true;
if (valid()) {
if (!isEmpty()) {
/* Vertical check */
if (altitude > _altitude_max || altitude < _altitude_min)
return false;
/*Horizontal check */
/* Adaptation of algorithm originally presented as
* PNPOLY - Point Inclusion in Polygon Test
* W. Randolph Franklin (WRF) */
unsigned int i, j, vertices = _fence.count;
bool c = false;
double lat = vehicle->lat / 1e7d;
double lon = vehicle->lon / 1e7d;
// skip vertex 0 (return point)
for (i = 0, j = vertices - 1; i < vertices; j = i++)
if (((_fence.vertices[i].lon) >= lon != (_fence.vertices[j].lon >= lon)) &&
(lat <= (_fence.vertices[j].lat - _fence.vertices[i].lat) * (lon - _fence.vertices[i].lon) /
(_fence.vertices[j].lon - _fence.vertices[i].lon) + _fence.vertices[i].lat))
c = !c;
return c;
}
struct fence_vertex_s temp_vertex_i;
struct fence_vertex_s temp_vertex_j;
bool
Geofence::loadFromDm(unsigned vertices)
{
struct fence_s temp_fence;
unsigned i;
for (i = 0; i < vertices; i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
/* Red until fence is finished */
for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
temp_fence.count = i;
// skip vertex 0 (return point)
if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
(temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
c = !c;
}
if (valid())
memcpy(&_fence, &temp_fence, sizeof(_fence));
else
warnx("Invalid fence file, ignored!");
}
return _fence.count != 0;
return c;
} else {
/* Empty fence --> accept all points */
return true;
}
} else {
/* Invalid fence --> accept all points */
return true;
}
}
bool
Geofence::valid()
{
// NULL fence is valid
if (_fence.count == 0) {
if (isEmpty())
return true;
}
// Otherwise
if ((_fence.count < 4) || (_fence.count > GEOFENCE_MAX_VERTICES)) {
if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
return false;
}
@ -214,9 +235,25 @@ Geofence::loadFromFile(const char *filename)
/* Parse the line as a geofence point */
struct fence_vertex_s vertex;
if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
/* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */
if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') {
/* Handle degree minute second format */
float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
return ERROR;
// warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f;
vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f;
} else {
/* Handle decimal degree format */
if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
return ERROR;
}
if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
return ERROR;
@ -239,17 +276,11 @@ Geofence::loadFromFile(const char *filename)
fclose(fp);
/* Re-Load imported geofence from DM */
/* Check if import was successful */
if(gotVertical && pointCounter > 0)
{
bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES);
if (fence_valid) {
warnx("Geofence: imported and loaded successfully");
return OK;
} else {
warnx("Geofence: datamanager read error");
return ERROR;
}
_verticesCount = pointCounter;
warnx("Geofence: imported successfully");
} else {
warnx("Geofence: import error");
}
@ -261,3 +292,8 @@ int Geofence::clearDm()
{
dm_clear(DM_KEY_FENCE_POINTS);
}
void Geofence::updateParams()
{
param_geofence_on.update();
}

18
src/modules/navigator/geofence.h
View File

@ -41,16 +41,21 @@
#define GEOFENCE_H_
#include <uORB/topics/fence.h>
#include <controllib/block/BlockParam.hpp>
#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
class Geofence {
private:
struct fence_s _fence;
orb_advert_t _fence_pub; /**< publish fence topic */
float _altitude_min;
float _altitude_max;
unsigned _verticesCount;
/* Params */
control::BlockParamInt param_geofence_on;
public:
Geofence();
~Geofence();
@ -64,12 +69,7 @@ public:
* @return true: craft is inside fence, false:craft is outside fence
*/
bool inside(const struct vehicle_global_position_s *craft);
/**
* Load fence parameters.
*/
bool loadFromDm(unsigned vertices);
bool inside(double lat, double lon, float altitude);
int clearDm();
@ -83,6 +83,10 @@ public:
void publishFence(unsigned vertices);
int loadFromFile(const char *filename);
bool isEmpty() {return _verticesCount == 0;}
void updateParams();
};

55
src/modules/navigator/geofence_params.c Normal file
View File

@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file geofence_params.c
*
* Parameters for geofence
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* geofence parameters, accessible via MAVLink
*
*/
// @DisplayName Switch to enable geofence
// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present
// @Range 0 or 1
PARAM_DEFINE_INT32(GF_ON, 1);

19
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -97,7 +97,24 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
{
//xxx: check geofence
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude
mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
return false;
}
}
}
return true;
}

3
src/modules/navigator/module.mk
View File

@ -41,6 +41,7 @@ SRCS = navigator_main.cpp \
navigator_params.c \
navigator_mission.cpp \
mission_feasibility_checker.cpp \
geofence.cpp
geofence.cpp \
geofence_params.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink

26
src/modules/navigator/navigator_main.cpp
View File

@ -160,6 +160,8 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence;
bool _geofence_violation_warning_sent;
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
@ -393,7 +395,8 @@ Navigator::Navigator() :
_time_first_inside_orbit(0),
_set_nav_state_timestamp(0),
_need_takeoff(true),
_do_takeoff(false)
_do_takeoff(false),
_geofence_violation_warning_sent(false)
{
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
@ -458,6 +461,8 @@ Navigator::parameters_update()
param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt));
_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);
_geofence.updateParams();
}
void
@ -547,8 +552,6 @@ Navigator::task_main()
mavlink_log_info(_mavlink_fd, "[navigator] started");
_fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES);
/* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file
* else clear geofence data in datamanager
@ -766,6 +769,21 @@ Navigator::task_main()
on_mission_item_reached();
}
}
/* Check geofence violation */
if(!_geofence.inside(&_global_pos)) {
//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent)
{
mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
}
/* notify user about state changes */
@ -1515,7 +1533,7 @@ void Navigator::load_fence_from_file(const char *filename)
static void usage()
{
errx(1, "usage: navigator {start|stop|status|fence}");
errx(1, "usage: navigator {start|stop|status|fence|fencefile}");
}
int navigator_main(int argc, char *argv[])