forked from Archive/PX4-Autopilot
commit
88af1d1dd5
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (c) 2012-2014 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
|
||||
|
|
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
* Copyright (c) 2013 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
|
||||
|
|
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
* Copyright (c) 2013 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
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013, 2014 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
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013 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
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013 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
|
||||
|
|
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
* Copyright (c) 2013 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
|
||||
|
|
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (c) 2012 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
|
||||
|
|
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (c) 2012 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
|
||||
|
|
|
@ -403,10 +403,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
|
|||
return arming_res;
|
||||
}
|
||||
|
||||
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
|
||||
bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
|
||||
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
|
||||
struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
|
||||
{
|
||||
/* only handle commands that are meant to be handled by this system and component */
|
||||
if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
|
||||
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -425,7 +427,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
|
||||
/* set HIL state */
|
||||
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
|
||||
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
|
||||
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");
|
||||
|
@ -434,43 +436,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
/* use autopilot-specific mode */
|
||||
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
|
||||
/* MANUAL */
|
||||
main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
|
||||
/* ALTCTL */
|
||||
main_ret = main_state_transition(status, MAIN_STATE_ALTCTL);
|
||||
main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
|
||||
/* POSCTL */
|
||||
main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
|
||||
main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
|
||||
/* AUTO */
|
||||
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
|
||||
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
|
||||
/* ACRO */
|
||||
main_ret = main_state_transition(status, MAIN_STATE_ACRO);
|
||||
main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
|
||||
/* OFFBOARD */
|
||||
main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD);
|
||||
main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* use base mode */
|
||||
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
|
||||
/* AUTO */
|
||||
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
|
||||
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
|
||||
|
||||
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
|
||||
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
|
||||
/* POSCTL */
|
||||
main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
|
||||
main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||
|
||||
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
|
||||
/* MANUAL */
|
||||
main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -485,22 +487,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
break;
|
||||
|
||||
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
|
||||
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
|
||||
// We use an float epsilon delta to test float equality.
|
||||
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
|
||||
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1);
|
||||
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
|
||||
// for logic state parameters
|
||||
|
||||
if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) {
|
||||
mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
|
||||
|
||||
} else {
|
||||
|
||||
bool cmd_arms = (static_cast<int>(cmd->param1 + 0.5f) == 1);
|
||||
|
||||
// Flick to inair restore first if this comes from an onboard system
|
||||
if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
|
||||
status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
|
||||
if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
|
||||
status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
|
||||
}
|
||||
|
||||
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
|
||||
transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
|
||||
|
||||
if (arming_res == TRANSITION_DENIED) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
|
||||
mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd");
|
||||
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
|
||||
} else {
|
||||
|
@ -512,20 +517,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
|
||||
case VEHICLE_CMD_OVERRIDE_GOTO: {
|
||||
// TODO listen vehicle_command topic directly from navigator (?)
|
||||
unsigned int mav_goto = cmd->param1;
|
||||
|
||||
// Increase by 0.5f and rely on the integer cast
|
||||
// implicit floor(). This is the *safest* way to
|
||||
// convert from floats representing small ints to actual ints.
|
||||
unsigned int mav_goto = (cmd->param1 + 0.5f);
|
||||
|
||||
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
|
||||
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
|
||||
mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
|
||||
status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
|
||||
mavlink_log_critical(mavlink_fd, "Pause mission cmd");
|
||||
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
|
||||
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
|
||||
mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
|
||||
status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
|
||||
mavlink_log_critical(mavlink_fd, "Continue mission cmd");
|
||||
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f",
|
||||
mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
|
||||
(double)cmd->param1,
|
||||
(double)cmd->param2,
|
||||
(double)cmd->param3,
|
||||
|
@ -543,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
|
||||
//XXX: to enable the parachute, a param needs to be set
|
||||
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
|
||||
if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
|
||||
if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
|
||||
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
|
||||
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
|
@ -561,7 +570,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
|
||||
if (use_current) {
|
||||
/* use current position */
|
||||
if (status->condition_global_position_valid) {
|
||||
if (status_local->condition_global_position_valid) {
|
||||
home->lat = global_pos->lat;
|
||||
home->lon = global_pos->lon;
|
||||
home->alt = global_pos->alt;
|
||||
|
@ -598,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
}
|
||||
|
||||
/* mark home position as set */
|
||||
status->condition_home_position_valid = true;
|
||||
status_local->condition_home_position_valid = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -688,11 +697,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* initialize */
|
||||
if (led_init() != 0) {
|
||||
warnx("ERROR: Failed to initialize leds");
|
||||
warnx("ERROR: LED INIT FAIL");
|
||||
}
|
||||
|
||||
if (buzzer_init() != OK) {
|
||||
warnx("ERROR: Failed to initialize buzzer");
|
||||
warnx("ERROR: BUZZER INIT FAIL");
|
||||
}
|
||||
|
||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
@ -766,8 +775,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
|
||||
mission.dataman_id, mission.count, mission.current_seq);
|
||||
} else {
|
||||
warnx("reading mission state failed");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed");
|
||||
const char *missionfail = "reading mission state failed";
|
||||
warnx("%s", missionfail);
|
||||
mavlink_log_critical(mavlink_fd, missionfail);
|
||||
|
||||
/* initialize mission state in dataman */
|
||||
mission.dataman_id = 0;
|
||||
|
@ -780,8 +790,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[cmd] started");
|
||||
|
||||
int ret;
|
||||
|
||||
pthread_attr_t commander_low_prio_attr;
|
||||
|
@ -1074,7 +1082,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
|
||||
|
||||
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
|
||||
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
|
||||
arming_state_changed = true;
|
||||
}
|
||||
}
|
||||
|
@ -1178,10 +1186,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status_changed = true;
|
||||
|
||||
if (status.condition_landed) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: LANDED");
|
||||
mavlink_log_critical(mavlink_fd, "LANDED MODE");
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
|
||||
mavlink_log_critical(mavlink_fd, "IN AIR MODE");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -1261,14 +1269,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* if battery voltage is getting lower, warn using buzzer, etc. */
|
||||
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
|
||||
low_battery_voltage_actions_done = true;
|
||||
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
|
||||
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
|
||||
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
|
||||
status_changed = true;
|
||||
|
||||
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
|
||||
/* critical battery voltage, this is rather an emergency, change state machine */
|
||||
critical_battery_voltage_actions_done = true;
|
||||
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
|
||||
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
|
||||
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
|
||||
|
||||
if (armed.armed) {
|
||||
|
@ -1330,12 +1338,12 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* handle the case where RC signal was regained */
|
||||
if (!status.rc_signal_found_once) {
|
||||
status.rc_signal_found_once = true;
|
||||
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
|
||||
mavlink_log_critical(mavlink_fd, "detected RC signal first time");
|
||||
status_changed = true;
|
||||
|
||||
} else {
|
||||
if (status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
|
||||
mavlink_log_critical(mavlink_fd, "RC signal regained");
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
|
@ -1376,7 +1384,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
* the system can be armed in auto if armed via the GCS.
|
||||
*/
|
||||
if (status.main_state != MAIN_STATE_MANUAL) {
|
||||
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
|
||||
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
|
||||
} else {
|
||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
|
@ -1396,16 +1404,16 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
if (status.arming_state == ARMING_STATE_ARMED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
|
||||
mavlink_log_info(mavlink_fd, "ARMED by RC");
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
|
||||
mavlink_log_info(mavlink_fd, "DISARMED by RC");
|
||||
}
|
||||
arming_state_changed = true;
|
||||
|
||||
} else if (arming_ret == TRANSITION_DENIED) {
|
||||
/* DENIED here indicates bug in the commander */
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
|
||||
mavlink_log_critical(mavlink_fd, "arming state transition denied");
|
||||
tune_negative(true);
|
||||
}
|
||||
|
||||
|
@ -1419,12 +1427,12 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
} else if (main_res == TRANSITION_DENIED) {
|
||||
/* DENIED here indicates bug in the commander */
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
|
||||
mavlink_log_critical(mavlink_fd, "main state transition denied");
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
|
||||
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
|
||||
status.rc_signal_lost = true;
|
||||
status_changed = true;
|
||||
}
|
||||
|
@ -1436,14 +1444,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
|
||||
/* handle the case where data link was regained */
|
||||
if (telemetry_lost[i]) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i);
|
||||
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
|
||||
telemetry_lost[i] = false;
|
||||
}
|
||||
have_link = true;
|
||||
|
||||
} else {
|
||||
if (!telemetry_lost[i]) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i);
|
||||
mavlink_log_critical(mavlink_fd, "data link %i lost", i);
|
||||
telemetry_lost[i] = true;
|
||||
}
|
||||
}
|
||||
|
@ -1458,7 +1466,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
} else {
|
||||
if (!status.data_link_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST");
|
||||
mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
|
||||
status.data_link_lost = true;
|
||||
status_changed = true;
|
||||
}
|
||||
|
@ -1642,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
|
|||
}
|
||||
|
||||
void
|
||||
control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed)
|
||||
control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed)
|
||||
{
|
||||
/* driving rgbled */
|
||||
if (changed) {
|
||||
bool set_normal_color = false;
|
||||
|
||||
/* set mode */
|
||||
if (status->arming_state == ARMING_STATE_ARMED) {
|
||||
if (status_local->arming_state == ARMING_STATE_ARMED) {
|
||||
rgbled_set_mode(RGBLED_MODE_ON);
|
||||
set_normal_color = true;
|
||||
|
||||
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
|
||||
} else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
|
||||
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
|
||||
rgbled_set_color(RGBLED_COLOR_RED);
|
||||
|
||||
} else if (status->arming_state == ARMING_STATE_STANDBY) {
|
||||
} else if (status_local->arming_state == ARMING_STATE_STANDBY) {
|
||||
rgbled_set_mode(RGBLED_MODE_BREATHE);
|
||||
set_normal_color = true;
|
||||
|
||||
|
@ -1668,12 +1676,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
|
|||
|
||||
if (set_normal_color) {
|
||||
/* set color */
|
||||
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) {
|
||||
if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
|
||||
rgbled_set_color(RGBLED_COLOR_AMBER);
|
||||
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
|
||||
|
||||
} else {
|
||||
if (status->condition_local_position_valid) {
|
||||
if (status_local->condition_local_position_valid) {
|
||||
rgbled_set_color(RGBLED_COLOR_GREEN);
|
||||
|
||||
} else {
|
||||
|
@ -1706,7 +1714,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
|
|||
#endif
|
||||
|
||||
/* give system warnings on error LED, XXX maybe add memory usage warning too */
|
||||
if (status->load > 0.95f) {
|
||||
if (status_local->load > 0.95f) {
|
||||
if (leds_counter % 2 == 0) {
|
||||
led_toggle(LED_AMBER);
|
||||
}
|
||||
|
@ -1719,16 +1727,16 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
|
|||
}
|
||||
|
||||
transition_result_t
|
||||
set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man)
|
||||
set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man)
|
||||
{
|
||||
/* set main state according to RC switches */
|
||||
transition_result_t res = TRANSITION_DENIED;
|
||||
|
||||
/* offboard switch overrides main switch */
|
||||
if (sp_man->offboard_switch == SWITCH_POS_ON) {
|
||||
res = main_state_transition(status, MAIN_STATE_OFFBOARD);
|
||||
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
|
||||
if (res == TRANSITION_DENIED) {
|
||||
print_reject_mode(status, "OFFBOARD");
|
||||
print_reject_mode(status_local, "OFFBOARD");
|
||||
|
||||
} else {
|
||||
return res;
|
||||
|
@ -1743,78 +1751,78 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
|||
|
||||
case SWITCH_POS_OFF: // MANUAL
|
||||
if (sp_man->acro_switch == SWITCH_POS_ON) {
|
||||
res = main_state_transition(status, MAIN_STATE_ACRO);
|
||||
res = main_state_transition(status_local, MAIN_STATE_ACRO);
|
||||
|
||||
} else {
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||
}
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
case SWITCH_POS_MIDDLE: // ASSIST
|
||||
if (sp_man->posctl_switch == SWITCH_POS_ON) {
|
||||
res = main_state_transition(status, MAIN_STATE_POSCTL);
|
||||
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
print_reject_mode(status, "POSCTL");
|
||||
print_reject_mode(status_local, "POSCTL");
|
||||
}
|
||||
|
||||
// fallback to ALTCTL
|
||||
res = main_state_transition(status, MAIN_STATE_ALTCTL);
|
||||
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this mode
|
||||
}
|
||||
|
||||
if (sp_man->posctl_switch != SWITCH_POS_ON) {
|
||||
print_reject_mode(status, "ALTCTL");
|
||||
print_reject_mode(status_local, "ALTCTL");
|
||||
}
|
||||
|
||||
// fallback to MANUAL
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
case SWITCH_POS_ON: // AUTO
|
||||
if (sp_man->return_switch == SWITCH_POS_ON) {
|
||||
res = main_state_transition(status, MAIN_STATE_AUTO_RTL);
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
print_reject_mode(status, "AUTO_RTL");
|
||||
print_reject_mode(status_local, "AUTO_RTL");
|
||||
|
||||
// fallback to LOITER if home position not set
|
||||
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
|
||||
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
print_reject_mode(status, "AUTO_LOITER");
|
||||
print_reject_mode(status_local, "AUTO_LOITER");
|
||||
|
||||
} else {
|
||||
res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
print_reject_mode(status, "AUTO_MISSION");
|
||||
print_reject_mode(status_local, "AUTO_MISSION");
|
||||
|
||||
// fallback to LOITER if home position not set
|
||||
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
|
@ -1822,21 +1830,21 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
|||
}
|
||||
|
||||
// fallback to POSCTL
|
||||
res = main_state_transition(status, MAIN_STATE_POSCTL);
|
||||
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
// fallback to ALTCTL
|
||||
res = main_state_transition(status, MAIN_STATE_ALTCTL);
|
||||
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
// fallback to MANUAL
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
|
@ -2001,15 +2009,13 @@ set_control_mode()
|
|||
}
|
||||
|
||||
void
|
||||
print_reject_mode(struct vehicle_status_s *status, const char *msg)
|
||||
print_reject_mode(struct vehicle_status_s *status_local, const char *msg)
|
||||
{
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
|
||||
last_print_mode_reject_time = t;
|
||||
char s[80];
|
||||
sprintf(s, "#audio: REJECT %s", msg);
|
||||
mavlink_log_critical(mavlink_fd, s);
|
||||
mavlink_log_critical(mavlink_fd, "REJECT %s", msg);
|
||||
|
||||
/* only buzz if armed, because else we're driving people nuts indoors
|
||||
they really need to look at the leds as well. */
|
||||
|
@ -2024,9 +2030,7 @@ print_reject_arm(const char *msg)
|
|||
|
||||
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
|
||||
last_print_mode_reject_time = t;
|
||||
char s[80];
|
||||
sprintf(s, "#audio: %s", msg);
|
||||
mavlink_log_critical(mavlink_fd, s);
|
||||
mavlink_log_critical(mavlink_fd, msg);
|
||||
tune_negative(true);
|
||||
}
|
||||
}
|
||||
|
@ -2039,12 +2043,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
|
|||
break;
|
||||
|
||||
case VEHICLE_CMD_RESULT_DENIED:
|
||||
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
|
||||
mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command);
|
||||
tune_negative(true);
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_RESULT_FAILED:
|
||||
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
|
||||
mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command);
|
||||
tune_negative(true);
|
||||
break;
|
||||
|
||||
|
@ -2055,7 +2059,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
|
|||
break;
|
||||
|
||||
case VEHICLE_CMD_RESULT_UNSUPPORTED:
|
||||
mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
|
||||
mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command);
|
||||
tune_negative(true);
|
||||
break;
|
||||
|
||||
|
|
|
@ -1,9 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
* Copyright (c) 2013, 2014 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
|
||||
|
@ -37,6 +34,11 @@
|
|||
/**
|
||||
* @file commander_helper.cpp
|
||||
* Commander helper functions implementations
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
|
|
|
@ -1,8 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Copyright (c) 2013, 2014 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
|
||||
|
@ -36,6 +34,9 @@
|
|||
/**
|
||||
* @file commander_helper.h
|
||||
* Commander helper functions definitions
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#ifndef COMMANDER_HELPER_H_
|
||||
|
@ -77,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
|
|||
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
|
||||
* else use simple estimate based on voltage.
|
||||
*
|
||||
* @param voltage the current battery voltage
|
||||
* @param discharged the discharged capacity
|
||||
* @return the estimated remaining capacity in 0..1
|
||||
*/
|
||||
float battery_remaining_estimate_voltage(float voltage, float discharged);
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013, 2014 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
|
||||
|
|
|
@ -87,7 +87,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
|
|||
};
|
||||
|
||||
// You can index into the array with an arming_state_t in order to get it's textual representation
|
||||
static const char *state_names[ARMING_STATE_MAX] = {
|
||||
static const char * const state_names[ARMING_STATE_MAX] = {
|
||||
"ARMING_STATE_INIT",
|
||||
"ARMING_STATE_STANDBY",
|
||||
"ARMING_STATE_ARMED",
|
||||
|
@ -160,7 +160,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
|
|||
// Fail transition if we need safety switch press
|
||||
} else if (safety->safety_switch_available && !safety->safety_off) {
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!");
|
||||
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
|
||||
|
||||
valid_transition = false;
|
||||
}
|
||||
|
@ -171,16 +171,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current
|
|||
// Fail transition if power is not good
|
||||
if (!status->condition_power_input_valid) {
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
|
||||
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
|
||||
valid_transition = false;
|
||||
}
|
||||
|
||||
// Fail transition if power levels on the avionics rail
|
||||
// are measured but are insufficient
|
||||
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
|
||||
(status->avionics_power_rail_voltage < 4.9f)) {
|
||||
if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
|
||||
(status->avionics_power_rail_voltage < 4.9f))) {
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
|
||||
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
|
||||
valid_transition = false;
|
||||
}
|
||||
}
|
||||
|
@ -625,7 +625,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
|||
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
@ -633,7 +633,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
|||
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
|
||||
|
||||
if (ret != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
@ -647,14 +647,14 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
|||
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
|
||||
|
||||
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
|
||||
mavlink_log_info(mavlink_fd, "#audio: hold still while arming");
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
|
||||
mavlink_log_critical(mavlink_fd, "hold still while arming");
|
||||
/* this is frickin' fatal */
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
|
||||
/* this is frickin' fatal */
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
|
@ -667,7 +667,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
|||
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd <= 0) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
@ -678,11 +678,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
|||
|
||||
if (ret == sizeof(diff_pres)) {
|
||||
if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
|
||||
mavlink_log_critical(mavlink_fd, "ARM WARNING: AIRSPEED CALIBRATION MISSING");
|
||||
// XXX do not make this fatal yet
|
||||
}
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED READ");
|
||||
/* this is frickin' fatal */
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
|
|
|
@ -1,8 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Copyright (c) 2013, 2014 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
|
||||
|
@ -36,14 +34,14 @@
|
|||
/**
|
||||
* @file state_machine_helper.h
|
||||
* State machine helper functions definitions
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#ifndef STATE_MACHINE_HELPER_H_
|
||||
#define STATE_MACHINE_HELPER_H_
|
||||
|
||||
#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
|
||||
#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
|
|
Loading…
Reference in New Issue