diff options
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 253 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.cpp | 7 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.h | 3 | ||||
-rw-r--r-- | src/modules/commander/rc_calibration.cpp | 11 | ||||
-rw-r--r-- | src/modules/commander/rc_calibration.h | 2 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 36 |
6 files changed, 270 insertions, 42 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52a..c039b8573 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,6 +153,11 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static struct vehicle_status_s status; +static struct actuator_armed_s armed; +static struct safety_s safety; +static struct vehicle_control_mode_s control_mode; + /* tasks waiting for low prio thread */ typedef enum { LOW_PRIO_TASK_NONE = 0, @@ -203,12 +208,19 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t set_main_state_rc(struct vehicle_status_s *status); -void print_reject_mode(const char *msg); +void set_control_mode(); + +void print_reject_mode(struct vehicle_status_s *current_status, const char *msg); void print_reject_arm(const char *msg); void print_status(); +int arm(); +int disarm(); + +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); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -274,6 +286,16 @@ int commander_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "arm")) { + arm(); + exit(0); + } + + if (!strcmp(argv[1], "disarm")) { + disarm(); + exit(0); + } + usage("unrecognized command"); exit(1); } @@ -340,6 +362,32 @@ void print_status() static orb_advert_t status_pub; +int arm() +{ + int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + + } else { + return 1; + } +} + +int disarm() +{ + int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + + } else { + return 1; + } +} + bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -554,13 +602,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } -static struct vehicle_status_s status; - -/* armed topic */ -static struct actuator_armed_s armed; - -static struct safety_s safety; - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -613,16 +654,11 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* Main state machine */ - /* make sure we are in preflight state */ + /* vehicle status topic */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value - - /* armed topic */ - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - + // 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; @@ -645,14 +681,20 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - /* publish current state machine */ - - /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + /* publish initial state */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* vehicle control mode topic */ + memset(&control_mode, 0, sizeof(control_mode)); + orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -872,7 +914,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -886,10 +928,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"); @@ -898,6 +942,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 */ @@ -1031,7 +1081,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.valid) { + && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ home.lat = global_position.lat; @@ -1056,7 +1106,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; @@ -1244,8 +1294,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + set_control_mode(); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1422,7 +1477,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) { @@ -1472,7 +1527,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta transition_result_t set_main_state_rc(struct vehicle_status_s *status) { - /* evaluate the main state machine */ + /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; switch (status->mode_switch) { @@ -1489,7 +1544,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); @@ -1498,7 +1553,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); @@ -1512,7 +1567,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) @@ -1531,16 +1586,122 @@ set_main_state_rc(struct vehicle_status_s *status) } void -print_reject_mode(const char *msg) + +set_control_mode() +{ + /* set vehicle_control_mode according to main state and failsafe state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + + control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator should act */ + bool navigator_enabled = false; + + switch (status.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (status.main_state) { + case MAIN_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + + default: + break; + } + + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + } +} + +void +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(); + } } } @@ -1688,7 +1849,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 */ @@ -1699,6 +1868,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) @@ -1757,6 +1938,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; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 21a1c4c2c..033e7dc88 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -124,10 +124,15 @@ void tune_neutral() void tune_negative() { + led_negative(); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); +} + +void led_negative() +{ blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } int tune_arm() diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index d0393f45a..af25a5e97 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -62,6 +62,9 @@ int tune_arm(void); int tune_low_bat(void); int tune_critical_bat(void); void tune_stop(void); + +void led_negative(); + int blink_msg_state(); int led_init(void); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 90ede499a..41f3ca0aa 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -53,17 +53,16 @@ #endif static const int ERROR = -1; -int do_rc_calibration(int mavlink_fd) +int do_trim_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "trim calibration starting"); - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + usleep(200000); struct manual_control_setpoint_s sp; bool changed; orb_check(sub_man, &changed); if (!changed) { - mavlink_log_critical(mavlink_fd, "no manual control, aborting"); + mavlink_log_critical(mavlink_fd, "no inputs, aborting"); return ERROR; } @@ -82,12 +81,12 @@ int do_rc_calibration(int mavlink_fd) int save_ret = param_save_default(); if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL"); close(sub_man); return ERROR; } - mavlink_log_info(mavlink_fd, "trim calibration done"); + mavlink_log_info(mavlink_fd, "trim cal done"); close(sub_man); return OK; } diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h index 9aa6faafa..45efedf55 100644 --- a/src/modules/commander/rc_calibration.h +++ b/src/modules/commander/rc_calibration.h @@ -41,6 +41,6 @@ #include <stdint.h> -int do_rc_calibration(int mavlink_fd); +int do_trim_calibration(int mavlink_fd); #endif /* RC_CALIBRATION_H_ */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c7256583a..31955d3e5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -42,6 +42,8 @@ #include <unistd.h> #include <stdint.h> #include <stdbool.h> +#include <dirent.h> +#include <fcntl.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -50,6 +52,7 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_device.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" @@ -332,6 +335,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; + + // Disable publication of all attached sensors + + /* list directory */ + DIR *d; + struct dirent *direntry; + d = opendir("/dev"); + if (d) { + + while ((direntry = readdir(d)) != NULL) { + + int sensfd = ::open(direntry->d_name, 0); + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + close(sensfd); + + printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); + return 1; + } } break; @@ -382,16 +412,22 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->set_nav_state = NAV_STATE_RTL; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } break; case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + status->set_nav_state = NAV_STATE_LAND; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } |