From 8ac4dd04ae75e8d6c7d76db68dfb23dd2e75e327 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 22 Aug 2016 01:03:58 -0400 Subject: [PATCH] implement MAV_CMD_MISSION_START --- msg/mission_result.msg | 1 + src/modules/commander/commander.cpp | 56 +++++++++++----- src/modules/navigator/mission.cpp | 66 +++++++++++-------- src/modules/navigator/mission.h | 11 +--- .../navigator/mission_feasibility_checker.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 12 +++- src/modules/navigator/navigator_mode.h | 2 +- 7 files changed, 96 insertions(+), 54 deletions(-) diff --git a/msg/mission_result.msg b/msg/mission_result.msg index 769015d4c0..8eaf4d2773 100644 --- a/msg/mission_result.msg +++ b/msg/mission_result.msg @@ -1,6 +1,7 @@ uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified uint32 seq_reached # Sequence of the mission item which has been reached uint32 seq_current # Sequence of the current mission item +uint32 seq_total # Total number of mission items bool valid # true if mission is valid bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings bool reached # true if mission has been reached diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 32fc04452c..27aca67c86 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -191,6 +191,8 @@ static struct home_position_s _home = {}; static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX]; static struct commander_state_s internal_state = {}; +struct mission_result_s _mission_result; + static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX; static unsigned _last_mission_instance = 0; struct manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint @@ -1118,7 +1120,31 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s break; } + case vehicle_command_s::VEHICLE_CMD_MISSION_START: { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + + // check if current mission and first item are valid + if (_mission_result.valid) { + + // requested first mission item valid + if (PX4_ISFINITE(cmd->param1) && (cmd->param1 >= 0) && (cmd->param1 < _mission_result.seq_total)) { + + // switch to AUTO_MISSION and ARM + if ((TRANSITION_DENIED != main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state)) + && (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "mission start command"))) { + + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + mavlink_log_critical(&mavlink_log_pub, "Mission start denied"); + } + } + } else { + mavlink_log_critical(&mavlink_log_pub, "Mission start denied, no valid mission"); + } + } + break; case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: @@ -1458,8 +1484,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to mission result topic */ int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - struct mission_result_s mission_result; - memset(&mission_result, 0, sizeof(mission_result)); /* Subscribe to geofence result topic */ int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result)); @@ -2335,10 +2359,10 @@ int commander_thread_main(int argc, char *argv[]) orb_check(mission_result_sub, &updated); if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result); - if (status.mission_failure != mission_result.mission_failure) { - status.mission_failure = mission_result.mission_failure; + if (status.mission_failure != _mission_result.mission_failure) { + status.mission_failure = _mission_result.mission_failure; status_changed = true; if (status.mission_failure) { @@ -2438,8 +2462,9 @@ int commander_thread_main(int argc, char *argv[]) /* Check for mission flight termination */ - if (armed.armed && mission_result.flight_termination && + if (armed.armed && _mission_result.flight_termination && !status_flags.circuit_breaker_flight_termination_disabled) { + armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; @@ -2461,12 +2486,12 @@ int commander_thread_main(int argc, char *argv[]) */ if (status_flags.condition_home_position_valid && (hrt_elapsed_time(&_home.timestamp) > 2000000) && - _last_mission_instance != mission_result.instance_count) { - if (!mission_result.valid) { + _last_mission_instance != _mission_result.instance_count) { + if (!_mission_result.valid) { /* the mission is invalid */ tune_mission_fail(true); warnx("mission fail"); - } else if (mission_result.warning) { + } else if (_mission_result.warning) { /* the mission has a warning */ tune_mission_fail(true); warnx("mission warning"); @@ -2476,7 +2501,7 @@ int commander_thread_main(int argc, char *argv[]) } /* prevent further feedback until the mission changes */ - _last_mission_instance = mission_result.instance_count; + _last_mission_instance = _mission_result.instance_count; } /* RC input check */ @@ -2751,7 +2776,7 @@ int commander_thread_main(int argc, char *argv[]) if (main_state_prev == commander_state_s::MAIN_STATE_POSCTL) { if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF - && mission_result.finished) { + && _mission_result.finished) { main_state_transition(&status, main_state_prev, main_state_prev, &status_flags, &internal_state); } @@ -2850,10 +2875,10 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, &internal_state, - &mavlink_log_pub, + &mavlink_log_pub, (datalink_loss_enabled > 0), - mission_result.finished, - mission_result.stay_in_failsafe, + _mission_result.finished, + _mission_result.stay_in_failsafe, &status_flags, land_detector.landed, (rc_loss_enabled > 0), @@ -3812,6 +3837,7 @@ void *commander_low_prio_loop(void *arg) cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_SERVO || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) { + continue; } @@ -3988,7 +4014,7 @@ void *commander_low_prio_loop(void *arg) } else if (((int)(cmd.param1)) == 1) { #ifdef __PX4_QURT - // TODO FIXME: on snapdragon the save happens to early when the params + // TODO FIXME: on snapdragon the save happens too early when the params // are not set yet. We therefore need to wait some time first. usleep(1000000); #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index de5263b680..d60dfd4a4d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 346b6e3e1e..4aa8ab8257 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -91,6 +91,8 @@ public: MISSION_YAWMODE_BACK_TO_HOME = 3, MISSION_YAWMODE_MAX = 4 }; + + bool set_current_offboard_mission_index(unsigned index); private: /** @@ -108,12 +110,6 @@ private: */ void advance_mission(); - /** - * Check distance to first waypoint (with lat/lon) - * @return true only if it's not too far from home (< MIS_DIST_1WP) - */ - bool check_dist_1wp(); - /** * Set new mission items */ @@ -214,7 +210,7 @@ private: void set_mission_finished(); /** - * Check wether a mission is ready to go + * Check whether a mission is ready to go */ void check_mission_valid(bool force); @@ -258,7 +254,6 @@ private: float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ - float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */ float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, only use if current and previous are valid */ diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index fa9957131e..03c173dd7a 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -182,7 +182,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error) { - /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + /* Check if all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem; const ssize_t len = sizeof(struct mission_item_s); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5741d69c48..40e4530de8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -492,6 +492,14 @@ Navigator::task_main() rep->current.valid = true; rep->next.valid = false; + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { + + if (get_mission_result()->valid && + PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) { + + _mission.set_current_offboard_mission_index(cmd.param1); + } + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { PX4_INFO("got pause/continue command"); } @@ -502,6 +510,7 @@ Navigator::task_main() if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; @@ -727,8 +736,6 @@ Navigator::get_altitude_acceptance_radius() } } - - float Navigator::get_cruising_speed() { @@ -863,6 +870,7 @@ int navigator_main(int argc, char *argv[]) void Navigator::publish_mission_result() { + _mission_result.timestamp = hrt_absolute_time(); _mission_result.instance_count = _mission_instance_count; /* lazily publish the mission result only once available */ diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 0839d9be60..18cfd26db9 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -74,7 +74,7 @@ public: virtual void on_inactive(); /** - * This function is called one time when mode become active, poos_sp_triplet must be initialized here + * This function is called one time when mode become active, pos_sp_triplet must be initialized here */ virtual void on_activation();