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

implement MAV_CMD_MISSION_START

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/mission_result.msg
View File

@ -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

56
src/modules/commander/commander.cpp
View File

@ -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

66
src/modules/navigator/mission.cpp
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();

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

@ -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 */

2
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -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);

12
src/modules/navigator/navigator_main.cpp
View File

@ -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 */

2
src/modules/navigator/navigator_mode.h
View File

@ -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();