From 37531c018a7f789949c0d7514bbb2850ad5b48a2 Mon Sep 17 00:00:00 2001 From: sander Date: Thu, 9 Jun 2016 17:15:57 +0200 Subject: [PATCH] Implement MAV_CMD_DO_CHANGE_SPEED throttle --- msg/position_setpoint.msg | 1 + .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 ++++++++++- src/modules/navigator/mission_block.cpp | 8 ++++++++ src/modules/navigator/navigator.h | 15 +++++++++++++++ src/modules/navigator/navigator_main.cpp | 12 ++++++++++++ 5 files changed, 46 insertions(+), 1 deletion(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index 394885933b..eba8ed9160 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -37,3 +37,4 @@ bool acceleration_valid # true if acceleration setpoint is valid/should be used bool acceleration_is_force # interprete acceleration as force float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation float32 cruising_speed # the generally desired cruising speed (not a hard constraint) +float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ec42455951..eb2c5efe34 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1323,6 +1323,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi mission_airspeed = _pos_sp_triplet.current.cruising_speed; } + float mission_throttle = _parameters.throttle_cruise; + + if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) && + _pos_sp_triplet.current.cruising_throttle > 0.1f) { + mission_throttle = _pos_sp_triplet.current.cruising_throttle; + } + + printf("%f \n", (double)mission_throttle); + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; _att_sp.roll_body = 0.0f; @@ -1336,7 +1345,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(pos_sp_triplet.current.alt, calculate_target_airspeed(mission_airspeed), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.throttle_min, _parameters.throttle_max, mission_throttle, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8c2c460c63..ae1820a4a6 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -127,7 +127,14 @@ MissionBlock::is_mission_item_reached() _navigator->set_cruising_speed(_mission_item.params[1]); } else { _navigator->set_cruising_speed(); + /* if no speed target was given try to set throttle */ + if (_mission_item.params[2] > 0.0f) { + _navigator->set_cruising_throttle(_mission_item.params[2] / 100); + } else { + _navigator->set_cruising_throttle(); + } } + return true; default: @@ -367,6 +374,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->acceptance_radius = item->acceptance_radius; sp->disable_mc_yaw_control = false; sp->cruising_speed = _navigator->get_cruising_speed(); + sp->cruising_throttle = _navigator->get_cruising_throttle(); switch (item->nav_cmd) { case NAV_CMD_IDLE: diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 2e6db02ab4..6092f6cc4a 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -178,6 +178,19 @@ public: */ void set_cruising_speed(float speed=-1.0f) { _mission_cruising_speed = speed; } + + /** + * Get the target throttle + * + * @return the desired throttle for this mission + */ + float get_cruising_throttle(); + + /** + * Set the target throttle + */ + void set_cruising_throttle(float percent=-1.0f) { _mission_throttle = percent; } + /** * Get the acceptance radius given the mission item preset radius * @@ -277,8 +290,10 @@ private: control::BlockParamFloat _param_cruising_speed_hover; control::BlockParamFloat _param_cruising_speed_plane; + control::BlockParamFloat _param_cruising_throttle_plane; float _mission_cruising_speed; + float _mission_throttle; /** * Retrieve global position diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fd7a54ff6e..7d70483561 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -159,6 +159,7 @@ Navigator::Navigator() : _param_rcloss_act(this, "RCL_ACT"), _param_cruising_speed_hover(this, "MPC_XY_CRUISE", false), _param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false), + _param_cruising_throttle_plane(this, "FW_THR_CRUISE", false), _mission_cruising_speed(-1.0f) { /* Create a list of our possible navigation types */ @@ -725,6 +726,17 @@ Navigator::get_cruising_speed() } } +float +Navigator::get_cruising_throttle() +{ + /* Return the mission-requested cruise speed, or -1 */ + if (_mission_throttle > 0.0f) { + return _mission_throttle; + } else { + return _param_cruising_throttle_plane.get(); + } +} + float Navigator::get_acceptance_radius(float mission_item_radius) {