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

Merge branch 'beta' into offboard2

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-05 08:15:32 +01:00
parent 5828f49c60 4499919f76
commit 7861d5e0e5
76 changed files with 1705 additions and 557 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+56 -10
View File
@@ -211,7 +211,7 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status);
void set_control_mode();
void print_reject_mode(const char *msg);
void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
void print_reject_arm(const char *msg);
@@ -370,6 +370,7 @@ int arm()
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
@@ -382,6 +383,7 @@ int disarm()
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
@@ -661,6 +663,8 @@ int commander_thread_main(int argc, char *argv[])
/* vehicle status topic */
memset(&status, 0, sizeof(status));
status.condition_landed = true; // initialize to safe value
// We want to accept RC inputs as default
status.rc_input_blocked = false;
status.main_state = MAIN_STATE_MANUAL;
status.set_nav_state = NAV_STATE_NONE;
status.set_nav_state_timestamp = 0;
@@ -939,10 +943,12 @@ int commander_thread_main(int argc, char *argv[])
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
static bool published_condition_landed_fw = false;
if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
status_changed = true;
published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes
if (status.condition_landed) {
mavlink_log_critical(mavlink_fd, "#audio: LANDED");
@@ -951,6 +957,12 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
}
}
} else {
if (!published_condition_landed_fw) {
status.condition_landed = false; // Fixedwing does not have a landing detector currently
published_condition_landed_fw = true;
status_changed = true;
}
}
/* update battery status */
@@ -1109,7 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* start RC input check */
if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
@@ -1544,7 +1556,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
{
/* main mode switch */
if (!isfinite(sp_man->mode_switch)) {
warnx("mode sw not finite");
/* default to manual if signal is invalid */
status->mode_switch = MODE_SWITCH_MANUAL;
} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
@@ -1634,7 +1646,7 @@ set_main_state_rc(struct vehicle_status_s *status)
break; // changed successfully or already in this state
// else fallback to SEATBELT
print_reject_mode("EASY");
print_reject_mode(status, "EASY");
}
res = main_state_transition(status, MAIN_STATE_SEATBELT);
@@ -1643,7 +1655,7 @@ set_main_state_rc(struct vehicle_status_s *status)
break; // changed successfully or already in this mode
if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
print_reject_mode("SEATBELT");
print_reject_mode(status, "SEATBELT");
// else fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
@@ -1657,7 +1669,7 @@ set_main_state_rc(struct vehicle_status_s *status)
break; // changed successfully or already in this state
// else fallback to SEATBELT (EASY likely will not work too)
print_reject_mode("AUTO");
print_reject_mode(status, "AUTO");
res = main_state_transition(status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
@@ -1676,6 +1688,7 @@ set_main_state_rc(struct vehicle_status_s *status)
}
void
set_control_mode()
{
/* set vehicle_control_mode according to main state and failsafe state */
@@ -1772,16 +1785,25 @@ set_control_mode()
}
void
print_reject_mode(const char *msg)
print_reject_mode(struct vehicle_status_s *status, 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: warning: reject %s", msg);
sprintf(s, "#audio: REJECT %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative();
// 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();
}
}
}
@@ -1929,7 +1951,15 @@ void *commander_low_prio_loop(void *arg)
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_rc_calibration(mavlink_fd);
/* disable RC control input completely */
status.rc_input_blocked = true;
calib_ret = OK;
mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_trim_calibration(mavlink_fd);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
@@ -1940,6 +1970,18 @@ void *commander_low_prio_loop(void *arg)
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
/* enable RC control input */
status.rc_input_blocked = false;
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
}
/* this always succeeds */
calib_ret = OK;
}
if (calib_ret == OK)
@@ -1998,6 +2040,10 @@ void *commander_low_prio_loop(void *arg)
break;
}
case VEHICLE_CMD_START_RX_PAIR:
/* handled in the IO driver */
break;
default:
/* don't answer on unsupported commands, it will be done in main loop */
break;