Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

navigator begin fixing code style

Browse Source
This commit is contained in:
Daniel Agar
2016-11-06 15:10:28 -05:00
committed by Lorenz Meier
parent 9e589cef48
commit 6f10f8de9a
10 changed files with 382 additions and 320 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/datalinkloss.cpp
+50 -40
View File
@@ -118,48 +118,51 @@ DataLinkLoss::set_dll_item()
switch (_dll_state) {
case DLL_STATE_FLYTOCOMMSHOLDWP: {
_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _param_commsholdalt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _param_commsholdalt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
_navigator->set_can_loiter_at_sp(true);
break;
}
case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _param_airfieldhomealt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _param_airfieldhomealt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
_navigator->set_can_loiter_at_sp(true);
break;
}
case DLL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
break;
}
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
break;
}
default:
break;
}
@@ -178,21 +181,24 @@ DataLinkLoss::advance_dll()
{
switch (_dll_state) {
case DLL_STATE_NONE:
/* Check the number of data link losses. If above home fly home directly */
/* if number of data link losses limit is not reached fly to comms hold wp */
if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
warnx("%d data link losses, limit is %d, fly to airfield home",
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
if (!_param_skipcommshold.get()) {
warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to comms hold");
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
} else {
/* comms hold wp not active, fly to airfield home directly */
warnx("Skipping comms hold wp. Flying directly to airfield home");
@@ -200,15 +206,18 @@ DataLinkLoss::advance_dll()
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
}
}
break;
case DLL_STATE_FLYTOCOMMSHOLDWP:
warnx("fly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
_dll_state = DLL_STATE_TERMINATE;
warnx("time is up, state should have been changed manually by now");
@@ -217,6 +226,7 @@ DataLinkLoss::advance_dll()
_navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case DLL_STATE_TERMINATE:
warnx("dll end");
_dll_state = DLL_STATE_END;
src/modules/navigator/enginefailure.cpp
+22 -20
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/* @file enginefailure.cpp
* Helper class for a fixedwing engine failure mode
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
/* @file enginefailure.cpp
* Helper class for a fixedwing engine failure mode
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
@@ -104,23 +104,24 @@ EngineFailure::set_ef_item()
switch (_ef_state) {
case EF_STATE_LOITERDOWN: {
//XXX create mission item at ground (below?) here
//XXX create mission item at ground (below?) here
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
//XXX setting altitude to a very low value, evaluate other options
_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
//XXX setting altitude to a very low value, evaluate other options
_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
_navigator->set_can_loiter_at_sp(true);
break;
}
default:
break;
}
@@ -142,6 +143,7 @@ EngineFailure::advance_ef()
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down");
_ef_state = EF_STATE_LOITERDOWN;
break;
default:
break;
}
src/modules/navigator/follow_target.cpp
+51 -38
View File
@@ -60,10 +60,10 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_navigator(navigator),
_param_min_alt(this, "NAV_MIN_FT_HT", false),
_param_tracking_dist(this,"NAV_FT_DST", false),
_param_tracking_side(this,"NAV_FT_FS", false),
_param_tracking_resp(this,"NAV_FT_RS", false),
_param_yaw_auto_max(this,"MC_YAWRAUTO_MAX", false),
_param_tracking_dist(this, "NAV_FT_DST", false),
_param_tracking_side(this, "NAV_FT_FS", false),
_param_tracking_resp(this, "NAV_FT_RS", false),
_param_yaw_auto_max(this, "MC_YAWRAUTO_MAX", false),
_follow_target_state(SET_WAIT_FOR_TARGET_POSITION),
_follow_target_position(FOLLOW_FROM_BEHIND),
_follow_target_sub(-1),
@@ -71,14 +71,16 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
_follow_offset(OFFSET_M),
_target_updates(0),
_last_update_time(0),
_current_target_motion({}),
_previous_target_motion({}),
_current_target_motion(),
_previous_target_motion(),
_yaw_rate(0.0F),
_responsiveness(0.0F),
_yaw_auto_max(0.0F),
_yaw_angle(0.0F)
{
updateParams();
_current_target_motion = {};
_previous_target_motion = {};
_current_vel.zero();
_step_vel.zero();
_est_target_vel.zero();
@@ -108,7 +110,7 @@ void FollowTarget::on_activation()
_follow_target_position = _param_tracking_side.get();
if((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
if ((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
_follow_target_position = FOLLOW_FROM_BEHIND;
}
@@ -143,18 +145,20 @@ void FollowTarget::on_active()
orb_copy(ORB_ID(follow_target), _follow_target_sub, &target_motion);
if(_current_target_motion.timestamp == 0) {
if (_current_target_motion.timestamp == 0) {
_current_target_motion = target_motion;
}
_current_target_motion.timestamp = target_motion.timestamp;
_current_target_motion.lat = (_current_target_motion.lat*(double)_responsiveness) + target_motion.lat*(double)(1 - _responsiveness);
_current_target_motion.lon = (_current_target_motion.lon*(double)_responsiveness) + target_motion.lon*(double)(1 - _responsiveness);
_current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)(
1 - _responsiveness);
_current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)(
1 - _responsiveness);
target_reported_velocity(0) = _current_target_motion.vx;
target_reported_velocity(1) = _current_target_motion.vy;
} else if (((current_time - _current_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
} else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
reset_target_validity();
}
@@ -165,7 +169,8 @@ void FollowTarget::on_active()
// get distance to target
map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1));
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
&_target_distance(1));
}
@@ -187,7 +192,8 @@ void FollowTarget::on_active()
// calculate distance the target has moved
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(_target_position_delta(0)), &(_target_position_delta(1)));
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon,
&(_target_position_delta(0)), &(_target_position_delta(1)));
// update the average velocity of the target based on the position
@@ -195,15 +201,15 @@ void FollowTarget::on_active()
// if the target is moving add an offset and rotation
if(_est_target_vel.length() > .5F) {
_target_position_offset = _rot_matrix*_est_target_vel.normalized()*_follow_offset;
if (_est_target_vel.length() > .5F) {
_target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset;
}
// are we within the target acceptance radius?
// give a buffer to exit/enter the radius to give the velocity controller
// a chance to catch up
_radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
_radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
_radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
// to keep the velocity increase/decrease smooth
@@ -221,7 +227,7 @@ void FollowTarget::on_active()
// if we are less than 1 meter from the target don't worry about trying to yaw
// lock the yaw until we are at a distance that makes sense
if((_target_distance).length() > 1.0F) {
if ((_target_distance).length() > 1.0F) {
// yaw rate smoothing
@@ -229,15 +235,15 @@ void FollowTarget::on_active()
// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_current_target_motion.lat,
_current_target_motion.lon);
_navigator->get_global_position()->lon,
_current_target_motion.lat,
_current_target_motion.lon);
_yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F);
_yaw_rate = _wrap_pi(_yaw_rate);
_yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max);
_yaw_rate = math::constrain(_yaw_rate, -1.0F * _yaw_auto_max, _yaw_auto_max);
} else {
_yaw_angle = _yaw_rate = NAN;
@@ -257,12 +263,13 @@ void FollowTarget::on_active()
// (double)_avg_cos_ratio, (double) _yaw_rate);
}
if(target_position_valid()) {
if (target_position_valid()) {
// get the target position using the calculated offset
map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon);
map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion_with_offset.lat, &target_motion_with_offset.lon);
map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1),
&target_motion_with_offset.lat, &target_motion_with_offset.lon);
}
// clamp yaw rate smoothing if we are with in
@@ -282,12 +289,14 @@ void FollowTarget::on_active()
if (_radius_entered == true) {
_follow_target_state = TRACK_VELOCITY;
} else if (target_velocity_valid()) {
set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle);
// keep the current velocity updated with the target velocity for when it's needed
_current_vel = _est_target_vel;
update_position_sp(true, true, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
@@ -299,6 +308,7 @@ void FollowTarget::on_active()
if (_radius_exited == true) {
_follow_target_state = TRACK_POSITION;
} else if (target_velocity_valid()) {
if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) {
@@ -309,37 +319,40 @@ void FollowTarget::on_active()
set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle);
update_position_sp(true, false, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
break;
}
case SET_WAIT_FOR_TARGET_POSITION: {
// Climb to the minimum altitude
// and wait until a position is received
// Climb to the minimum altitude
// and wait until a position is received
follow_target_s target = {};
follow_target_s target = {};
// for now set the target at the minimum height above the uav
// for now set the target at the minimum height above the uav
target.lat = _navigator->get_global_position()->lat;
target.lon = _navigator->get_global_position()->lon;
target.alt = 0.0F;
target.lat = _navigator->get_global_position()->lat;
target.lon = _navigator->get_global_position()->lon;
target.alt = 0.0F;
set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle);
set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle);
update_position_sp(false, false, _yaw_rate);
update_position_sp(false, false, _yaw_rate);
_follow_target_state = WAIT_FOR_TARGET_POSITION;
}
_follow_target_state = WAIT_FOR_TARGET_POSITION;
}
case WAIT_FOR_TARGET_POSITION: {
if (is_mission_item_reached() && target_velocity_valid()) {
_target_position_offset(0) = _follow_offset;
_follow_target_state = TRACK_POSITION;
}
if (is_mission_item_reached() && target_velocity_valid()) {
_target_position_offset(0) = _follow_offset;
_follow_target_state = TRACK_POSITION;
}
break;
}
src/modules/navigator/follow_target.h
+28 -19
View File
@@ -85,21 +85,30 @@ private:
};
float _follow_position_matricies[4][9] = {
{1.0F, -1.0F, 0.0F,
1.0F, 1.0F, 0.0F,
0.0F, 0.0F, 1.0F}, // follow right
{
1.0F, -1.0F, 0.0F,
1.0F, 1.0F, 0.0F,
0.0F, 0.0F, 1.0F
}, // follow right
{-1.0F, 0.0F, 0.0F,
0.0F, -1.0F, 0.0F,
0.0F, 0.0F, 1.0F}, // follow behind
{
-1.0F, 0.0F, 0.0F,
0.0F, -1.0F, 0.0F,
0.0F, 0.0F, 1.0F
}, // follow behind
{1.0F, 0.0F, 0.0F,
0.0F, 1.0F, 0.0F,
0.0F, 0.0F, 1.0F}, // follow front
{
1.0F, 0.0F, 0.0F,
0.0F, 1.0F, 0.0F,
0.0F, 0.0F, 1.0F
}, // follow front
{1.0F, 1.0F, 0.0F,
-1.0F, 1.0F, 0.0F,
0.0F, 0.0F, 1.0F}}; // follow left side
{
1.0F, 1.0F, 0.0F,
-1.0F, 1.0F, 0.0F,
0.0F, 0.0F, 1.0F
}
}; // follow left side
Navigator *_navigator;
@@ -137,14 +146,14 @@ private:
// Mavlink defined motion reporting capabilities
enum {
POS = 0,
VEL = 1,
ACCEL = 2,
ATT_RATES = 3
};
enum {
POS = 0,
VEL = 1,
ACCEL = 2,
ATT_RATES = 3
};
math::Matrix<3,3> _rot_matrix;
math::Matrix<3, 3> _rot_matrix;
void track_target_position();
void track_target_velocity();
bool target_velocity_valid();
src/modules/navigator/gpsfailure.cpp
+28 -21
View File
@@ -100,29 +100,32 @@ GpsFailure::on_active()
switch (_gpsf_state) {
case GPSF_STATE_LOITER: {
/* Position controller does not run in this mode:
* navigator has to publish an attitude setpoint */
_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
_navigator->publish_att_sp();
/* Position controller does not run in this mode:
* navigator has to publish an attitude setpoint */
_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
_navigator->publish_att_sp();
/* Measure time */
hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
/* Measure time */
hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
if (elapsed > _param_loitertime.get() * 1e6f) {
/* no recovery, adavance the state machine */
warnx("gps not recovered, switch to next state");
advance_gpsf();
//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
if (elapsed > _param_loitertime.get() * 1e6f) {
/* no recovery, adavance the state machine */
warnx("gps not recovered, switch to next state");
advance_gpsf();
}
break;
}
break;
}
case GPSF_STATE_TERMINATE:
set_gpsf_item();
advance_gpsf();
break;
default:
break;
}
@@ -140,11 +143,12 @@ GpsFailure::set_gpsf_item()
switch (_gpsf_state) {
case GPSF_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->set_mission_result_updated();
warnx("gps fail: request flight termination");
}
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->set_mission_result_updated();
warnx("gps fail: request flight termination");
}
default:
break;
}
@@ -164,15 +168,18 @@ GpsFailure::advance_gpsf()
warnx("gpsf loiter");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter");
break;
case GPSF_STATE_LOITER:
_gpsf_state = GPSF_STATE_TERMINATE;
warnx("gpsf terminate");
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination");
warnx("mavlink sent");
break;
case GPSF_STATE_TERMINATE:
warnx("gpsf end");
_gpsf_state = GPSF_STATE_END;
default:
break;
}
src/modules/navigator/land.cpp
+24 -24
View File
@@ -54,10 +54,10 @@
#include "navigator.h"
Land::Land(Navigator *navigator, const char *name) :
MissionBlock(navigator, name)
MissionBlock(navigator, name)
{
/* load initial params */
updateParams();
/* load initial params */
updateParams();
}
Land::~Land()
@@ -72,34 +72,34 @@ Land::on_inactive()
void
Land::on_activation()
{
/* set current mission item to Land */
set_land_item(&_mission_item, true);
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
/* set current mission item to Land */
set_land_item(&_mission_item, true);
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(false);
_navigator->set_can_loiter_at_sp(false);
_navigator->set_position_setpoint_triplet_updated();
_navigator->set_position_setpoint_triplet_updated();
}
void
Land::on_active()
{
if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
set_idle_item(&_mission_item);
if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
set_idle_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
}
src/modules/navigator/land.h
+5 -5
View File
@@ -50,15 +50,15 @@
class Land : public MissionBlock
{
public:
Land(Navigator *navigator, const char *name);
Land(Navigator *navigator, const char *name);
~Land();
~Land();
virtual void on_inactive();
virtual void on_inactive();
virtual void on_activation();
virtual void on_activation();
virtual void on_active();
virtual void on_active();
};
src/modules/navigator/loiter.cpp
+10 -7
View File
@@ -80,6 +80,7 @@ Loiter::on_activation()
{
if (_navigator->get_reposition_triplet()->current.valid) {
reposition();
} else {
set_loiter_position();
}
@@ -103,8 +104,9 @@ Loiter::set_loiter_position()
{
// not setting loiter position until armed
if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED ||
_loiter_pos_set) {
_loiter_pos_set) {
return;
} else {
_loiter_pos_set = true;
}
@@ -149,16 +151,17 @@ Loiter::reposition()
// set yaw
float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
if (travel_dist > 1.0f) {
// calculate direction the vehicle should point to.
pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon);
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon);
}
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
src/modules/navigator/rcloss.cpp
+30 -23
View File
@@ -108,31 +108,33 @@ RCLoss::set_rcl_item()
switch (_rcl_state) {
case RCL_STATE_LOITER: {
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = _navigator->get_global_position()->alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = _navigator->get_global_position()->alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
_navigator->set_can_loiter_at_sp(true);
break;
}
case RCL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->set_mission_result_updated();
warnx("rc not recovered: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
break;
}
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->set_mission_result_updated();
warnx("rc not recovered: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
break;
}
default:
break;
}
@@ -155,6 +157,7 @@ RCLoss::advance_rcl()
warnx("RC loss, OBC mode, loiter");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, loitering");
_rcl_state = RCL_STATE_LOITER;
} else {
warnx("RC loss, OBC mode, slip loiter, terminate");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, terminating");
@@ -163,7 +166,9 @@ RCLoss::advance_rcl()
_navigator->set_mission_result_updated();
reset_mission_item_reached();
}
break;
case RCL_STATE_LOITER:
_rcl_state = RCL_STATE_TERMINATE;
warnx("time is up, no RC regain, terminating");
@@ -172,10 +177,12 @@ RCLoss::advance_rcl()
_navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case RCL_STATE_TERMINATE:
warnx("rcl end");
_rcl_state = RCL_STATE_END;
break;
default:
break;
}
src/modules/navigator/rtl.cpp
+134 -123
View File
@@ -94,8 +94,9 @@ RTL::on_activation()
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* check if we are pretty close to home already */
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
@@ -104,12 +105,15 @@ RTL::on_activation()
_rtl_state = RTL_STATE_LANDED;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");
/* if lower than return altitude, climb up first */
} else if (home_dist > _param_rtl_min_dist.get() && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
/* if lower than return altitude, climb up first */
} else if (home_dist > _param_rtl_min_dist.get()
&& _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
@@ -147,153 +151,157 @@ RTL::set_rtl_item()
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
(int)(climb_alt),
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
(int)(climb_alt),
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_RETURN: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
// use home yaw if close to home
/* check if we are pretty close to home already */
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
// use home yaw if close to home
/* check if we are pretty close to home already */
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
if (home_dist < _param_rtl_min_dist.get()) {
_mission_item.yaw = _navigator->get_home_position()->yaw;
} else {
if (pos_sp_triplet->previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
_mission_item.yaw = get_bearing_to_next_waypoint(
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
_mission_item.lat, _mission_item.lon);
if (home_dist < _param_rtl_min_dist.get()) {
_mission_item.yaw = _navigator->get_home_position()->yaw;
} else {
/* else use current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
if (pos_sp_triplet->previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
_mission_item.yaw = get_bearing_to_next_waypoint(
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
_mission_item.lat, _mission_item.lon);
} else {
/* else use current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
_rtl_start_lock = true;
break;
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
_rtl_start_lock = true;
break;
}
case RTL_STATE_TRANSITION_TO_MC: {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
break;
}
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
break;
}
case RTL_STATE_DESCEND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
// check if we are already lower - then we will just stay there
if (_mission_item.altitude > _navigator->get_global_position()->alt) {
_mission_item.altitude = _navigator->get_global_position()->alt;
// check if we are already lower - then we will just stay there
if (_mission_item.altitude > _navigator->get_global_position()->alt) {
_mission_item.altitude = _navigator->get_global_position()->alt;
}
_mission_item.yaw = _navigator->get_home_position()->yaw;
// except for vtol which might be still off here and should point towards this location
float d_current = get_distance_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
/* disable previous setpoint to prevent drift */
pos_sp_triplet->previous.valid = false;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
_mission_item.yaw = _navigator->get_home_position()->yaw;
// except for vtol which might be still off here and should point towards this location
float d_current = get_distance_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
/* disable previous setpoint to prevent drift */
pos_sp_triplet->previous.valid = false;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_LOITER: {
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_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.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_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.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
_navigator->set_can_loiter_at_sp(true);
if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)Navigator::get_time_inside(_mission_item));
if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
(double)Navigator::get_time_inside(_mission_item));
} else {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
} else {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
}
break;
}
break;
}
case RTL_STATE_LAND: {
_mission_item.yaw = _navigator->get_home_position()->yaw;
set_land_item(&_mission_item, false);
_mission_item.yaw = _navigator->get_home_position()->yaw;
set_land_item(&_mission_item, false);
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
break;
}
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
break;
}
case RTL_STATE_LANDED: {
set_idle_item(&_mission_item);
set_idle_item(&_mission_item);
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
break;
}
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
break;
}
default:
break;
@@ -327,6 +335,7 @@ RTL::advance_rtl()
if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
_rtl_state = RTL_STATE_TRANSITION_TO_MC;
}
break;
case RTL_STATE_TRANSITION_TO_MC:
@@ -334,6 +343,7 @@ RTL::advance_rtl()
break;
case RTL_STATE_DESCEND:
/* only go to land if autoland is enabled */
if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
_rtl_state = RTL_STATE_LOITER;
@@ -341,6 +351,7 @@ RTL::advance_rtl()
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LOITER: