fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 02:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into acro2

Browse Source
This commit is contained in:
Anton Babushkin
2014-03-08 22:19:18 +04:00
parent e0fbb0fb60 501dc0cfa7
commit b165e6ba20
137 changed files with 3282 additions and 2538 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+47 -55
View File
@@ -152,6 +152,7 @@ static uint64_t last_print_mode_reject_time = 0;
static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static int parachute_enabled = 0;
static struct vehicle_status_s status;
static struct actuator_armed_s armed;
@@ -429,7 +430,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
@@ -515,7 +516,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
if (safety->safety_switch_available && !safety->safety_off) {
if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
@@ -563,7 +564,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
//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) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
@@ -607,7 +610,6 @@ int commander_thread_main(int argc, char *argv[])
/* not yet initialized */
commander_initialized = false;
bool battery_tune_played = false;
bool arm_tune_played = false;
/* set parameters */
@@ -615,6 +617,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_system_id = param_find("MAV_SYS_ID");
param_t _param_component_id = param_find("MAV_COMP_ID");
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
/* welcome user */
warnx("starting");
@@ -861,10 +864,10 @@ int commander_thread_main(int argc, char *argv[])
/* re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
}
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled);
}
orb_check(sp_man_sub, &updated);
@@ -899,11 +902,13 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(safety), safety_sub, &safety);
// XXX this would be the right approach to do it, but do we *WANT* this?
// /* disarm if safety is now on and still armed */
// if (safety.safety_switch_available && !safety.safety_off) {
// (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
// }
/* disarm if safety is now on and still armed */
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
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_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
}
}
}
/* update global position estimate */
@@ -958,7 +963,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
@@ -1021,14 +1026,12 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
} 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");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
@@ -1102,7 +1105,7 @@ int commander_thread_main(int argc, char *argv[])
/* mark home position as set */
status.condition_home_position_valid = true;
tune_positive();
tune_positive(true);
}
}
@@ -1153,7 +1156,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off) {
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
@@ -1197,8 +1200,9 @@ int commander_thread_main(int argc, char *argv[])
/* evaluate the main state machine according to mode switches */
res = set_main_state_rc(&status);
/* play tune on mode change only if armed, blink LED always */
if (res == TRANSITION_CHANGED) {
tune_positive();
tune_positive(armed.armed);
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
@@ -1252,9 +1256,9 @@ int commander_thread_main(int argc, char *argv[])
// TODO remove this hack
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive();
tune_positive(armed.armed);
}
}
@@ -1309,26 +1313,23 @@ int commander_thread_main(int argc, char *argv[])
/* play arming and battery warning tunes */
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
/* play tune when armed */
if (tune_arm() == OK)
arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
/* play tune on battery warning */
if (tune_low_bat() == OK)
battery_tune_played = true;
set_tune(TONE_ARMING_WARNING_TUNE);
arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
/* play tune on battery critical */
if (tune_critical_bat() == OK)
battery_tune_played = true;
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
} else if (battery_tune_played) {
tune_stop();
battery_tune_played = false;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* play tune on battery warning or failsafe */
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
} else {
set_tune(TONE_STOP_TUNE);
}
/* reset arm_tune_played when disarmed */
if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
arm_tune_played = false;
}
@@ -1423,11 +1424,8 @@ 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_NONE) {
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
rgbled_set_color(RGBLED_COLOR_AMBER);
}
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
rgbled_set_color(RGBLED_COLOR_AMBER);
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
} else {
@@ -1720,15 +1718,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg)
sprintf(s, "#audio: REJECT %s", msg);
mavlink_log_critical(mavlink_fd, s);
// only buzz if armed, because else we're driving people nuts indoors
// they really need to look at the leds as well.
if (status->arming_state == ARMING_STATE_ARMED) {
tune_negative();
} else {
// Always show the led indication
led_negative();
}
/* only buzz if armed, because else we're driving people nuts indoors
they really need to look at the leds as well. */
tune_negative(armed.armed);
}
}
@@ -1742,7 +1734,7 @@ print_reject_arm(const char *msg)
char s[80];
sprintf(s, "#audio: %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative();
tune_negative(true);
}
}
@@ -1750,27 +1742,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive();
tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
tune_negative();
tune_negative(true);
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
tune_negative();
tune_negative(true);
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
tune_negative();
tune_negative(true);
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
tune_negative();
tune_negative(true);
break;
default:
@@ -1910,9 +1902,9 @@ void *commander_low_prio_loop(void *arg)
}
if (calib_ret == OK)
tune_positive();
tune_positive(true);
else
tune_negative();
tune_negative(true);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);