Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

commander: move some flags out of vehicle_status

Browse Source 
All the removed flags were not used anywhere else than inside the
commander.
This commit is contained in:
Julian Oes
2016-02-26 11:01:12 +00:00
parent 0988fef831
commit 70cff975cc
4 changed files with 88 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_status.msg
-12
View File
@@ -74,26 +74,14 @@ bool is_vtol # True if the system is VTOL capable
bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode
bool in_transition_mode # True if VTOL is doing a transition
float32 avionics_power_rail_voltage # voltage of the avionics power rail
bool rc_signal_found_once
bool rc_signal_lost # true if RC reception lost
uint64 rc_signal_lost_timestamp # Time at which the RC reception was lost
bool rc_signal_lost_cmd # true if RC lost mode is commanded
bool rc_input_blocked # set if RC input should be ignored temporarily
uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping.
bool data_link_lost # datalink to GCS lost
bool data_link_lost_cmd # datalink to GCS lost mode commanded
uint8 data_link_lost_counter # counts unique data link lost events
bool engine_failure # Set to true if an engine failure is detected
bool engine_failure_cmd # Set to true if an engine failure mode is commanded
bool vtol_transition_failure # Set to true if vtol transition failed
bool vtol_transition_failure_cmd # Set to true if vtol transition failure mode is commanded
bool gps_failure # Set to true if a gps failure is detected
bool gps_failure_cmd # Set to true if a gps failure mode is commanded
bool mission_failure # Set to true if mission could not continue/finish
bool barometer_failure # Set to true if a barometer failure is detected
# see SYS_STATUS mavlink message for the following
uint32 onboard_control_sensors_present
src/modules/commander/commander.cpp
+59 -47
View File
@@ -224,6 +224,11 @@ static uint8_t main_state_prev = 0;
static struct status_flags_s status_flags = {};
static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was lost
static float avionics_power_rail_voltage; // voltage of the avionics power rail
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@@ -555,9 +560,9 @@ void usage(const char *reason)
void print_status()
{
warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion");
warnx("power: USB: %s, BRICK: %s", (status.usb_connected) ? "OK" : "NO",
(status.condition_power_input_valid) ? " OK" : "NO");
warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
warnx("power: USB: %s, BRICK: %s", (status_flags.usb_connected) ? "[OK]" : "[NO]",
(status_flags.condition_power_input_valid) ? " [OK]" : "[NO]");
warnx("avionics rail: %6.2f V", (double)avionics_power_rail_voltage);
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f, yaw: %.2f", _home.lat, _home.lon, (double)_home.alt, (double)_home.yaw);
warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z);
warnx("datalink: %s", (status.data_link_lost) ? "LOST" : "OK");
@@ -634,7 +639,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
&armed,
true /* fRunPreArmChecks */,
mavlink_log_pub_local,
&status_flags);
&status_flags,
avionics_power_rail_voltage);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -880,10 +886,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* param2 is currently used for other failsafe modes */
status_local->engine_failure_cmd = false;
status_local->data_link_lost_cmd = false;
status_local->gps_failure_cmd = false;
status_local->rc_signal_lost_cmd = false;
status_local->vtol_transition_failure_cmd = false;
status_flags.data_link_lost_cmd = false;
status_flags.gps_failure_cmd = false;
status_flags.rc_signal_lost_cmd = false;
status_flags.vtol_transition_failure_cmd = false;
if ((int)cmd->param2 <= 0) {
/* reset all commanded failure modes */
@@ -896,22 +902,22 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else if ((int)cmd->param2 == 2) {
/* trigger data link loss mode */
status_local->data_link_lost_cmd = true;
status_flags.data_link_lost_cmd = true;
warnx("data link loss mode commanded");
} else if ((int)cmd->param2 == 3) {
/* trigger gps loss mode */
status_local->gps_failure_cmd = true;
status_flags.gps_failure_cmd = true;
warnx("gps loss mode commanded");
} else if ((int)cmd->param2 == 4) {
/* trigger rc loss mode */
status_local->rc_signal_lost_cmd = true;
status_flags.rc_signal_lost_cmd = true;
warnx("rc loss mode commanded");
} else if ((int)cmd->param2 == 5) {
/* trigger vtol transition failure mode */
status_local->vtol_transition_failure_cmd = true;
status_flags.vtol_transition_failure_cmd = true;
warnx("vtol transition failure mode commanded");
}
@@ -1222,7 +1228,7 @@ int commander_thread_main(int argc, char *argv[])
memset(&status, 0, sizeof(status));
// We want to accept RC inputs as default
status.rc_input_blocked = false;
status_flags.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
main_state_prev = vehicle_status_s::MAIN_STATE_MAX;
@@ -1238,7 +1244,7 @@ int commander_thread_main(int argc, char *argv[])
/* neither manual nor offboard control commands have been received */
status_flags.offboard_control_signal_found_once = false;
status.rc_signal_found_once = false;
status_flags.rc_signal_found_once = false;
/* mark all signals lost as long as they haven't been found */
status.rc_signal_lost = true;
@@ -1251,7 +1257,7 @@ int commander_thread_main(int argc, char *argv[])
status.timestamp = hrt_absolute_time();
status_flags.condition_power_input_valid = true;
status.avionics_power_rail_voltage = -1.0f;
avionics_power_rail_voltage = -1.0f;
status_flags.usb_connected = false;
// CIRCUIT BREAKERS
@@ -1706,15 +1712,15 @@ int commander_thread_main(int argc, char *argv[])
* */
if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) {
/* handle the case where baro was regained */
if (status.barometer_failure) {
status.barometer_failure = false;
if (status_flags.barometer_failure) {
status_flags.barometer_failure = false;
status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "baro healthy");
}
} else {
if (!status.barometer_failure) {
status.barometer_failure = true;
if (!status_flags.barometer_failure) {
status_flags.barometer_failure = true;
status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "baro failed");
}
@@ -1744,7 +1750,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* copy avionics voltage */
status.avionics_power_rail_voltage = system_power.voltage5V_v;
avionics_power_rail_voltage = system_power.voltage5V_v;
/* if the USB hardware connection went away, reboot */
if (status_flags.usb_connected && !system_power.usb_connected) {
@@ -1782,7 +1788,8 @@ int commander_thread_main(int argc, char *argv[])
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags)) {
&status_flags,
avionics_power_rail_voltage)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
@@ -1814,8 +1821,8 @@ int commander_thread_main(int argc, char *argv[])
if (is_vtol(&status)) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
status.in_transition_mode = vtol_status.vtol_in_trans_mode;
status.vtol_transition_failure = vtol_status.vtol_transition_failsafe;
status.vtol_transition_failure_cmd = vtol_status.vtol_transition_failsafe;
status_flags.vtol_transition_failure = vtol_status.vtol_transition_failsafe;
status_flags.vtol_transition_failure_cmd = vtol_status.vtol_transition_failsafe;
}
status_changed = true;
@@ -2039,7 +2046,8 @@ int commander_thread_main(int argc, char *argv[])
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags);
&status_flags,
avionics_power_rail_voltage);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -2082,26 +2090,26 @@ int commander_thread_main(int argc, char *argv[])
//Check if GPS receiver is too noisy while we are disarmed
if (!armed.armed && gpsIsNoisy) {
if (!status.gps_failure) {
if (!status_flags.gps_failure) {
mavlink_log_critical(&mavlink_log_pub, "GPS signal noisy");
set_tune_override(TONE_GPS_WARNING_TUNE);
//GPS suffers from signal jamming or excessive noise, disable GPS-aided flight
status.gps_failure = true;
status_flags.gps_failure = true;
status_changed = true;
}
}
if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
/* handle the case where gps was regained */
if (status.gps_failure && !gpsIsNoisy) {
status.gps_failure = false;
if (status_flags.gps_failure && !gpsIsNoisy) {
status_flags.gps_failure = false;
status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "gps fix regained");
}
} else if (!status.gps_failure) {
status.gps_failure = true;
} else if (!status_flags.gps_failure) {
status_flags.gps_failure = true;
status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "gps fix lost");
}
@@ -2257,17 +2265,17 @@ int commander_thread_main(int argc, char *argv[])
}
/* RC input check */
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
if (!status_flags.rc_input_blocked && sp_man.timestamp != 0 &&
(hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
if (!status_flags.rc_signal_found_once) {
status_flags.rc_signal_found_once = true;
status_changed = true;
} else {
if (status.rc_signal_lost) {
mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums",
(hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000);
(hrt_absolute_time() - rc_signal_lost_timestamp) / 1000);
status_changed = true;
}
}
@@ -2294,7 +2302,8 @@ int commander_thread_main(int argc, char *argv[])
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags);
&status_flags,
avionics_power_rail_voltage);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -2336,7 +2345,8 @@ int commander_thread_main(int argc, char *argv[])
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags);
&status_flags,
avionics_power_rail_voltage);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -2405,10 +2415,10 @@ int commander_thread_main(int argc, char *argv[])
/* no else case: do not change lockdown flag in unconfigured case */
} else {
if (!status.rc_input_blocked && !status.rc_signal_lost) {
if (!status_flags.rc_input_blocked && !status.rc_signal_lost) {
mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);
status.rc_signal_lost = true;
status.rc_signal_lost_timestamp = sp_man.timestamp;
rc_signal_lost_timestamp = sp_man.timestamp;
status_changed = true;
}
}
@@ -2552,8 +2562,8 @@ int commander_thread_main(int argc, char *argv[])
status.main_state !=vehicle_status_s::MAIN_STATE_STAB &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
((status.data_link_lost && status.gps_failure) ||
(status.data_link_lost_cmd && status.gps_failure_cmd))) {
((status.data_link_lost && status_flags.gps_failure) ||
(status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
@@ -2578,8 +2588,8 @@ int commander_thread_main(int argc, char *argv[])
status.main_state ==vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
((status.rc_signal_lost && status_flags.gps_failure) ||
(status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
@@ -3529,7 +3539,8 @@ void *commander_low_prio_loop(void *arg)
&armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags)) {
&status_flags,
avionics_power_rail_voltage)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack);
break;
} else {
@@ -3554,7 +3565,7 @@ void *commander_low_prio_loop(void *arg)
/* RC calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
/* disable RC control input completely */
status.rc_input_blocked = true;
status_flags.rc_input_blocked = true;
calib_ret = OK;
mavlink_log_info(&mavlink_log_pub, "CAL: Disabling RC IN");
@@ -3583,10 +3594,10 @@ void *commander_low_prio_loop(void *arg)
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {
if (status_flags.rc_input_blocked) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
/* enable RC control input */
status.rc_input_blocked = false;
status_flags.rc_input_blocked = false;
mavlink_log_info(&mavlink_log_pub, "CAL: Re-enabling RC IN");
calib_ret = OK;
}
@@ -3621,7 +3632,8 @@ void *commander_low_prio_loop(void *arg)
&armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags);
&status_flags,
avionics_power_rail_voltage);
} else {
tune_negative(true);
src/modules/commander/state_machine_helper.cpp
+17 -16
View File
@@ -114,8 +114,9 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
arming_state_t new_arming_state,
struct actuator_armed_s *armed,
bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
status_flags_s *status_flags)
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
status_flags_s *status_flags,
float avionics_power_rail_voltage)
{
// Double check that our static arrays are still valid
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
@@ -221,17 +222,17 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
if (status_flags->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
if (status_flags->condition_power_input_valid && (avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < AVIONICS_ERROR_VOLTAGE) {
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
if (avionics_power_rail_voltage < 4.5f) {
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < AVIONICS_WARN_VOLTAGE) {
mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
} else if (avionics_power_rail_voltage < 4.9f) {
mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
} else if (avionics_power_rail_voltage > 5.4f) {
mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage);
feedback_provided = true;
}
}
@@ -613,7 +614,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !landed) {
if ((status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {
status->failsafe = true;
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
@@ -670,21 +671,21 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
/* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
} else if (status_flags->data_link_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->gps_failure_cmd) {
} else if (status_flags->gps_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
} else if (status_flags->rc_signal_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->vtol_transition_failure_cmd) {
} else if (status_flags->vtol_transition_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
/* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
} else if (status_flags->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->vtol_transition_failure) {
} else if (status_flags->vtol_transition_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
} else if (status->mission_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
src/modules/commander/state_machine_helper.h
+12 -2
View File
@@ -79,17 +79,27 @@ struct status_flags_s {
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;
bool offboard_control_set_by_command; // true if the offboard mode was set by a mavlink command and should not be overridden by RC
bool rc_signal_found_once;
bool rc_signal_lost_cmd; // true if RC lost mode is commanded
bool rc_input_blocked; // set if RC input should be ignored temporarily
bool data_link_lost_cmd; // datalink to GCS lost mode commanded
bool vtol_transition_failure; // Set to true if vtol transition failed
bool vtol_transition_failure_cmd; // Set to true if vtol transition failure mode is commanded
bool gps_failure; // Set to true if a gps failure is detected
bool gps_failure_cmd; // Set to true if a gps failure mode is commanded
bool barometer_failure; // Set to true if a barometer failure is detected
};
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state,
transition_result_t arming_state_transition(struct vehicle_status_s *status,
const struct safety_s *safety,
arming_state_t new_arming_state,
struct actuator_armed_s *armed,
bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub,
status_flags_s *status_flags);
status_flags_s *status_flags,
float avionics_power_rail_voltage);
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,