dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Load missions correctly after restart. Fixes #2564

This commit is contained in:
Lorenz Meier 2015-07-17 20:35:07 +02:00
parent 482641e403
commit 253b8f50ce
2 changed files with 53 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -70,8 +70,8 @@ Mission::Mission(Navigator *navigator, const char *name) :
_param_dist_1wp(this, "MIS_DIST_1WP", false),
_param_altmode(this, "MIS_ALTMODE", false),
_param_yawmode(this, "MIS_YAWMODE", false),
_onboard_mission({0}),
_offboard_mission({0}),
_onboard_mission{},
_offboard_mission{},
_current_onboard_mission_index(-1),
_current_offboard_mission_index(-1),
_need_takeoff(true),
@ -110,32 +110,31 @@ Mission::on_inactive()
update_offboard_mission();
}
/* check if the home position became valid in the meantime */
if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) &&
!_home_inited && _navigator->home_position_valid()) {
} else {
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
/* load missions from storage */
mission_s mission_state;
_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
dm_lock(DM_KEY_MISSION_STATE);
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
/* read current state */
int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
_home_inited = true;
dm_unlock(DM_KEY_MISSION_STATE);
if (read_res == sizeof(mission_s)) {
_offboard_mission.dataman_id = mission_state.dataman_id;
_offboard_mission.count = mission_state.count;
_current_offboard_mission_index = mission_state.current_seq;
warnx("LOADED MISSION STATE FROM STORAGE");
}
} else {
/* read mission topics on initialization */
_inited = true;
update_onboard_mission();
update_offboard_mission();
}
check_mission_valid();
/* require takeoff after non-loiter or landing */
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
@ -151,6 +150,8 @@ Mission::on_activation()
void
Mission::on_active()
{
check_mission_valid();
/* check if anything has changed */
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@ -272,6 +273,8 @@ Mission::update_offboard_mission()
_offboard_mission.count = 0;
_offboard_mission.current_seq = 0;
_current_offboard_mission_index = 0;
warnx("mission check failed");
}
set_current_offboard_mission_item();
@ -652,6 +655,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds */
warnx("err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
return false;
}
@ -795,3 +799,26 @@ Mission::set_mission_finished()
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
}
bool
Mission::check_mission_valid()
{
/* check if the home position became valid in the meantime */
if (!_home_inited && _navigator->home_position_valid()) {
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
_home_inited = true;
}
return _navigator->get_mission_result()->valid;
}

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -39,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Ban Siesta <bansiesta@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#ifndef NAVIGATOR_MISSION_H
@ -165,6 +166,11 @@ private:
*/
void set_mission_finished();
/**
* Check wether a mission is ready to go
*/
bool check_mission_valid();
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
control::BlockParamFloat _param_dist_1wp;