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

commander: Only update home position if not armed already

This commit is contained in:
Lorenz Meier 2015-06-11 12:22:47 +02:00
parent 9155e8a7fe
commit f2b81ce69a
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -499,10 +499,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
// Transition the arming state
arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command");
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (arming_ret == TRANSITION_CHANGED && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
}
@ -2041,11 +2045,11 @@ int commander_thread_main(int argc, char *argv[])
}
}
//Get current timestamp
/* Get current timestamp */
const hrt_abstime now = hrt_absolute_time();
//First time home position update
if (!status.condition_home_position_valid) {
/* First time home position update - but only if disarmed */
if (!status.condition_home_position_valid && !armed.armed) {
commander_set_home_position(home_pub, _home, local_position, global_position);
}