mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-17 06:07:35 +08:00
Formatting commander.cpp to simplify later rework by ensuring formatting match. NO CODE CHANGES
This commit is contained in:
Lorenz Meier
@@ -207,7 +207,9 @@ void usage(const char *reason);
|
||||
/**
|
||||
* React to commands that are sent e.g. from the mavlink module.
|
||||
*/
|
||||
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, 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);
|
||||
|
||||
/**
|
||||
* Mainloop of commander.
|
||||
@@ -230,7 +232,8 @@ void print_reject_arm(const char *msg);
|
||||
|
||||
void print_status();
|
||||
|
||||
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
|
||||
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status,
|
||||
struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
|
||||
|
||||
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
|
||||
|
||||
@@ -393,7 +396,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
|
||||
|
||||
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
|
||||
// output appropriate error messages if the state cannot transition.
|
||||
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
|
||||
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed,
|
||||
true /* fRunPreArmChecks */, mavlink_fd_local);
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
|
||||
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
|
||||
@@ -406,11 +410,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
|
||||
}
|
||||
|
||||
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)
|
||||
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_local->system_id || ((cmd->target_component != status_local->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;
|
||||
}
|
||||
|
||||
@@ -537,13 +542,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
|
||||
(double)cmd->param1,
|
||||
(double)cmd->param2,
|
||||
(double)cmd->param3,
|
||||
(double)cmd->param4,
|
||||
(double)cmd->param5,
|
||||
(double)cmd->param6,
|
||||
(double)cmd->param7);
|
||||
(double)cmd->param1,
|
||||
(double)cmd->param2,
|
||||
(double)cmd->param3,
|
||||
(double)cmd->param4,
|
||||
(double)cmd->param5,
|
||||
(double)cmd->param6,
|
||||
(double)cmd->param7);
|
||||
}
|
||||
}
|
||||
break;
|
||||
@@ -554,35 +559,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
//XXX update state machine?
|
||||
armed_local->force_failsafe = true;
|
||||
warnx("forcing failsafe (termination)");
|
||||
|
||||
} else {
|
||||
armed_local->force_failsafe = false;
|
||||
warnx("disabling failsafe (termination)");
|
||||
}
|
||||
|
||||
/* 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;
|
||||
|
||||
if ((int)cmd->param2 <= 0) {
|
||||
/* reset all commanded failure modes */
|
||||
warnx("reset all non-flighttermination failsafe commands");
|
||||
|
||||
} else if ((int)cmd->param2 == 1) {
|
||||
/* trigger engine failure mode */
|
||||
status_local->engine_failure_cmd = true;
|
||||
warnx("engine failure mode commanded");
|
||||
|
||||
} else if ((int)cmd->param2 == 2) {
|
||||
/* trigger data link loss mode */
|
||||
status_local->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;
|
||||
warnx("gps loss mode commanded");
|
||||
|
||||
} else if ((int)cmd->param2 == 4) {
|
||||
/* trigger rc loss mode */
|
||||
status_local->rc_signal_lost_cmd = true;
|
||||
warnx("rc loss mode commanded");
|
||||
}
|
||||
|
||||
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
}
|
||||
break;
|
||||
@@ -633,21 +646,27 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
|
||||
transition_result_t res = TRANSITION_DENIED;
|
||||
static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
|
||||
|
||||
if (status_local->main_state != MAIN_STATE_OFFBOARD) {
|
||||
main_state_pre_offboard = status_local->main_state;
|
||||
}
|
||||
|
||||
if (cmd->param1 > 0.5f) {
|
||||
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
print_reject_mode(status_local, "OFFBOARD");
|
||||
status_local->offboard_control_set_by_command = false;
|
||||
|
||||
} else {
|
||||
/* Set flag that offboard was set via command, main state is not overridden by rc */
|
||||
status_local->offboard_control_set_by_command = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* If the mavlink command is used to enable or disable offboard control:
|
||||
* switch back to previous mode when disabling */
|
||||
@@ -656,6 +675,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
|
||||
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
|
||||
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
|
||||
@@ -802,6 +822,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
/* publish initial state */
|
||||
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
|
||||
|
||||
if (status_pub < 0) {
|
||||
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
|
||||
warnx("exiting.");
|
||||
@@ -827,11 +848,14 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
|
||||
orb_advert_t mission_pub = -1;
|
||||
mission_s mission;
|
||||
|
||||
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
|
||||
if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
|
||||
warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq);
|
||||
warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count,
|
||||
mission.current_seq);
|
||||
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
|
||||
mission.dataman_id, mission.count, mission.current_seq);
|
||||
mission.dataman_id, mission.count, mission.current_seq);
|
||||
|
||||
} else {
|
||||
const char *missionfail = "reading mission state failed";
|
||||
warnx("%s", missionfail);
|
||||
@@ -1097,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
status.offboard_control_signal_lost = false;
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.offboard_control_signal_lost) {
|
||||
status.offboard_control_signal_lost = true;
|
||||
@@ -1115,9 +1140,9 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
/* perform system checks when new telemetry link connected */
|
||||
if (mavlink_fd &&
|
||||
telemetry_last_heartbeat[i] == 0 &&
|
||||
telemetry.heartbeat_time > 0 &&
|
||||
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
|
||||
telemetry_last_heartbeat[i] == 0 &&
|
||||
telemetry.heartbeat_time > 0 &&
|
||||
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
|
||||
|
||||
(void)rc_calibration_check(mavlink_fd);
|
||||
}
|
||||
@@ -1130,6 +1155,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
|
||||
|
||||
/* Check if the barometer is healthy and issue a warning in the GCS if not so.
|
||||
* Because the barometer is used for calculating AMSL altitude which is used to ensure
|
||||
* vertical separation from other airtraffic the operator has to know when the
|
||||
@@ -1142,6 +1168,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
status_changed = true;
|
||||
mavlink_log_critical(mavlink_fd, "baro healthy");
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.barometer_failure) {
|
||||
status.barometer_failure = true;
|
||||
@@ -1164,10 +1191,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
|
||||
if (system_power.servo_valid &&
|
||||
!system_power.brick_valid &&
|
||||
!system_power.usb_connected) {
|
||||
!system_power.brick_valid &&
|
||||
!system_power.usb_connected) {
|
||||
/* flying only on servo rail, this is unsafe */
|
||||
status.condition_power_input_valid = false;
|
||||
|
||||
} else {
|
||||
status.condition_power_input_valid = true;
|
||||
}
|
||||
@@ -1187,9 +1215,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
/* 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);
|
||||
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, true /* fRunPreArmChecks */, mavlink_fd)) {
|
||||
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
|
||||
true /* fRunPreArmChecks */, mavlink_fd)) {
|
||||
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
|
||||
arming_state_changed = true;
|
||||
}
|
||||
@@ -1233,7 +1263,8 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
|
||||
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
|
||||
&status_changed);
|
||||
|
||||
/* update home position */
|
||||
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
|
||||
@@ -1283,8 +1314,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||
local_eph_good = false;
|
||||
}
|
||||
}
|
||||
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(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);
|
||||
|
||||
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid
|
||||
&& local_eph_good, &(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);
|
||||
|
||||
if (status.condition_local_altitude_valid) {
|
||||
if (status.condition_landed != local_position.landed) {
|
||||
@@ -1315,7 +1349,8 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
/* get throttle (if armed), as we only care about energy negative throttle also counts */
|
||||
float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
|
||||
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
|
||||
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah,
|
||||
throttle);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1383,26 +1418,30 @@ int commander_thread_main(int argc, char *argv[])
|
||||
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
|
||||
status_changed = true;
|
||||
|
||||
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
|
||||
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
|
||||
&& !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_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
|
||||
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
|
||||
|
||||
if (armed.armed) {
|
||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
|
||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
|
||||
mavlink_fd);
|
||||
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
arming_state_changed = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
|
||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
|
||||
mavlink_fd);
|
||||
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
arming_state_changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
@@ -1411,7 +1450,8 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* If in INIT state, try to proceed to STANDBY state */
|
||||
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
|
||||
/* TODO: check for sensors */
|
||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
|
||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
|
||||
mavlink_fd);
|
||||
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
arming_state_changed = true;
|
||||
@@ -1439,23 +1479,25 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
/* Initialize map projection if gps is valid */
|
||||
if (!map_projection_global_initialized()
|
||||
&& (gps_position.eph < eph_threshold)
|
||||
&& (gps_position.epv < epv_threshold)
|
||||
&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
|
||||
&& (gps_position.eph < eph_threshold)
|
||||
&& (gps_position.epv < epv_threshold)
|
||||
&& hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) {
|
||||
/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
|
||||
globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
|
||||
globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
|
||||
(float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
|
||||
}
|
||||
|
||||
/* check if GPS fix is ok */
|
||||
if (status.circuit_breaker_engaged_gpsfailure_check ||
|
||||
(gps_position.fix_type >= 3 &&
|
||||
hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
|
||||
(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) {
|
||||
status.gps_failure = false;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(mavlink_fd, "gps regained");
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.gps_failure) {
|
||||
status.gps_failure = true;
|
||||
@@ -1477,12 +1519,14 @@ int commander_thread_main(int argc, char *argv[])
|
||||
armed.force_failsafe = true;
|
||||
status_changed = true;
|
||||
static bool flight_termination_printed = false;
|
||||
|
||||
if (!flight_termination_printed) {
|
||||
warnx("Flight termination because of navigator request or geofence");
|
||||
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
|
||||
flight_termination_printed = true;
|
||||
}
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
|
||||
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
||||
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
|
||||
}
|
||||
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
|
||||
@@ -1490,7 +1534,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
/* RC input check */
|
||||
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
|
||||
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
|
||||
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;
|
||||
@@ -1515,11 +1559,15 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
|
||||
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
|
||||
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
|
||||
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
|
||||
ARMING_STATE_STANDBY_ERROR);
|
||||
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
|
||||
mavlink_fd);
|
||||
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
arming_state_changed = true;
|
||||
}
|
||||
|
||||
stick_off_counter = 0;
|
||||
|
||||
} else {
|
||||
@@ -1541,8 +1589,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||
*/
|
||||
if (status.main_state != MAIN_STATE_MANUAL) {
|
||||
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
|
||||
|
||||
} else {
|
||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
|
||||
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
|
||||
mavlink_fd);
|
||||
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
arming_state_changed = true;
|
||||
}
|
||||
@@ -1565,6 +1616,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "DISARMED by RC");
|
||||
}
|
||||
|
||||
arming_state_changed = true;
|
||||
|
||||
} else if (arming_ret == TRANSITION_DENIED) {
|
||||
@@ -1600,18 +1652,20 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
/* data links check */
|
||||
bool have_link = false;
|
||||
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
if (telemetry_last_heartbeat[i] != 0 &&
|
||||
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
|
||||
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
|
||||
/* handle the case where data link was regained,
|
||||
* accept datalink as healthy only after datalink_regain_timeout seconds
|
||||
* */
|
||||
if (telemetry_lost[i] &&
|
||||
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
|
||||
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
|
||||
telemetry_lost[i] = false;
|
||||
have_link = true;
|
||||
|
||||
} else if (!telemetry_lost[i]) {
|
||||
/* telemetry was healthy also in last iteration
|
||||
* we don't have to check a timeout */
|
||||
@@ -1620,6 +1674,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
} else {
|
||||
telemetry_last_dl_loss[i] = hrt_absolute_time();
|
||||
|
||||
if (!telemetry_lost[i]) {
|
||||
mavlink_log_critical(mavlink_fd, "data link %i lost", i);
|
||||
telemetry_lost[i] = true;
|
||||
@@ -1647,24 +1702,26 @@ int commander_thread_main(int argc, char *argv[])
|
||||
* only for fixed wing for now
|
||||
*/
|
||||
if (!status.circuit_breaker_engaged_enginefailure_check &&
|
||||
status.is_rotary_wing == false &&
|
||||
armed.armed &&
|
||||
((actuator_controls.control[3] > ef_throttle_thres &&
|
||||
battery.current_a/actuator_controls.control[3] <
|
||||
ef_current2throttle_thres) ||
|
||||
(status.engine_failure))) {
|
||||
status.is_rotary_wing == false &&
|
||||
armed.armed &&
|
||||
((actuator_controls.control[3] > ef_throttle_thres &&
|
||||
battery.current_a / actuator_controls.control[3] <
|
||||
ef_current2throttle_thres) ||
|
||||
(status.engine_failure))) {
|
||||
/* potential failure, measure time */
|
||||
if (timestamp_engine_healthy > 0 &&
|
||||
hrt_elapsed_time(×tamp_engine_healthy) >
|
||||
ef_time_thres * 1e6 &&
|
||||
!status.engine_failure) {
|
||||
status.engine_failure = true;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(mavlink_fd, "Engine Failure");
|
||||
hrt_elapsed_time(×tamp_engine_healthy) >
|
||||
ef_time_thres * 1e6 &&
|
||||
!status.engine_failure) {
|
||||
status.engine_failure = true;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(mavlink_fd, "Engine Failure");
|
||||
}
|
||||
|
||||
} else {
|
||||
/* no failure reset flag */
|
||||
timestamp_engine_healthy = hrt_absolute_time();
|
||||
|
||||
if (status.engine_failure) {
|
||||
status.engine_failure = false;
|
||||
status_changed = true;
|
||||
@@ -1691,20 +1748,22 @@ int commander_thread_main(int argc, char *argv[])
|
||||
* If we are not in a manual (RC stick controlled mode)
|
||||
* and both failed we want to terminate the flight */
|
||||
if (status.main_state != MAIN_STATE_MANUAL &&
|
||||
status.main_state != MAIN_STATE_ACRO &&
|
||||
status.main_state != MAIN_STATE_ALTCTL &&
|
||||
status.main_state != MAIN_STATE_POSCTL &&
|
||||
((status.data_link_lost && status.gps_failure) ||
|
||||
(status.data_link_lost_cmd && status.gps_failure_cmd))) {
|
||||
status.main_state != MAIN_STATE_ACRO &&
|
||||
status.main_state != MAIN_STATE_ALTCTL &&
|
||||
status.main_state != MAIN_STATE_POSCTL &&
|
||||
((status.data_link_lost && status.gps_failure) ||
|
||||
(status.data_link_lost_cmd && status.gps_failure_cmd))) {
|
||||
armed.force_failsafe = true;
|
||||
status_changed = true;
|
||||
static bool flight_termination_printed = false;
|
||||
|
||||
if (!flight_termination_printed) {
|
||||
warnx("Flight termination because of data link loss && gps failure");
|
||||
mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
|
||||
flight_termination_printed = true;
|
||||
}
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
|
||||
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
||||
mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
|
||||
}
|
||||
}
|
||||
@@ -1713,19 +1772,21 @@ int commander_thread_main(int argc, char *argv[])
|
||||
* If we are in manual (controlled with RC):
|
||||
* if both failed we want to terminate the flight */
|
||||
if ((status.main_state == MAIN_STATE_ACRO ||
|
||||
status.main_state == MAIN_STATE_MANUAL ||
|
||||
status.main_state == MAIN_STATE_ALTCTL ||
|
||||
status.main_state == MAIN_STATE_POSCTL) &&
|
||||
((status.rc_signal_lost && status.gps_failure) ||
|
||||
(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
|
||||
status.main_state == MAIN_STATE_MANUAL ||
|
||||
status.main_state == MAIN_STATE_ALTCTL ||
|
||||
status.main_state == MAIN_STATE_POSCTL) &&
|
||||
((status.rc_signal_lost && status.gps_failure) ||
|
||||
(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
|
||||
armed.force_failsafe = true;
|
||||
status_changed = true;
|
||||
static bool flight_termination_printed = false;
|
||||
|
||||
if (!flight_termination_printed) {
|
||||
warnx("Flight termination because of RC signal loss && gps failure");
|
||||
flight_termination_printed = true;
|
||||
}
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
|
||||
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
||||
mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination");
|
||||
}
|
||||
}
|
||||
@@ -1766,6 +1827,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* mark home position as set */
|
||||
status.condition_home_position_valid = true;
|
||||
}
|
||||
|
||||
arming_state_changed = false;
|
||||
}
|
||||
|
||||
@@ -1810,7 +1872,8 @@ 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))) {
|
||||
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available
|
||||
&& safety.safety_off))) {
|
||||
/* play tune when armed */
|
||||
set_tune(TONE_ARMING_WARNING_TUNE);
|
||||
arm_tune_played = true;
|
||||
@@ -1988,6 +2051,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
|
||||
/* offboard switch overrides main switch */
|
||||
if (sp_man->offboard_switch == SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
print_reject_mode(status_local, "OFFBOARD");
|
||||
|
||||
@@ -2009,6 +2073,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
|
||||
} else {
|
||||
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
|
||||
}
|
||||
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
@@ -2023,7 +2088,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
|
||||
print_reject_mode(status_local, "POSCTL");
|
||||
}
|
||||
|
||||
// fallback to ALTCTL
|
||||
// fallback to ALTCTL
|
||||
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
@@ -2047,14 +2112,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
print_reject_mode(status_local, "AUTO_RTL");
|
||||
print_reject_mode(status_local, "AUTO_RTL");
|
||||
|
||||
// fallback to LOITER if home position not set
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||
// fallback to LOITER if home position not set
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
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_local, MAIN_STATE_AUTO_LOITER);
|
||||
@@ -2063,7 +2128,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
print_reject_mode(status_local, "AUTO_LOITER");
|
||||
print_reject_mode(status_local, "AUTO_LOITER");
|
||||
|
||||
} else {
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
|
||||
@@ -2072,22 +2137,22 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
print_reject_mode(status_local, "AUTO_MISSION");
|
||||
print_reject_mode(status_local, "AUTO_MISSION");
|
||||
|
||||
// fallback to LOITER if home position not set
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||
// fallback to LOITER if home position not set
|
||||
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
}
|
||||
|
||||
// fallback to POSCTL
|
||||
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||
// fallback to POSCTL
|
||||
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
// fallback to ALTCTL
|
||||
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
|
||||
@@ -2169,6 +2234,7 @@ set_control_mode()
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = true;
|
||||
@@ -2177,6 +2243,7 @@ set_control_mode()
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = false;
|
||||
@@ -2186,6 +2253,7 @@ set_control_mode()
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
|
||||
@@ -2198,6 +2266,7 @@ set_control_mode()
|
||||
control_mode.flag_control_velocity_enabled = true;
|
||||
//XXX: the flags could depend on sp_offboard.ignore
|
||||
break;
|
||||
|
||||
default:
|
||||
control_mode.flag_control_rates_enabled = false;
|
||||
control_mode.flag_control_attitude_enabled = false;
|
||||
@@ -2206,6 +2275,7 @@ set_control_mode()
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_POSCTL:
|
||||
@@ -2415,7 +2485,8 @@ void *commander_low_prio_loop(void *arg)
|
||||
int calib_ret = ERROR;
|
||||
|
||||
/* try to go to INIT/PREFLIGHT arming state */
|
||||
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
|
||||
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed,
|
||||
true /* fRunPreArmChecks */, mavlink_fd)) {
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
||||
break;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user