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

navigator: param enhancements

This commit is contained in:
Julian Oes 2014-06-06 18:13:45 +02:00
parent d78c3a2242
commit 85d7fdc741
7 changed files with 93 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/navigator/mission.cpp
View File

@ -60,7 +60,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
_param_onboard_enabled(this, "ONBOARD_EN"),
_param_loiter_radius(this, "LOITER_RAD"),
_onboard_mission({0}),
_offboard_mission({0}),
_current_onboard_mission_index(-1),
@ -241,6 +240,8 @@ Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos
bool
Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{
/* make sure param is up to date */
updateParams();
if (_param_onboard_enabled.get() > 0
&& _current_onboard_mission_index < (int)_onboard_mission.count) {
struct mission_item_s new_mission_item;

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

@ -157,7 +157,6 @@ private:
void publish_mission_result();
control::BlockParamFloat _param_onboard_enabled;
control::BlockParamFloat _param_loiter_radius;
struct mission_s _onboard_mission;
struct mission_s _offboard_mission;

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

@ -58,16 +58,6 @@
*/
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
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f);
/**
* Enable onboard mission

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

@ -38,6 +38,7 @@
MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c \
navigator_mode.cpp \
mission_block.cpp \
mission.cpp \

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

@ -42,11 +42,15 @@
#include <systemlib/perf_counter.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include "navigator_mode.h"
#include "mission.h"
@ -54,7 +58,7 @@
#include "rtl.h"
#include "geofence.h"
class Navigator
class Navigator : public control::SuperBlock
{
public:
/**
@ -105,7 +109,7 @@ public:
Geofence& get_geofence() { return _geofence; }
bool get_is_in_loiter() { return _is_in_loiter; }
float get_loiter_radius() { return 50.0f; }; /* TODO: make param*/
float get_loiter_radius() { return _param_loiter_radius.get(); };
private:
@ -121,6 +125,7 @@ private:
int _control_mode_sub; /**< vehicle control mode subscription */
int _onboard_mission_sub; /**< onboard mission subscription */
int _offboard_mission_sub; /**< offboard mission subscription */
int _param_update_sub; /**< param update subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
@ -150,6 +155,7 @@ private:
bool _is_in_loiter; /**< flags if current position SP can be used to loiter */
bool _update_triplet; /**< flags if position SP triplet needs to be published */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
/**
* Retrieve global position
*/
@ -175,6 +181,11 @@ private:
*/
void vehicle_control_mode_update();
/**
* Update parameters
*/
void params_update();
/**
* Shim for calling task_main from task_create.
*/

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

@ -92,8 +92,7 @@ Navigator *g_navigator;
}
Navigator::Navigator() :
/* state machine transition table */
SuperBlock(NULL, "NAV"),
_task_should_exit(false),
_navigator_task(-1),
_mavlink_fd(-1),
@ -122,8 +121,10 @@ Navigator::Navigator() :
_mission(this, "MIS"),
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_update_triplet(false)
_update_triplet(false),
_param_loiter_radius(this, "LOITER_RAD")
{
updateParams();
}
Navigator::~Navigator()
@ -188,6 +189,13 @@ Navigator::vehicle_control_mode_update()
}
}
void
Navigator::params_update()
{
parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
}
void
Navigator::task_main_trampoline(int argc, char *argv[])
{
@ -226,6 +234,7 @@ Navigator::task_main()
_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));
_param_update_sub - orb_subscribe(ORB_ID(parameter_update));
/* copy all topics first time */
vehicle_status_update();
@ -233,6 +242,7 @@ Navigator::task_main()
global_position_update();
home_position_update();
navigation_capabilities_update();
params_update();
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@ -241,7 +251,7 @@ Navigator::task_main()
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[5];
struct pollfd fds[6];
/* Setup of loop */
fds[0].fd = _global_pos_sub;
@ -254,6 +264,8 @@ Navigator::task_main()
fds[3].events = POLLIN;
fds[4].fd = _control_mode_sub;
fds[4].events = POLLIN;
fds[5].fd = _param_update_sub;
fds[5].events = POLLIN;
while (!_task_should_exit) {
@ -278,6 +290,12 @@ Navigator::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
/* parameters updated */
if (fds[5].revents & POLLIN) {
params_update();
updateParams();
}
/* vehicle control mode updated */
if (fds[4].revents & POLLIN) {
vehicle_control_mode_update();

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

@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (c) 2014 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 for navigator in general
*
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* Loiter radius (FW only)
*
* Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
*
* @unit meters
* @min 0.0
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);