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

navigator: parameter cleanup

This commit is contained in:
Julian Oes 2014-06-04 15:33:09 +02:00
parent 09e4cc605b
commit 425b454a87
8 changed files with 39 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/navigator/mission_params.c
View File

@ -48,9 +48,20 @@
*/
/**
* Loiter radius (fixed wing only)
* Take-off altitude
*
* Default value of loiter radius (if not specified in mission item).
* Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to
* MIS_TAKEOFF_ALT on takeoff, then go to waypoint.
*
* @unit meters
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
/**
* Loiter radius after/during mission (FW only)
*
* Default value of loiter radius (fixedwing only).
*
* @unit meters
* @min 0.0

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

@ -38,10 +38,10 @@
MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c \
mission.cpp \
mission_params.c \
rtl.cpp \
rtl_params.c \
mission_feasibility_checker.cpp \
geofence.cpp \
geofence_params.c

20
src/modules/navigator/navigator.h
View File

@ -107,7 +107,6 @@ private:
int _global_pos_sub; /**< global position subscription */
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
int _onboard_mission_sub; /**< onboard mission subscription */
@ -143,25 +142,6 @@ private:
bool _update_triplet; /**< flags if position setpoint triplet needs to be published */
struct {
float acceptance_radius;
float loiter_radius;
int onboard_mission_enabled;
float takeoff_alt;
} _parameters; /**< local copies of parameters */
struct {
param_t acceptance_radius;
param_t loiter_radius;
param_t onboard_mission_enabled;
param_t takeoff_alt;
} _parameter_handles; /**< handles for parameters */
/**
* Update our local parameter cache.
*/
void parameters_update();
/**
* Retrieve global position
*/

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

@ -64,12 +64,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
#include <uORB/topics/navigation_capabilities.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <geo/geo.h>
@ -102,7 +100,6 @@ Navigator::Navigator() :
_global_pos_sub(-1),
_home_pos_sub(-1),
_vstatus_sub(-1),
_params_sub(-1),
_capabilities_sub(-1),
_control_mode_sub(-1),
_pos_sp_triplet_pub(-1),
@ -127,14 +124,8 @@ Navigator::Navigator() :
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_update_triplet(false),
_parameters({}),
_parameter_handles({})
_update_triplet(false)
{
_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
}
Navigator::~Navigator()
@ -162,23 +153,6 @@ Navigator::~Navigator()
navigator::g_navigator = nullptr;
}
void
Navigator::parameters_update()
{
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius));
param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
//_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
_geofence.updateParams();
}
void
Navigator::global_position_update()
{
@ -251,7 +225,6 @@ Navigator::task_main()
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@ -259,7 +232,6 @@ Navigator::task_main()
/* copy all topics first time */
vehicle_status_update();
vehicle_control_mode_update();
parameters_update();
global_position_update();
home_position_update();
navigation_capabilities_update();
@ -271,21 +243,19 @@ Navigator::task_main()
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[6];
struct pollfd fds[5];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].fd = _global_pos_sub;
fds[0].events = POLLIN;
fds[1].fd = _global_pos_sub;
fds[1].fd = _home_pos_sub;
fds[1].events = POLLIN;
fds[2].fd = _home_pos_sub;
fds[2].fd = _capabilities_sub;
fds[2].events = POLLIN;
fds[3].fd = _capabilities_sub;
fds[3].fd = _vstatus_sub;
fds[3].events = POLLIN;
fds[4].fd = _vstatus_sub;
fds[4].fd = _control_mode_sub;
fds[4].events = POLLIN;
fds[5].fd = _control_mode_sub;
fds[5].events = POLLIN;
while (!_task_should_exit) {
@ -311,33 +281,27 @@ Navigator::task_main()
}
/* vehicle control mode updated */
if (fds[5].revents & POLLIN) {
if (fds[4].revents & POLLIN) {
vehicle_control_mode_update();
}
/* vehicle status updated */
if (fds[4].revents & POLLIN) {
if (fds[3].revents & POLLIN) {
vehicle_status_update();
}
/* parameters updated */
if (fds[0].revents & POLLIN) {
parameters_update();
/* note that these new parameters won't be in effect until a mission triplet is published again */
}
/* navigation capabilities updated */
if (fds[3].revents & POLLIN) {
if (fds[2].revents & POLLIN) {
navigation_capabilities_update();
}
/* home position updated */
if (fds[2].revents & POLLIN) {
if (fds[1].revents & POLLIN) {
home_position_update();
}
/* global position updated */
if (fds[1].revents & POLLIN) {
if (fds[0].revents & POLLIN) {
global_position_update();
/* Check geofence violation */

96
src/modules/navigator/navigator_params.c
View File

@ -1,96 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* 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 navigator_params.c
*
* Parameters defined by the navigator task.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Navigator parameters, accessible via MAVLink
*/
/**
* Waypoint acceptance radius
*
* Default value of acceptance radius (if not specified in mission item).
*
* @unit meters
* @min 0.0
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
/**
* Loiter radius (fixed wing only)
*
* Default value of loiter radius (if not specified in mission item).
*
* @unit meters
* @min 0.0
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Enable onboard mission
*
* @group Navigation
*/
PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
/**
* Take-off altitude
*
* Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
/**
* Enable parachute deployment
*
* @group Navigation
*/
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);

1
src/modules/navigator/rtl.cpp
View File

@ -58,6 +58,7 @@ RTL::RTL(Navigator *navigator, const char *name) :
_home_position({}),
_loiter_radius(50),
_acceptance_radius(50),
_param_loiter_rad(this, "LOITER_RAD"),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY")

1
src/modules/navigator/rtl.h
View File

@ -92,6 +92,7 @@ private:
float _loiter_radius;
float _acceptance_radius;
control::BlockParamFloat _param_loiter_rad;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;

11
src/modules/navigator/rtl_params.c
View File

@ -47,6 +47,17 @@
* RTL parameters, accessible via MAVLink
*/
/**
* Loiter radius after RTL (FW only)
*
* Default value of loiter radius after RTL (fixedwing only).
*
* @unit meters
* @min 0.0
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
/**
* RTL altitude
*