fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

Browse Source
This commit is contained in:
Julian Oes 2014-06-27 13:36:33 +02:00
commit 049e448abf
13 changed files with 48 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -199,8 +199,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
}
/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits
BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == TECS_MODE_TAKEOFF) {
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
@ -218,12 +218,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
* is running) */
bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ?
_controlTotalEnergy.getOutputLimiter() :
*outputLimiterThrottle,
outputLimiterPitch == NULL ?
_controlEnergyDistribution.getOutputLimiter() :
*outputLimiterPitch);
bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
/* Write part of the status message */
_status.airspeedDerivativeSp = airspeedDerivativeSp;

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

@ -53,8 +53,7 @@
#include "loiter.h"
Loiter::Loiter(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator)
MissionBlock(navigator, name)
{
/* load initial params */
updateParams();

2
src/modules/navigator/loiter.h
View File

@ -47,7 +47,7 @@
#include "navigator_mode.h"
#include "mission_block.h"
class Loiter : public NavigatorMode, MissionBlock
class Loiter : public MissionBlock
{
public:
/**

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

@ -58,8 +58,7 @@
#include "mission.h"
Mission::Mission(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"),
_onboard_mission({0}),
_offboard_mission({0}),

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

@ -62,7 +62,7 @@
class Navigator;
class Mission : public NavigatorMode, MissionBlock
class Mission : public MissionBlock
{
public:
/**

44
src/modules/navigator/mission_block.cpp
View File

@ -52,13 +52,13 @@
#include "mission_block.h"
MissionBlock::MissionBlock(Navigator *navigator) :
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_mission_item({0}),
_mission_item_valid(false),
_navigator_priv(navigator)
_mission_item_valid(false)
{
}
@ -70,7 +70,7 @@ bool
MissionBlock::is_mission_item_reached()
{
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return _navigator_priv->get_vstatus()->condition_landed;
return _navigator->get_vstatus()->condition_landed;
}
/* TODO: count turns */
@ -88,24 +88,24 @@ MissionBlock::is_mission_item_reached()
float dist_z = -1.0f;
float altitude_amsl = _mission_item.altitude_is_relative
? _mission_item.altitude + _navigator_priv->get_home_position()->alt
? _mission_item.altitude + _navigator->get_home_position()->alt
: _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_navigator_priv->get_global_position()->lat,
_navigator_priv->get_global_position()->lon,
_navigator_priv->get_global_position()->alt,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */
if (_navigator_priv->get_global_position()->alt >
altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else {
@ -119,10 +119,10 @@ MissionBlock::is_mission_item_reached()
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
_waypoint_yaw_reached = true;
@ -167,7 +167,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->valid = true;
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
@ -211,25 +211,25 @@ bool
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* don't change setpoint if 'can_loiter_at_sp' flag set */
if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
/* use current position */
pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
_navigator_priv->set_can_loiter_at_sp(true);
_navigator->set_can_loiter_at_sp(true);
}
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|| pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius()
|| pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
|| pos_sp_triplet->current.loiter_direction != 1
|| pos_sp_triplet->previous.valid
|| !pos_sp_triplet->current.valid
|| pos_sp_triplet->next.valid) {
/* position setpoint triplet should be updated */
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
pos_sp_triplet->current.loiter_direction = 1;
pos_sp_triplet->previous.valid = false;

11
src/modules/navigator/mission_block.h
View File

@ -47,17 +47,17 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include "navigator_mode.h"
class Navigator;
class MissionBlock
class MissionBlock : public NavigatorMode
{
public:
/**
* Constructor
*
* @param pointer to parent class
*/
MissionBlock(Navigator *navigator);
MissionBlock(Navigator *navigator, const char *name);
/**
* Destructor
@ -101,9 +101,6 @@ public:
mission_item_s _mission_item;
bool _mission_item_valid;
private:
Navigator *_navigator_priv;
};
#endif

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

@ -115,7 +115,7 @@ public:
Geofence& get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
float get_acceptance_radius() { return _param_acceptance_radius.get(); }
int get_mavlink_fd() { return _mavlink_fd; }
private:
@ -165,7 +165,7 @@ private:
bool _update_triplet; /**< flags if position SP triplet needs to be published */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_takeoff_acceptance_radius; /**< acceptance for takeoff */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
/**
* Retrieve global position
*/

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

@ -123,7 +123,7 @@ Navigator::Navigator() :
_rtl(this, "RTL"),
_update_triplet(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
_param_acceptance_radius(this, "ACC_RAD")
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;

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

@ -55,12 +55,12 @@
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Takeoff Acceptance Radius (FW only)
* Acceptance Radius
*
* Acceptance radius for fixedwing.
* Default acceptance radius, overridden by acceptance radius of waypoint if set.
*
* @unit meters
* @min 1.0
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);

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

@ -56,13 +56,11 @@
#define DELAY_SIGMA 0.01f
RTL::RTL(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY"),
_param_acceptance_radius(this, "ACCEPT_RAD")
_param_land_delay(this, "LAND_DELAY")
{
/* load initial params */
updateParams();
@ -147,7 +145,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
@ -178,7 +176,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
@ -198,7 +196,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = false;
@ -220,7 +218,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = autoland;
@ -246,7 +244,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
@ -265,7 +263,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_IDLE;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;

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

@ -54,7 +54,7 @@
class Navigator;
class RTL : public NavigatorMode, MissionBlock
class RTL : public MissionBlock
{
public:
/**
@ -105,7 +105,6 @@ private:
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
control::BlockParamFloat _param_acceptance_radius;
};
#endif

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

@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
/**
* RTL acceptance radius
*
* Acceptance radius for waypoints set for RTL
*
* @unit meters
* @min 1
* @max
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);