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

reset mission after vehicle was in mission and then disarms

Browse Source
This commit is contained in:
Andreas Antener 2016-03-20 13:40:19 +01:00 committed by Lorenz Meier
parent fbd2edaae5
commit d9cabde13e
2 changed files with 93 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
src/modules/navigator/mission.cpp
View File

@ -80,6 +80,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_home_inited(false),
_need_mission_reset(false),
_missionFeasibilityChecker(),
_min_current_sp_distance_xy(FLT_MAX),
_mission_item_previous_alt(NAN),
@ -111,6 +112,12 @@ Mission::on_inactive()
update_offboard_mission();
}
/* reset the current offboard mission if needed */
if (need_to_reset_mission(false)) {
reset_offboard_mission(_offboard_mission);
update_offboard_mission();
}
} else {
/* load missions from storage */
@ -164,6 +171,13 @@ Mission::on_active()
update_offboard_mission();
}
/* reset the current offboard mission if needed */
if (need_to_reset_mission(true)) {
reset_offboard_mission(_offboard_mission);
update_offboard_mission();
offboard_updated = true;
}
/* reset mission items if needed */
if (onboard_updated || offboard_updated) {
set_mission_items();
@ -1038,3 +1052,71 @@ Mission::check_mission_valid()
return _navigator->get_mission_result()->valid;
}
void
Mission::reset_offboard_mission(struct mission_s &mission)
{
dm_lock(DM_KEY_MISSION_STATE);
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
/* set current item to 0 */
mission.current_seq = 0;
/* reset jump counters */
if (mission.count > 0) {
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
for (int index = 0; index < mission.count; index++) {
struct mission_item_s item;
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, index, &item, len) != len) {
PX4_WARN("could not read mission item during reset");
break;
}
if (item.nav_cmd == NAV_CMD_DO_JUMP) {
item.do_jump_current_count = 0;
if (dm_write(dm_current, index, DM_PERSIST_POWER_ON_RESET,
&item, len) != len) {
PX4_WARN("could not save mission item during reset");
break;
}
}
}
}
} else {
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "ERROR: could not read mission");
/* initialize mission state in dataman */
mission.dataman_id = 0;
mission.count = 0;
mission.current_seq = 0;
}
dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
mavlink_and_console_log_info(_navigator->get_mavlink_fd(), "mission reset");
}
dm_unlock(DM_KEY_MISSION_STATE);
}
bool
Mission::need_to_reset_mission(bool active)
{
/* reset mission state when disarmed */
if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) {
_need_mission_reset = false;
return true;
} else if (_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED && active) {
/* mission is running, need reset after disarm */
_need_mission_reset = true;
}
return false;
}

11
src/modules/navigator/mission.h
View File

@ -203,6 +203,16 @@ private:
*/
bool check_mission_valid();
/**
* Reset offboard mission
*/
void reset_offboard_mission(struct mission_s &mission);
/**
* Returns true if we need to reset the mission
*/
bool need_to_reset_mission(bool active);
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
control::BlockParamFloat _param_dist_1wp;
@ -224,6 +234,7 @@ private:
bool _inited;
bool _home_inited;
bool _need_mission_reset;
MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */