mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-29 22:30:35 +08:00
Merge branch 'beta' into offboard2
This commit is contained in:
Anton Babushkin
+352
-173
@@ -200,9 +200,11 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
|
||||
|
||||
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
|
||||
|
||||
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
|
||||
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
|
||||
|
||||
transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
|
||||
transition_result_t set_main_state_rc(struct vehicle_status_s *status);
|
||||
|
||||
void set_control_mode();
|
||||
|
||||
void print_reject_mode(const char *msg);
|
||||
|
||||
@@ -414,51 +416,45 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||
/* set main state */
|
||||
transition_result_t main_res = TRANSITION_DENIED;
|
||||
|
||||
if (status->rc_signal_lost) {
|
||||
/* allow mode switching by command only if no RC signal */
|
||||
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
|
||||
/* use autopilot-specific mode */
|
||||
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
|
||||
/* MANUAL */
|
||||
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
|
||||
/* use autopilot-specific mode */
|
||||
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
|
||||
/* MANUAL */
|
||||
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
|
||||
/* SEATBELT */
|
||||
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
|
||||
/* SEATBELT */
|
||||
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
|
||||
/* EASY */
|
||||
main_res = main_state_transition(status, MAIN_STATE_EASY);
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
|
||||
/* EASY */
|
||||
main_res = main_state_transition(status, MAIN_STATE_EASY);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
|
||||
/* AUTO */
|
||||
main_res = main_state_transition(status, MAIN_STATE_AUTO);
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
|
||||
/* AUTO */
|
||||
main_res = main_state_transition(status, MAIN_STATE_AUTO);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
|
||||
/* OFFBOARD */
|
||||
main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* use base mode */
|
||||
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
|
||||
/* AUTO */
|
||||
main_res = main_state_transition(status, MAIN_STATE_AUTO);
|
||||
|
||||
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
|
||||
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
|
||||
/* EASY */
|
||||
main_res = main_state_transition(status, MAIN_STATE_EASY);
|
||||
|
||||
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
|
||||
/* MANUAL */
|
||||
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
}
|
||||
}
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
|
||||
/* OFFBOARD */
|
||||
main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
|
||||
}
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd");
|
||||
/* use base mode */
|
||||
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
|
||||
/* AUTO */
|
||||
main_res = main_state_transition(status, MAIN_STATE_AUTO);
|
||||
|
||||
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
|
||||
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
|
||||
/* EASY */
|
||||
main_res = main_state_transition(status, MAIN_STATE_EASY);
|
||||
|
||||
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
|
||||
/* MANUAL */
|
||||
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (main_res == TRANSITION_CHANGED)
|
||||
@@ -527,7 +523,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
|
||||
|
||||
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
|
||||
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
|
||||
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
|
||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
ret = true;
|
||||
|
||||
@@ -566,17 +562,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||
}
|
||||
|
||||
static struct vehicle_status_s status;
|
||||
|
||||
/* armed topic */
|
||||
static struct vehicle_control_mode_s control_mode;
|
||||
static struct actuator_armed_s armed;
|
||||
|
||||
static struct safety_s safety;
|
||||
|
||||
int commander_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* not yet initialized */
|
||||
commander_initialized = false;
|
||||
bool home_position_set = false;
|
||||
|
||||
bool battery_tune_played = false;
|
||||
bool arm_tune_played = false;
|
||||
@@ -590,6 +583,28 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* welcome user */
|
||||
warnx("starting");
|
||||
|
||||
char *main_states_str[MAIN_STATE_MAX];
|
||||
main_states_str[0] = "MANUAL";
|
||||
main_states_str[1] = "SEATBELT";
|
||||
main_states_str[2] = "EASY";
|
||||
main_states_str[3] = "AUTO";
|
||||
main_states_str[4] = "OFFBOARD";
|
||||
|
||||
char *arming_states_str[ARMING_STATE_MAX];
|
||||
arming_states_str[0] = "INIT";
|
||||
arming_states_str[1] = "STANDBY";
|
||||
arming_states_str[2] = "ARMED";
|
||||
arming_states_str[3] = "ARMED_ERROR";
|
||||
arming_states_str[4] = "STANDBY_ERROR";
|
||||
arming_states_str[5] = "REBOOT";
|
||||
arming_states_str[6] = "IN_AIR_RESTORE";
|
||||
|
||||
char *failsafe_states_str[FAILSAFE_STATE_MAX];
|
||||
failsafe_states_str[0] = "NORMAL";
|
||||
failsafe_states_str[1] = "RTL";
|
||||
failsafe_states_str[2] = "LAND";
|
||||
failsafe_states_str[3] = "TERMINATION";
|
||||
|
||||
/* pthread for slow low prio thread */
|
||||
pthread_t commander_low_prio_thread;
|
||||
|
||||
@@ -604,21 +619,15 @@ 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));
|
||||
|
||||
status.main_state = MAIN_STATE_MANUAL;
|
||||
status.set_nav_state = NAV_STATE_NONE;
|
||||
status.set_nav_state_timestamp = 0;
|
||||
status.arming_state = ARMING_STATE_INIT;
|
||||
status.hil_state = HIL_STATE_OFF;
|
||||
status.failsafe_state = FAILSAFE_STATE_NORMAL;
|
||||
|
||||
/* neither manual nor offboard control commands have been received */
|
||||
status.offboard_control_signal_found_once = false;
|
||||
@@ -635,14 +644,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);
|
||||
|
||||
@@ -703,10 +718,15 @@ int commander_thread_main(int argc, char *argv[])
|
||||
struct manual_control_setpoint_s sp_man;
|
||||
memset(&sp_man, 0, sizeof(sp_man));
|
||||
|
||||
/* Subscribe to offboard control data */
|
||||
int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
struct offboard_control_setpoint_s offboard_sp;
|
||||
memset(&offboard_sp, 0, sizeof(offboard_sp));
|
||||
|
||||
/* Subscribe to offboard control data */
|
||||
int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
struct offboard_control_setpoint_s offboard_sp;
|
||||
memset(&offboard_sp, 0, sizeof(offboard_sp));
|
||||
int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
struct offboard_control_setpoint_s sp_offboard;
|
||||
memset(&sp_offboard, 0, sizeof(sp_offboard));
|
||||
|
||||
/* Subscribe to global position */
|
||||
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
@@ -820,12 +840,16 @@ int commander_thread_main(int argc, char *argv[])
|
||||
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
|
||||
}
|
||||
|
||||
orb_check(offboard_sp_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp);
|
||||
}
|
||||
|
||||
orb_check(sp_offboard_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
|
||||
}
|
||||
|
||||
orb_check(sensor_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
@@ -862,7 +886,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);
|
||||
@@ -1018,19 +1042,18 @@ int commander_thread_main(int argc, char *argv[])
|
||||
* position to the current position.
|
||||
*/
|
||||
|
||||
if (!home_position_set && gps_position.fix_type >= 3 &&
|
||||
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
|
||||
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;
|
||||
home.lon = global_position.lon;
|
||||
home.alt = global_position.alt;
|
||||
|
||||
|
||||
home.lat = (double)global_position.lat / 1e7d;
|
||||
home.lon = (double)global_position.lon / 1e7d;
|
||||
home.altitude = (float)global_position.alt;
|
||||
|
||||
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
|
||||
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
|
||||
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
|
||||
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
|
||||
|
||||
/* announce new home position */
|
||||
if (home_pub > 0) {
|
||||
@@ -1041,13 +1064,13 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
/* mark home position as set */
|
||||
home_position_set = true;
|
||||
status.condition_home_position_valid = true;
|
||||
tune_positive();
|
||||
}
|
||||
}
|
||||
|
||||
/* start RC input check */
|
||||
if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
|
||||
if (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;
|
||||
@@ -1122,24 +1145,27 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
} else if (res == TRANSITION_DENIED) {
|
||||
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
/* DENIED here indicates bug in the commander */
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
|
||||
}
|
||||
|
||||
/* fill current_status according to mode switches */
|
||||
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
|
||||
/* recover from failsafe */
|
||||
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
|
||||
}
|
||||
|
||||
/* fill status according to mode switches */
|
||||
check_mode_switches(&sp_man, &status);
|
||||
|
||||
/* evaluate the main state machine */
|
||||
res = check_main_state_machine(&status);
|
||||
/* evaluate the main state machine according to mode switches */
|
||||
res = set_main_state_rc(&status);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
|
||||
tune_positive();
|
||||
|
||||
} else if (res == TRANSITION_DENIED) {
|
||||
/* DENIED here indicates bug in the commander */
|
||||
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
|
||||
}
|
||||
|
||||
} else {
|
||||
@@ -1149,26 +1175,66 @@ int commander_thread_main(int argc, char *argv[])
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
if (status.main_state != MAIN_STATE_AUTO && armed.armed && status.main_state != MAIN_STATE_OFFBOARD) {
|
||||
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
|
||||
/* switch to OFFBOARD mode if offboard signal available */
|
||||
transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
|
||||
status.set_nav_state = NAV_STATE_RTL;
|
||||
status.set_nav_state_timestamp = hrt_absolute_time();
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* can't switch to OFFBOARD, do normal failsafe if needed */
|
||||
if (armed.armed) {
|
||||
if (status.main_state == MAIN_STATE_AUTO) {
|
||||
/* check if AUTO mode still allowed */
|
||||
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
|
||||
|
||||
} else if (status.main_state != MAIN_STATE_SEATBELT) {
|
||||
res = main_state_transition(&status, MAIN_STATE_SEATBELT);
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
|
||||
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* LAND not allowed, set TERMINATION state */
|
||||
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
|
||||
}
|
||||
}
|
||||
|
||||
} else if (status.main_state == MAIN_STATE_OFFBOARD) {
|
||||
/* check if OFFBOARD mode still allowed */
|
||||
transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
|
||||
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* LAND not allowed, set TERMINATION state */
|
||||
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/* failsafe for manual modes */
|
||||
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* RTL not allowed (no global position estimate), try LAND */
|
||||
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* LAND not allowed, set TERMINATION state */
|
||||
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
|
||||
/* reset failsafe when disarmed */
|
||||
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* check offboard signal */
|
||||
if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) {
|
||||
if (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) {
|
||||
if (!status.offboard_control_signal_found_once) {
|
||||
status.offboard_control_signal_found_once = true;
|
||||
mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time");
|
||||
@@ -1189,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1211,19 +1277,14 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
/* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
|
||||
if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
|
||||
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
|
||||
|
||||
if (flighttermination_res == TRANSITION_CHANGED) {
|
||||
// TODO remove this hack
|
||||
/* flight termination in manual mode if assisted switch is on easy position */
|
||||
if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
|
||||
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
|
||||
tune_positive();
|
||||
}
|
||||
|
||||
} else {
|
||||
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
|
||||
}
|
||||
|
||||
|
||||
/* handle commands last, as the system needs to be updated to handle them */
|
||||
orb_check(cmd_sub, &updated);
|
||||
|
||||
@@ -1239,19 +1300,35 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* check which state machines for changes, clear "changed" flag */
|
||||
bool arming_state_changed = check_arming_state_changed();
|
||||
bool main_state_changed = check_main_state_changed();
|
||||
bool flighttermination_state_changed = check_flighttermination_state_changed();
|
||||
bool failsafe_state_changed = check_failsafe_state_changed();
|
||||
|
||||
hrt_abstime t1 = hrt_absolute_time();
|
||||
|
||||
if (arming_state_changed || main_state_changed) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
|
||||
/* print new state */
|
||||
if (arming_state_changed) {
|
||||
status_changed = true;
|
||||
mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
|
||||
}
|
||||
|
||||
if (main_state_changed) {
|
||||
status_changed = true;
|
||||
mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
|
||||
}
|
||||
|
||||
if (failsafe_state_changed) {
|
||||
status_changed = true;
|
||||
mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
|
||||
}
|
||||
|
||||
/* 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);
|
||||
}
|
||||
@@ -1317,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
led_deinit();
|
||||
buzzer_deinit();
|
||||
close(sp_man_sub);
|
||||
close(offboard_sp_sub);
|
||||
close(sp_offboard_sub);
|
||||
close(local_position_sub);
|
||||
close(global_position_sub);
|
||||
close(gps_sub);
|
||||
@@ -1424,133 +1501,235 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
|
||||
}
|
||||
|
||||
void
|
||||
check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
|
||||
check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
|
||||
{
|
||||
/* main mode switch */
|
||||
if (!isfinite(sp_man->mode_switch)) {
|
||||
warnx("mode sw not finite");
|
||||
current_status->mode_switch = MODE_SWITCH_MANUAL;
|
||||
status->mode_switch = MODE_SWITCH_MANUAL;
|
||||
|
||||
} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
|
||||
current_status->mode_switch = MODE_SWITCH_AUTO;
|
||||
status->mode_switch = MODE_SWITCH_AUTO;
|
||||
|
||||
} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
|
||||
current_status->mode_switch = MODE_SWITCH_MANUAL;
|
||||
status->mode_switch = MODE_SWITCH_MANUAL;
|
||||
|
||||
} else {
|
||||
current_status->mode_switch = MODE_SWITCH_ASSISTED;
|
||||
status->mode_switch = MODE_SWITCH_ASSISTED;
|
||||
}
|
||||
|
||||
/* return switch */
|
||||
if (!isfinite(sp_man->return_switch)) {
|
||||
current_status->return_switch = RETURN_SWITCH_NONE;
|
||||
status->return_switch = RETURN_SWITCH_NONE;
|
||||
|
||||
} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
|
||||
current_status->return_switch = RETURN_SWITCH_RETURN;
|
||||
status->return_switch = RETURN_SWITCH_RETURN;
|
||||
|
||||
} else {
|
||||
current_status->return_switch = RETURN_SWITCH_NORMAL;
|
||||
status->return_switch = RETURN_SWITCH_NORMAL;
|
||||
}
|
||||
|
||||
/* assisted switch */
|
||||
if (!isfinite(sp_man->assisted_switch)) {
|
||||
current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
|
||||
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
|
||||
|
||||
} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
|
||||
current_status->assisted_switch = ASSISTED_SWITCH_EASY;
|
||||
status->assisted_switch = ASSISTED_SWITCH_EASY;
|
||||
|
||||
} else {
|
||||
current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
|
||||
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
|
||||
}
|
||||
|
||||
/* mission switch */
|
||||
if (!isfinite(sp_man->mission_switch)) {
|
||||
current_status->mission_switch = MISSION_SWITCH_NONE;
|
||||
status->mission_switch = MISSION_SWITCH_NONE;
|
||||
|
||||
} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
|
||||
current_status->mission_switch = MISSION_SWITCH_LOITER;
|
||||
status->mission_switch = MISSION_SWITCH_LOITER;
|
||||
|
||||
} else {
|
||||
current_status->mission_switch = MISSION_SWITCH_MISSION;
|
||||
status->mission_switch = MISSION_SWITCH_MISSION;
|
||||
}
|
||||
|
||||
/* offboard switch */
|
||||
/* offboard switch */
|
||||
if (!isfinite(sp_man->offboard_switch)) {
|
||||
current_status->offboard_switch = OFFBOARD_SWITCH_NONE;
|
||||
status->offboard_switch = OFFBOARD_SWITCH_NONE;
|
||||
|
||||
} else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) {
|
||||
current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD;
|
||||
status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD;
|
||||
|
||||
} else {
|
||||
current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD;
|
||||
status->offboard_switch = OFFBOARD_SWITCH_ONBOARD;
|
||||
}
|
||||
}
|
||||
|
||||
transition_result_t
|
||||
check_main_state_machine(struct vehicle_status_s *current_status)
|
||||
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;
|
||||
|
||||
if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) {
|
||||
/* offboard switch overrides main switch */
|
||||
res = main_state_transition(current_status, MAIN_STATE_OFFBOARD);
|
||||
/* offboard switch overrides main switch */
|
||||
if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) {
|
||||
res = main_state_transition(status, MAIN_STATE_OFFBOARD);
|
||||
if (res == TRANSITION_DENIED) {
|
||||
print_reject_mode("OFFBOARD");
|
||||
|
||||
} else {
|
||||
switch (current_status->mode_switch) {
|
||||
case MODE_SWITCH_MANUAL:
|
||||
res = main_state_transition(current_status, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
} else {
|
||||
return res;
|
||||
}
|
||||
}
|
||||
|
||||
case MODE_SWITCH_ASSISTED:
|
||||
if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
|
||||
res = main_state_transition(current_status, MAIN_STATE_EASY);
|
||||
/* offboard switched off or denied, check mode switch */
|
||||
switch (status->mode_switch) {
|
||||
case MODE_SWITCH_MANUAL:
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
break; // changed successfully or already in this state
|
||||
|
||||
// else fallback to SEATBELT
|
||||
print_reject_mode("EASY");
|
||||
}
|
||||
|
||||
res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
break; // changed successfully or already in this mode
|
||||
|
||||
if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
|
||||
print_reject_mode("SEATBELT");
|
||||
|
||||
// else fallback to MANUAL
|
||||
res = main_state_transition(current_status, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
case MODE_SWITCH_AUTO:
|
||||
res = main_state_transition(current_status, MAIN_STATE_AUTO);
|
||||
case MODE_SWITCH_ASSISTED:
|
||||
if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
|
||||
res = main_state_transition(status, MAIN_STATE_EASY);
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
break; // changed successfully or already in this state
|
||||
|
||||
// else fallback to SEATBELT (EASY likely will not work too)
|
||||
print_reject_mode("AUTO");
|
||||
res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
|
||||
// else fallback to SEATBELT
|
||||
print_reject_mode("EASY");
|
||||
}
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
break; // changed successfully or already in this state
|
||||
res = main_state_transition(status, MAIN_STATE_SEATBELT);
|
||||
|
||||
// else fallback to MANUAL
|
||||
res = main_state_transition(current_status, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
if (res != TRANSITION_DENIED)
|
||||
break; // changed successfully or already in this mode
|
||||
|
||||
if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
|
||||
print_reject_mode("SEATBELT");
|
||||
|
||||
// else fallback to MANUAL
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
case MODE_SWITCH_AUTO:
|
||||
res = main_state_transition(status, MAIN_STATE_AUTO);
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
break; // changed successfully or already in this state
|
||||
|
||||
// else fallback to SEATBELT (EASY likely will not work too)
|
||||
print_reject_mode("AUTO");
|
||||
res = main_state_transition(status, MAIN_STATE_SEATBELT);
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
break; // changed successfully or already in this state
|
||||
|
||||
// else fallback to MANUAL
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void
|
||||
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;
|
||||
}
|
||||
|
||||
return res;
|
||||
/* 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
|
||||
|
||||
Reference in New Issue
Block a user