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

implement MAV_CMD_MISSION_START

Browse Source
This commit is contained in:
Daniel Agar
2016-08-22 01:03:58 -04:00
committed by Lorenz Meier
parent 223595e978
commit 8ac4dd04ae
7 changed files with 96 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+39 -27
View File
@@ -249,6 +249,27 @@ Mission::on_active()
}
bool Mission::set_current_offboard_mission_index(unsigned index)
{
if (index < _offboard_mission.count) {
_current_offboard_mission_index = index;
set_current_offboard_mission_item();
// update mission items if already in active mission
if (_navigator->is_planned_mission()) {
// prevent following "previous - current" line
_navigator->get_position_setpoint_triplet()->previous.valid = false;
_navigator->get_position_setpoint_triplet()->current.valid = false;
set_mission_items();
}
return true;
}
return false;
}
void
Mission::update_onboard_mission()
{
@@ -256,6 +277,7 @@ Mission::update_onboard_mission()
/* accept the current index set by the onboard mission if it is within bounds */
if (_onboard_mission.current_seq >= 0
&& _onboard_mission.current_seq < (int)_onboard_mission.count) {
_current_onboard_mission_index = _onboard_mission.current_seq;
} else {
@@ -279,6 +301,7 @@ Mission::update_onboard_mission()
/* reset reached info as well */
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->seq_reached = 0;
_navigator->get_mission_result()->seq_total = _onboard_mission.count;
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
@@ -308,9 +331,8 @@ Mission::update_offboard_mission()
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0;
/* if not initialized, set it to 0 */
} else if (_current_offboard_mission_index < 0) {
/* if not initialized, set it to 0 */
_current_offboard_mission_index = 0;
}
@@ -328,6 +350,7 @@ Mission::update_offboard_mission()
/* reset reached info as well */
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->seq_reached = 0;
_navigator->get_mission_result()->seq_total = _offboard_mission.count;
}
} else {
@@ -464,7 +487,6 @@ Mission::set_mission_items()
if (_navigator->get_land_detected()->landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
} else {
@@ -472,7 +494,6 @@ Mission::set_mission_items()
}
user_feedback_done = true;
}
_navigator->set_position_setpoint_triplet_updated();
@@ -482,8 +503,6 @@ Mission::set_mission_items()
/*********************************** handle mission item *********************************************/
/* handle position mission items */
if (item_contains_position(&_mission_item)) {
/* force vtol land */
@@ -508,7 +527,7 @@ Mission::set_mission_items()
float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home",
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home",
(double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
@@ -538,7 +557,6 @@ Mission::set_mission_items()
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
_mission_item.yaw = _navigator->get_global_position()->yaw;
@@ -550,7 +568,6 @@ Mission::set_mission_items()
_mission_item.yaw = NAN;
_mission_item.time_inside = 0.0f;
}
}
/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
@@ -632,8 +649,6 @@ Mission::set_mission_items()
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
/* ignore yaw for landing items */
/* XXX: if specified heading for landing is desired we could add another step before the descent
* that aligns the vehicle first */
@@ -641,9 +656,8 @@ Mission::set_mission_items()
_mission_item.yaw = NAN;
}
/* handle non-position mission items such as commands */
} else {
/* handle non-position mission items such as commands */
/* turn towards next waypoint before MC to FW transition */
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
@@ -661,7 +675,6 @@ Mission::set_mission_items()
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
set_previous_pos_setpoint();
}
}
/*********************************** set setpoints and check next *********************************************/
@@ -849,7 +862,7 @@ Mission::heading_sp_update()
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid */
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid) {
if (!pos_sp_triplet->current.valid) {
return;
}
@@ -880,9 +893,8 @@ Mission::heading_sp_update()
point_to_latlon[0] = _navigator->get_home_position()->lat;
point_to_latlon[1] = _navigator->get_home_position()->lon;
/* target location is next (current) waypoint */
} else {
/* target location is next (current) waypoint */
point_to_latlon[0] = pos_sp_triplet->current.lat;
point_to_latlon[1] = pos_sp_triplet->current.lon;
}
@@ -966,10 +978,10 @@ Mission::altitude_sp_foh_update()
} else {
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
* of the mission item as it is used to check if the mission item is reached
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
* radius around the current waypoint
**/
* of the mission item as it is used to check if the mission item is reached
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
* radius around the current waypoint
**/
float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt);
float grad = -delta_alt / (_distance_current_previous - _navigator->get_acceptance_radius(
_mission_item.acceptance_radius));
@@ -1000,8 +1012,8 @@ Mission::do_abort_landing()
// loiter at the larger of MIS_LTRMIN_ALT above the landing point
// or 2 * FW_CLMBOUT_DIFF above the current altitude
float alt_landing = get_absolute_altitude_for_item(_mission_item);
float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(),
_navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()));
float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(),
_navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()));
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item.altitude_is_relative = false;
@@ -1018,8 +1030,7 @@ Mission::do_abort_landing()
_navigator->set_position_setpoint_triplet_updated();
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing)",
(int)(alt_sp - alt_landing));
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing)", (int)(alt_sp - alt_landing));
// move mission index back 1 (landing approach point) so that re-entering
// the mission doesn't try to land from the loiter above land
@@ -1113,7 +1124,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) {
/* only raise the repeat count if this is for the current mission item
* but not for the read ahead mission item */
* but not for the read ahead mission item */
if (offset == 0) {
(mission_item_tmp.do_jump_current_count)++;
@@ -1130,7 +1141,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
* we don't have to validate here, if it's invalid, we should realize this later .*/
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
@@ -1259,6 +1270,7 @@ Mission::check_mission_valid(bool force)
_navigator->get_default_acceptance_radius(),
_navigator->get_land_detected()->landed);
_navigator->get_mission_result()->seq_total = _offboard_mission.count;
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
_home_inited = _navigator->home_position_valid();