forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'px4/fix_mission_dive' into beta_mavlink
This commit is contained in:
commit
3ae35d8c18
|
@ -38,7 +38,6 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
#include <geo/geo.h>
|
|
||||||
|
|
||||||
#define ecl_absolute_time hrt_absolute_time
|
#define ecl_absolute_time hrt_absolute_time
|
||||||
#define ecl_elapsed_time hrt_elapsed_time
|
#define ecl_elapsed_time hrt_elapsed_time
|
||||||
|
|
|
@ -3,13 +3,10 @@
|
||||||
#include "tecs.h"
|
#include "tecs.h"
|
||||||
#include <ecl/ecl.h>
|
#include <ecl/ecl.h>
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
#include <geo/geo.h>
|
||||||
|
|
||||||
using namespace math;
|
using namespace math;
|
||||||
|
|
||||||
#ifndef CONSTANTS_ONE_G
|
|
||||||
#define CONSTANTS_ONE_G GRAVITY
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file tecs.cpp
|
* @file tecs.cpp
|
||||||
*
|
*
|
||||||
|
|
|
@ -28,16 +28,7 @@ class __EXPORT TECS
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
TECS() :
|
TECS() :
|
||||||
|
_pitch_dem(0.0f),
|
||||||
_airspeed_enabled(false),
|
|
||||||
_throttle_slewrate(0.0f),
|
|
||||||
_climbOutDem(false),
|
|
||||||
_hgt_dem_prev(0.0f),
|
|
||||||
_hgt_dem_adj_last(0.0f),
|
|
||||||
_hgt_dem_in_old(0.0f),
|
|
||||||
_TAS_dem_last(0.0f),
|
|
||||||
_TAS_dem_adj(0.0f),
|
|
||||||
_TAS_dem(0.0f),
|
|
||||||
_integ1_state(0.0f),
|
_integ1_state(0.0f),
|
||||||
_integ2_state(0.0f),
|
_integ2_state(0.0f),
|
||||||
_integ3_state(0.0f),
|
_integ3_state(0.0f),
|
||||||
|
@ -45,8 +36,16 @@ public:
|
||||||
_integ5_state(0.0f),
|
_integ5_state(0.0f),
|
||||||
_integ6_state(0.0f),
|
_integ6_state(0.0f),
|
||||||
_integ7_state(0.0f),
|
_integ7_state(0.0f),
|
||||||
_pitch_dem(0.0f),
|
|
||||||
_last_pitch_dem(0.0f),
|
_last_pitch_dem(0.0f),
|
||||||
|
_vel_dot(0.0f),
|
||||||
|
_TAS_dem(0.0f),
|
||||||
|
_TAS_dem_last(0.0f),
|
||||||
|
_hgt_dem_in_old(0.0f),
|
||||||
|
_hgt_dem_adj_last(0.0f),
|
||||||
|
_hgt_dem_prev(0.0f),
|
||||||
|
_TAS_dem_adj(0.0f),
|
||||||
|
_STEdotErrLast(0.0f),
|
||||||
|
_climbOutDem(false),
|
||||||
_SPE_dem(0.0f),
|
_SPE_dem(0.0f),
|
||||||
_SKE_dem(0.0f),
|
_SKE_dem(0.0f),
|
||||||
_SPEdot_dem(0.0f),
|
_SPEdot_dem(0.0f),
|
||||||
|
@ -55,9 +54,9 @@ public:
|
||||||
_SKE_est(0.0f),
|
_SKE_est(0.0f),
|
||||||
_SPEdot(0.0f),
|
_SPEdot(0.0f),
|
||||||
_SKEdot(0.0f),
|
_SKEdot(0.0f),
|
||||||
_vel_dot(0.0f),
|
_airspeed_enabled(false),
|
||||||
_STEdotErrLast(0.0f) {
|
_throttle_slewrate(0.0f)
|
||||||
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
bool airspeed_sensor_enabled() {
|
bool airspeed_sensor_enabled() {
|
||||||
|
|
|
@ -130,23 +130,23 @@ private:
|
||||||
int _att_sub; /**< vehicle attitude subscription */
|
int _att_sub; /**< vehicle attitude subscription */
|
||||||
int _attitude_sub; /**< raw rc channels data subscription */
|
int _attitude_sub; /**< raw rc channels data subscription */
|
||||||
int _airspeed_sub; /**< airspeed subscription */
|
int _airspeed_sub; /**< airspeed subscription */
|
||||||
int _control_mode_sub; /**< vehicle status subscription */
|
int _control_mode_sub; /**< vehicle status subscription */
|
||||||
int _params_sub; /**< notification of parameter updates */
|
int _params_sub; /**< notification of parameter updates */
|
||||||
int _manual_control_sub; /**< notification of manual control updates */
|
int _manual_control_sub; /**< notification of manual control updates */
|
||||||
int _sensor_combined_sub; /**< for body frame accelerations */
|
int _sensor_combined_sub; /**< for body frame accelerations */
|
||||||
|
|
||||||
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
|
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
|
||||||
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
|
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
|
||||||
|
|
||||||
struct vehicle_attitude_s _att; /**< vehicle attitude */
|
struct vehicle_attitude_s _att; /**< vehicle attitude */
|
||||||
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
|
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
|
||||||
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
|
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
|
||||||
struct manual_control_setpoint_s _manual; /**< r/c channel data */
|
struct manual_control_setpoint_s _manual; /**< r/c channel data */
|
||||||
struct airspeed_s _airspeed; /**< airspeed */
|
struct airspeed_s _airspeed; /**< airspeed */
|
||||||
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
|
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
|
||||||
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||||
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
|
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
|
||||||
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
|
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
|
||||||
|
|
||||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||||
|
|
||||||
|
@ -154,13 +154,13 @@ private:
|
||||||
|
|
||||||
/** manual control states */
|
/** manual control states */
|
||||||
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
|
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
|
||||||
float _loiter_hold_lat;
|
double _loiter_hold_lat;
|
||||||
float _loiter_hold_lon;
|
double _loiter_hold_lon;
|
||||||
float _loiter_hold_alt;
|
float _loiter_hold_alt;
|
||||||
bool _loiter_hold;
|
bool _loiter_hold;
|
||||||
|
|
||||||
float _launch_lat;
|
double _launch_lat;
|
||||||
float _launch_lon;
|
double _launch_lon;
|
||||||
float _launch_alt;
|
float _launch_alt;
|
||||||
bool _launch_valid;
|
bool _launch_valid;
|
||||||
|
|
||||||
|
@ -192,7 +192,7 @@ private:
|
||||||
uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures
|
uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures
|
||||||
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
|
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
|
||||||
bool _global_pos_valid; ///< global position is valid
|
bool _global_pos_valid; ///< global position is valid
|
||||||
math::Matrix<3, 3> _R_nb; ///< current attitude
|
math::Matrix<3, 3> _R_nb; ///< current attitude
|
||||||
|
|
||||||
ECL_L1_Pos_Controller _l1_control;
|
ECL_L1_Pos_Controller _l1_control;
|
||||||
TECS _tecs;
|
TECS _tecs;
|
||||||
|
@ -362,6 +362,7 @@ FixedwingPositionControl *g_control;
|
||||||
|
|
||||||
FixedwingPositionControl::FixedwingPositionControl() :
|
FixedwingPositionControl::FixedwingPositionControl() :
|
||||||
|
|
||||||
|
_mavlink_fd(-1),
|
||||||
_task_should_exit(false),
|
_task_should_exit(false),
|
||||||
_control_task(-1),
|
_control_task(-1),
|
||||||
|
|
||||||
|
@ -380,38 +381,33 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||||
|
|
||||||
/* performance counters */
|
/* performance counters */
|
||||||
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
|
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
|
||||||
|
|
||||||
/* states */
|
/* states */
|
||||||
_setpoint_valid(false),
|
_setpoint_valid(false),
|
||||||
_loiter_hold(false),
|
_loiter_hold(false),
|
||||||
_airspeed_error(0.0f),
|
|
||||||
_airspeed_valid(false),
|
|
||||||
_groundspeed_undershoot(0.0f),
|
|
||||||
_global_pos_valid(false),
|
|
||||||
land_noreturn_horizontal(false),
|
land_noreturn_horizontal(false),
|
||||||
land_noreturn_vertical(false),
|
land_noreturn_vertical(false),
|
||||||
land_stayonground(false),
|
land_stayonground(false),
|
||||||
land_motor_lim(false),
|
land_motor_lim(false),
|
||||||
land_onslope(false),
|
land_onslope(false),
|
||||||
flare_curve_alt_last(0.0f),
|
|
||||||
_mavlink_fd(-1),
|
|
||||||
launchDetector(),
|
|
||||||
launch_detected(false),
|
launch_detected(false),
|
||||||
usePreTakeoffThrust(false)
|
usePreTakeoffThrust(false),
|
||||||
|
flare_curve_alt_last(0.0f),
|
||||||
|
launchDetector(),
|
||||||
|
_airspeed_error(0.0f),
|
||||||
|
_airspeed_valid(false),
|
||||||
|
_groundspeed_undershoot(0.0f),
|
||||||
|
_global_pos_valid(false),
|
||||||
|
_att(),
|
||||||
|
_att_sp(),
|
||||||
|
_nav_capabilities(),
|
||||||
|
_manual(),
|
||||||
|
_airspeed(),
|
||||||
|
_control_mode(),
|
||||||
|
_global_pos(),
|
||||||
|
_pos_sp_triplet(),
|
||||||
|
_sensor_combined()
|
||||||
{
|
{
|
||||||
/* safely initialize structs */
|
|
||||||
vehicle_attitude_s _att = {0};
|
|
||||||
vehicle_attitude_setpoint_s _att_sp = {0};
|
|
||||||
navigation_capabilities_s _nav_capabilities = {0};
|
|
||||||
manual_control_setpoint_s _manual = {0};
|
|
||||||
airspeed_s _airspeed = {0};
|
|
||||||
vehicle_control_mode_s _control_mode = {0};
|
|
||||||
vehicle_global_position_s _global_pos = {0};
|
|
||||||
position_setpoint_triplet_s _pos_sp_triplet = {0};
|
|
||||||
sensor_combined_s _sensor_combined = {0};
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
_nav_capabilities.turn_distance = 0.0f;
|
_nav_capabilities.turn_distance = 0.0f;
|
||||||
|
|
||||||
_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
|
_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
|
||||||
|
@ -587,8 +583,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
|
||||||
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
|
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
|
||||||
|
|
||||||
if (!was_armed && _control_mode.flag_armed) {
|
if (!was_armed && _control_mode.flag_armed) {
|
||||||
_launch_lat = _global_pos.lat / 1e7f;
|
_launch_lat = _global_pos.lat;
|
||||||
_launch_lon = _global_pos.lon / 1e7f;
|
_launch_lon = _global_pos.lon;
|
||||||
_launch_alt = _global_pos.alt;
|
_launch_alt = _global_pos.alt;
|
||||||
_launch_valid = true;
|
_launch_valid = true;
|
||||||
}
|
}
|
||||||
|
@ -916,7 +912,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
|
float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
|
||||||
|
|
||||||
/* avoid climbout */
|
/* avoid climbout */
|
||||||
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
|
if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground)
|
||||||
{
|
{
|
||||||
flare_curve_alt = pos_sp_triplet.current.alt;
|
flare_curve_alt = pos_sp_triplet.current.alt;
|
||||||
land_stayonground = true;
|
land_stayonground = true;
|
||||||
|
@ -1074,13 +1070,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
|
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* climb out control */
|
//XXX not used
|
||||||
bool climb_out = false;
|
|
||||||
|
|
||||||
/* user wants to climb out */
|
/* climb out control */
|
||||||
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
|
// bool climb_out = false;
|
||||||
climb_out = true;
|
//
|
||||||
}
|
// /* user wants to climb out */
|
||||||
|
// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
|
||||||
|
// climb_out = true;
|
||||||
|
// }
|
||||||
|
|
||||||
/* if in seatbelt mode, set airspeed based on manual control */
|
/* if in seatbelt mode, set airspeed based on manual control */
|
||||||
|
|
||||||
|
@ -1287,7 +1285,7 @@ FixedwingPositionControl::task_main()
|
||||||
float turn_distance = _l1_control.switch_distance(100.0f);
|
float turn_distance = _l1_control.switch_distance(100.0f);
|
||||||
|
|
||||||
/* lazily publish navigation capabilities */
|
/* lazily publish navigation capabilities */
|
||||||
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
|
if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
|
||||||
|
|
||||||
/* set new turn distance */
|
/* set new turn distance */
|
||||||
_nav_capabilities.turn_distance = turn_distance;
|
_nav_capabilities.turn_distance = turn_distance;
|
||||||
|
|
|
@ -153,17 +153,16 @@ private:
|
||||||
int _capabilities_sub; /**< notification of vehicle capabilities updates */
|
int _capabilities_sub; /**< notification of vehicle capabilities updates */
|
||||||
int _control_mode_sub; /**< vehicle control mode subscription */
|
int _control_mode_sub; /**< vehicle control mode subscription */
|
||||||
|
|
||||||
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
|
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
|
||||||
orb_advert_t _mission_result_pub; /**< publish mission result topic */
|
orb_advert_t _mission_result_pub; /**< publish mission result topic */
|
||||||
|
|
||||||
struct vehicle_status_s _vstatus; /**< vehicle status */
|
struct vehicle_status_s _vstatus; /**< vehicle status */
|
||||||
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
|
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
|
||||||
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||||
struct home_position_s _home_pos; /**< home position for RTL */
|
struct home_position_s _home_pos; /**< home position for RTL */
|
||||||
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
|
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
|
||||||
struct mission_result_s _mission_result; /**< mission result for commander/mavlink */
|
struct mission_result_s _mission_result; /**< mission result for commander/mavlink */
|
||||||
struct mission_item_s _mission_item; /**< current mission item */
|
struct mission_item_s _mission_item; /**< current mission item */
|
||||||
bool _mission_item_valid; /**< current mission item valid */
|
|
||||||
|
|
||||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||||
|
|
||||||
|
@ -177,21 +176,22 @@ private:
|
||||||
|
|
||||||
class Mission _mission;
|
class Mission _mission;
|
||||||
|
|
||||||
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
|
bool _mission_item_valid; /**< current mission item valid */
|
||||||
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
|
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
|
||||||
|
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
|
||||||
bool _waypoint_position_reached;
|
bool _waypoint_position_reached;
|
||||||
bool _waypoint_yaw_reached;
|
bool _waypoint_yaw_reached;
|
||||||
uint64_t _time_first_inside_orbit;
|
uint64_t _time_first_inside_orbit;
|
||||||
bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
|
bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
|
||||||
bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
|
bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
|
||||||
|
|
||||||
MissionFeasibilityChecker missionFeasiblityChecker;
|
MissionFeasibilityChecker missionFeasiblityChecker;
|
||||||
|
|
||||||
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
|
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
|
||||||
|
|
||||||
bool _pos_sp_triplet_updated;
|
bool _pos_sp_triplet_updated;
|
||||||
|
|
||||||
char *nav_states_str[NAV_STATE_MAX];
|
const char *nav_states_str[NAV_STATE_MAX];
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
float min_altitude;
|
float min_altitude;
|
||||||
|
@ -381,11 +381,11 @@ Navigator::Navigator() :
|
||||||
_global_pos_sub(-1),
|
_global_pos_sub(-1),
|
||||||
_home_pos_sub(-1),
|
_home_pos_sub(-1),
|
||||||
_vstatus_sub(-1),
|
_vstatus_sub(-1),
|
||||||
_control_mode_sub(-1),
|
|
||||||
_params_sub(-1),
|
_params_sub(-1),
|
||||||
_offboard_mission_sub(-1),
|
_offboard_mission_sub(-1),
|
||||||
_onboard_mission_sub(-1),
|
_onboard_mission_sub(-1),
|
||||||
_capabilities_sub(-1),
|
_capabilities_sub(-1),
|
||||||
|
_control_mode_sub(-1),
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_pos_sp_triplet_pub(-1),
|
_pos_sp_triplet_pub(-1),
|
||||||
|
@ -393,22 +393,22 @@ Navigator::Navigator() :
|
||||||
/* performance counters */
|
/* performance counters */
|
||||||
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
|
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
|
||||||
|
|
||||||
/* states */
|
_geofence_violation_warning_sent(false),
|
||||||
_rtl_state(RTL_STATE_NONE),
|
|
||||||
_fence_valid(false),
|
_fence_valid(false),
|
||||||
_inside_fence(true),
|
_inside_fence(true),
|
||||||
_mission(),
|
_mission(),
|
||||||
|
_mission_item_valid(false),
|
||||||
_global_pos_valid(false),
|
_global_pos_valid(false),
|
||||||
_reset_loiter_pos(true),
|
_reset_loiter_pos(true),
|
||||||
_waypoint_position_reached(false),
|
_waypoint_position_reached(false),
|
||||||
_waypoint_yaw_reached(false),
|
_waypoint_yaw_reached(false),
|
||||||
_time_first_inside_orbit(0),
|
_time_first_inside_orbit(0),
|
||||||
_set_nav_state_timestamp(0),
|
|
||||||
_mission_item_valid(false),
|
|
||||||
_need_takeoff(true),
|
_need_takeoff(true),
|
||||||
_do_takeoff(false),
|
_do_takeoff(false),
|
||||||
|
_set_nav_state_timestamp(0),
|
||||||
_pos_sp_triplet_updated(false),
|
_pos_sp_triplet_updated(false),
|
||||||
_geofence_violation_warning_sent(false)
|
/* states */
|
||||||
|
_rtl_state(RTL_STATE_NONE)
|
||||||
{
|
{
|
||||||
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
|
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
|
||||||
_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
|
_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
|
||||||
|
@ -1165,7 +1165,7 @@ Navigator::set_mission_item()
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_do_takeoff) {
|
if (_do_takeoff) {
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt);
|
mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (onboard) {
|
if (onboard) {
|
||||||
|
@ -1321,7 +1321,7 @@ Navigator::set_rtl_item()
|
||||||
|
|
||||||
_pos_sp_triplet.next.valid = false;
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt);
|
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1347,7 +1347,7 @@ Navigator::set_rtl_item()
|
||||||
|
|
||||||
_pos_sp_triplet.next.valid = false;
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
|
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1365,7 +1365,7 @@ Navigator::set_rtl_item()
|
||||||
_mission_item.loiter_direction = 1;
|
_mission_item.loiter_direction = 1;
|
||||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||||
_mission_item.acceptance_radius = _parameters.acceptance_radius;
|
_mission_item.acceptance_radius = _parameters.acceptance_radius;
|
||||||
_mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay;
|
_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
|
||||||
_mission_item.pitch_min = 0.0f;
|
_mission_item.pitch_min = 0.0f;
|
||||||
_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
|
_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
|
||||||
_mission_item.origin = ORIGIN_ONBOARD;
|
_mission_item.origin = ORIGIN_ONBOARD;
|
||||||
|
@ -1374,7 +1374,7 @@ Navigator::set_rtl_item()
|
||||||
|
|
||||||
_pos_sp_triplet.next.valid = false;
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt);
|
mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1528,7 +1528,7 @@ Navigator::check_mission_item_reached()
|
||||||
_time_first_inside_orbit = now;
|
_time_first_inside_orbit = now;
|
||||||
|
|
||||||
if (_mission_item.time_inside > 0.01f) {
|
if (_mission_item.time_inside > 0.01f) {
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside);
|
mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", (double)_mission_item.time_inside);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue