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

Merged release_v1.0.0 into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-05-27 15:28:41 -07:00
parent a2bb8cd766 d7547d388f
commit 4565f57468
102 changed files with 977 additions and 2266 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+89 -82
View File
@@ -252,7 +252,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
*/
void *commander_low_prio_loop(void *arg);
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result);
void answer_command(struct vehicle_command_s &cmd, unsigned result);
int commander_main(int argc, char *argv[])
@@ -463,11 +463,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
/* result of the command */
enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED;
unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
/* request to set different system mode */
switch (cmd->command) {
case VEHICLE_CMD_DO_SET_MODE: {
case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t)cmd->param1;
uint8_t custom_main_mode = (uint8_t)cmd->param2;
@@ -528,15 +528,15 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: {
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// for logic state parameters
@@ -556,7 +556,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// Refuse to arm if preflight checks have failed
if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
cmd_result = VEHICLE_CMD_RESULT_DENIED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
@@ -566,16 +566,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd");
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
}
}
break;
case VEHICLE_CMD_OVERRIDE_GOTO: {
case vehicle_command_s::VEHICLE_CMD_OVERRIDE_GOTO: {
// TODO listen vehicle_command topic directly from navigator (?)
// Increase by 0.5f and rely on the integer cast
@@ -586,12 +586,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
mavlink_log_critical(mavlink_fd, "Pause mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
mavlink_log_critical(mavlink_fd, "Continue mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
@@ -607,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
break;
/* Flight termination */
case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
@@ -649,11 +649,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
warnx("rc loss mode commanded");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
case VEHICLE_CMD_DO_SET_HOME: {
case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
if (use_current) {
@@ -665,10 +665,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
home->timestamp = hrt_absolute_time();
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
@@ -679,10 +679,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
home->timestamp = hrt_absolute_time();
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) {
if (cmd_result == vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED) {
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
@@ -700,16 +700,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE: {
transition_result_t res = TRANSITION_DENIED;
static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL;
static main_state_t main_state_pre_offboard = vehicle_status_s::MAIN_STATE_MANUAL;
if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) {
if (status_local->main_state != vehicle_status_s::MAIN_STATE_OFFBOARD) {
main_state_pre_offboard = status_local->main_state;
}
if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -729,35 +729,35 @@ 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:
case VEHICLE_CMD_PREFLIGHT_STORAGE:
case VEHICLE_CMD_CUSTOM_0:
case VEHICLE_CMD_CUSTOM_1:
case VEHICLE_CMD_CUSTOM_2:
case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
case VEHICLE_CMD_DO_MOUNT_CONTROL:
case VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
case VEHICLE_CMD_DO_MOUNT_CONFIGURE:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
/* ignore commands that handled in low prio loop */
break;
default:
/* Warn about unsupported commands, this makes sense because only commands
* to this component ID (or all) are passed by mavlink. */
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
answer_command(*cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, cmd_result);
}
/* send any requested ACKs */
if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
if (cmd->confirmation > 0 && cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* send acknowledge command */
// XXX TODO
}
@@ -835,6 +835,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
param_t _param_autosave_params = param_find("COM_AUTOS_PAR");
param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
@@ -895,6 +896,7 @@ int commander_thread_main(int argc, char *argv[])
status.condition_landed = true; // initialize to safe value
// We want to accept RC inputs as default
status.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
@@ -1139,7 +1141,7 @@ int commander_thread_main(int argc, char *argv[])
}
// Run preflight check
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check);
if (!status.condition_system_sensors_initialized) {
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
@@ -1154,6 +1156,7 @@ int commander_thread_main(int argc, char *argv[])
int32_t datalink_loss_enabled = false;
int32_t datalink_loss_timeout = 10;
float rc_loss_timeout = 0.5;
int32_t rc_in_off = 0;
int32_t datalink_regain_timeout = 0;
/* Thresholds for engine failure detection */
@@ -1240,6 +1243,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
param_get(_param_rc_loss_timeout, &rc_loss_timeout);
param_get(_param_rc_in_off, &rc_in_off);
status.rc_input_mode = rc_in_off;
param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
@@ -1312,7 +1317,8 @@ int commander_thread_main(int argc, char *argv[])
}
/* provide RC and sensor status feedback to the user */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@@ -1485,20 +1491,20 @@ int commander_thread_main(int argc, char *argv[])
/* Update land detector */
orb_check(land_detector_sub, &updated);
if(updated) {
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
}
if (status.condition_local_altitude_valid) {
if (updated && status.condition_local_altitude_valid) {
if (status.condition_landed != land_detector.landed) {
status.condition_landed = land_detector.landed;
status_changed = true;
if (status.condition_landed) {
mavlink_log_critical(mavlink_fd, "LANDED MODE");
mavlink_log_critical(mavlink_fd, "LANDING DETECTED");
} else {
mavlink_log_critical(mavlink_fd, "IN AIR MODE");
mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
}
}
}
@@ -1714,12 +1720,12 @@ int commander_thread_main(int argc, char *argv[])
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
/* RC input check */
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.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;
mavlink_log_critical(mavlink_fd, "detected RC signal first time");
mavlink_log_info(mavlink_fd, "Detected RC signal first time");
status_changed = true;
} else {
@@ -2566,30 +2572,30 @@ print_reject_arm(const char *msg)
}
}
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
void answer_command(struct vehicle_command_s &cmd, unsigned result)
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command);
tune_negative(true);
break;
case VEHICLE_CMD_RESULT_FAILED:
case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command);
tune_negative(true);
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
/* this needs additional hints to the user - so let other messages pass and be spoken */
mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command);
tune_negative(true);
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command);
tune_negative(true);
break;
@@ -2655,49 +2661,49 @@ void *commander_low_prio_loop(void *arg)
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* ignore commands the high-prio loop handles */
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_MODE ||
cmd.command == vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF ||
cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_SERVO) {
continue;
}
/* only handle low-priority commands here */
switch (cmd.command) {
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_safe(&status, &safety, &armed)) {
if (((int)(cmd.param1)) == 1) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
usleep(100000);
/* reboot */
px4_systemreset(false);
} else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
usleep(100000);
/* reboot to bootloader */
px4_systemreset(true);
} else {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
}
} else {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
}
break;
case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
false /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
break;
} else {
status.calibration_enabled = true;
@@ -2705,21 +2711,21 @@ void *commander_low_prio_loop(void *arg)
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_gyro_calibration(mavlink_fd);
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_mag_calibration(mavlink_fd);
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
/* disable RC control input completely */
status.rc_input_blocked = true;
calib_ret = OK;
@@ -2727,31 +2733,31 @@ void *commander_low_prio_loop(void *arg)
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_trim_calibration(mavlink_fd);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_accel_calibration(mavlink_fd);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_level_calibration(mavlink_fd);
} else if ((int)(cmd.param6) == 1) {
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
} else if ((int)(cmd.param7) == 1) {
/* do esc calibration */
answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_esc_calibration(mavlink_fd, &armed);
} 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);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
/* enable RC control input */
status.rc_input_blocked = false;
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
@@ -2778,7 +2784,8 @@ void *commander_low_prio_loop(void *arg)
checkAirspeed = true;
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
@@ -2789,14 +2796,14 @@ void *commander_low_prio_loop(void *arg)
break;
}
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
if (((int)(cmd.param1)) == 0) {
int ret = param_load_default();
if (ret == OK) {
mavlink_log_info(mavlink_fd, "settings loaded");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "settings load ERROR");
@@ -2810,7 +2817,7 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
}
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
}
} else if (((int)(cmd.param1)) == 1) {
@@ -2824,7 +2831,7 @@ void *commander_low_prio_loop(void *arg)
}
/* do not spam MAVLink, but provide the answer / green led mechanism */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "settings save error");
@@ -2838,14 +2845,14 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
}
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
}
}
break;
}
case VEHICLE_CMD_START_RX_PAIR:
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
/* handled in the IO driver */
break;
@@ -2855,8 +2862,8 @@ void *commander_low_prio_loop(void *arg)
}
/* send any requested ACKs */
if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
&& cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
if (cmd.confirmation > 0 && cmd.command != vehicle_command_s::VEHICLE_CMD_DO_SET_MODE
&& cmd.command != vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM) {
/* send acknowledge command */
// XXX TODO
}