forked from Archive/PX4-Autopilot
Controllers should not access state machine anymore but access the vehicle_control_mode flags
This commit is contained in:
parent
e556649f2f
commit
9f5565de32
|
@ -1233,6 +1233,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
// XXX for now just set sensors as initialized
|
||||
current_status.condition_system_sensors_initialized = true;
|
||||
|
||||
// XXX just disable offboard control for now
|
||||
control_mode.flag_control_offboard_enabled = false;
|
||||
|
||||
/* advertise to ORB */
|
||||
status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status);
|
||||
/* publish current state machine */
|
||||
|
@ -1240,6 +1243,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety);
|
||||
|
||||
control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
|
||||
|
||||
/* home position */
|
||||
orb_advert_t home_pub = -1;
|
||||
struct home_position_s home;
|
||||
|
@ -1824,7 +1829,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* just manual, XXX this might depend on the return switch */
|
||||
if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
|
||||
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
|
||||
}
|
||||
|
@ -1832,9 +1837,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* Try seatbelt or fallback to manual */
|
||||
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
|
||||
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// fallback to MANUAL_STANDBY
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
|
||||
}
|
||||
|
@ -1843,11 +1848,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* Try auto or fallback to seatbelt or even manual */
|
||||
} else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
|
||||
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// first fallback to SEATBELT_STANDY
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// or fallback to MANUAL_STANDBY
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
|
||||
}
|
||||
|
@ -1863,7 +1868,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* Always accept manual mode */
|
||||
if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
|
||||
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL rejected");
|
||||
}
|
||||
|
@ -1872,9 +1877,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
|
||||
&& current_status.return_switch == RETURN_SWITCH_NONE) {
|
||||
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// fallback to MANUAL_STANDBY
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL rejected");
|
||||
}
|
||||
|
@ -1884,9 +1889,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
|
||||
&& current_status.return_switch == RETURN_SWITCH_RETURN) {
|
||||
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// fallback to MANUAL_STANDBY
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL rejected");
|
||||
}
|
||||
|
@ -1898,19 +1903,19 @@ int commander_thread_main(int argc, char *argv[])
|
|||
&& current_status.mission_switch == MISSION_SWITCH_NONE) {
|
||||
|
||||
/* we might come from the disarmed state AUTO_STANDBY */
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// fallback to MANUAL_STANDBY
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL rejected");
|
||||
}
|
||||
}
|
||||
/* or from some other armed state like SEATBELT or MANUAL */
|
||||
} else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
|
||||
} else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// fallback to MANUAL_STANDBY
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL rejected");
|
||||
}
|
||||
|
@ -1922,10 +1927,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
&& current_status.return_switch == RETURN_SWITCH_NONE
|
||||
&& current_status.mission_switch == MISSION_SWITCH_MISSION) {
|
||||
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// fallback to MANUAL_STANDBY
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL rejected");
|
||||
}
|
||||
|
@ -1937,10 +1942,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
&& current_status.return_switch == RETURN_SWITCH_RETURN
|
||||
&& (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) {
|
||||
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// fallback to MANUAL_STANDBY
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
|
||||
if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
|
||||
// These is not supposed to happen
|
||||
warnx("ERROR: Navigation state MANUAL rejected");
|
||||
}
|
||||
|
@ -2070,43 +2075,43 @@ int commander_thread_main(int argc, char *argv[])
|
|||
if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
|
||||
if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
|
||||
|
||||
/* decide about attitude control flag, enable in att/pos/vel */
|
||||
bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
|
||||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
|
||||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
|
||||
// /* decide about attitude control flag, enable in att/pos/vel */
|
||||
// bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
|
||||
// sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
|
||||
// sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
|
||||
|
||||
/* decide about rate control flag, enable it always XXX (for now) */
|
||||
bool rates_ctrl_enabled = true;
|
||||
// /* decide about rate control flag, enable it always XXX (for now) */
|
||||
// bool rates_ctrl_enabled = true;
|
||||
|
||||
/* set up control mode */
|
||||
if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
|
||||
current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
|
||||
state_changed = true;
|
||||
}
|
||||
// /* set up control mode */
|
||||
// if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
|
||||
// current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
|
||||
// state_changed = true;
|
||||
// }
|
||||
|
||||
if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
|
||||
current_status.flag_control_rates_enabled = rates_ctrl_enabled;
|
||||
state_changed = true;
|
||||
}
|
||||
// if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
|
||||
// current_status.flag_control_rates_enabled = rates_ctrl_enabled;
|
||||
// state_changed = true;
|
||||
// }
|
||||
|
||||
/* handle the case where offboard control signal was regained */
|
||||
if (!current_status.offboard_control_signal_found_once) {
|
||||
current_status.offboard_control_signal_found_once = true;
|
||||
/* enable offboard control, disable manual input */
|
||||
current_status.flag_control_manual_enabled = false;
|
||||
current_status.flag_control_offboard_enabled = true;
|
||||
state_changed = true;
|
||||
tune_positive();
|
||||
// /* handle the case where offboard control signal was regained */
|
||||
// if (!current_status.offboard_control_signal_found_once) {
|
||||
// current_status.offboard_control_signal_found_once = true;
|
||||
// /* enable offboard control, disable manual input */
|
||||
// current_status.flag_control_manual_enabled = false;
|
||||
// current_status.flag_control_offboard_enabled = true;
|
||||
// state_changed = true;
|
||||
// tune_positive();
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
|
||||
// mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
|
||||
|
||||
} else {
|
||||
if (current_status.offboard_control_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
|
||||
state_changed = true;
|
||||
tune_positive();
|
||||
}
|
||||
}
|
||||
// } else {
|
||||
// if (current_status.offboard_control_signal_lost) {
|
||||
// mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
|
||||
// state_changed = true;
|
||||
// tune_positive();
|
||||
// }
|
||||
// }
|
||||
|
||||
current_status.offboard_control_signal_weak = false;
|
||||
current_status.offboard_control_signal_lost = false;
|
||||
|
|
|
@ -46,6 +46,7 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
|
@ -161,7 +162,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|
|||
* This functions does not evaluate any input flags but only checks if the transitions
|
||||
* are valid.
|
||||
*/
|
||||
int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) {
|
||||
int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) {
|
||||
|
||||
int ret = ERROR;
|
||||
|
||||
|
@ -179,11 +180,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) {
|
||||
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = false;
|
||||
current_state->flag_control_attitude_enabled = false;
|
||||
current_state->flag_control_velocity_enabled = false;
|
||||
current_state->flag_control_position_enabled = false;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = false;
|
||||
control_mode->flag_control_attitude_enabled = false;
|
||||
control_mode->flag_control_velocity_enabled = false;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
break;
|
||||
|
||||
|
@ -201,11 +202,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = false;
|
||||
current_state->flag_control_position_enabled = false;
|
||||
current_state->flag_control_manual_enabled = true;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = false;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -218,11 +219,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = false;
|
||||
current_state->flag_control_position_enabled = false;
|
||||
current_state->flag_control_manual_enabled = true;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = false;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = true;
|
||||
}
|
||||
break;
|
||||
|
||||
|
@ -244,11 +245,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = false;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -275,11 +276,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = false;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -305,11 +306,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = false;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -334,11 +335,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = true;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -357,11 +358,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = true;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -372,11 +373,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) {
|
||||
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = true;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
break;
|
||||
|
||||
|
@ -398,11 +399,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = true;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -422,11 +423,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = true;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -449,11 +450,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = true;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -473,11 +474,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
tune_negative();
|
||||
} else {
|
||||
ret = OK;
|
||||
current_state->flag_control_rates_enabled = true;
|
||||
current_state->flag_control_attitude_enabled = true;
|
||||
current_state->flag_control_velocity_enabled = true;
|
||||
current_state->flag_control_position_enabled = true;
|
||||
current_state->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -491,6 +492,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
|
|||
current_state->counter++;
|
||||
current_state->timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_status), status_pub, current_state);
|
||||
|
||||
control_mode->timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -47,6 +47,7 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_safety.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
||||
void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
|
@ -54,8 +55,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st
|
|||
|
||||
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd);
|
||||
int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd);
|
||||
int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd);
|
||||
|
||||
int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state);
|
||||
|
||||
|
|
|
@ -54,6 +54,7 @@
|
|||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls_effective.h>
|
||||
|
|
|
@ -274,18 +274,18 @@ void mavlink_update_system(void)
|
|||
}
|
||||
|
||||
void
|
||||
get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety,
|
||||
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety,
|
||||
uint8_t *mavlink_state, uint8_t *mavlink_mode)
|
||||
{
|
||||
/* reset MAVLink mode bitfield */
|
||||
*mavlink_mode = 0;
|
||||
|
||||
/* set mode flags independent of system state */
|
||||
if (v_status->flag_control_manual_enabled) {
|
||||
if (control_mode->flag_control_manual_enabled) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
||||
}
|
||||
|
||||
if (v_status->flag_hil_enabled) {
|
||||
if (safety->hil_enabled) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
||||
}
|
||||
|
||||
|
@ -296,7 +296,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
|
|||
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
}
|
||||
|
||||
if (v_status->flag_control_velocity_enabled) {
|
||||
if (control_mode->flag_control_velocity_enabled) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
|
||||
} else {
|
||||
*mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
|
||||
|
@ -368,7 +368,9 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
char *device_name = "/dev/ttyS1";
|
||||
baudrate = 57600;
|
||||
|
||||
/* XXX this is never written? */
|
||||
struct vehicle_status_s v_status;
|
||||
struct vehicle_control_mode_s control_mode;
|
||||
struct actuator_safety_s safety;
|
||||
|
||||
/* work around some stupidity in task_create's argv handling */
|
||||
|
@ -437,7 +439,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
/* translate the current system state to mavlink state and mode */
|
||||
uint8_t mavlink_state = 0;
|
||||
uint8_t mavlink_mode = 0;
|
||||
get_mavlink_mode_and_state(&v_status, &safety, &mavlink_state, &mavlink_mode);
|
||||
get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode);
|
||||
|
||||
/* send heartbeat */
|
||||
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
|
||||
|
|
|
@ -52,6 +52,7 @@
|
|||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
|
|
@ -50,5 +50,6 @@ extern volatile bool thread_should_exit;
|
|||
/**
|
||||
* Translate the custom state into standard mavlink modes and state.
|
||||
*/
|
||||
extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety,
|
||||
extern void
|
||||
get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety,
|
||||
uint8_t *mavlink_state, uint8_t *mavlink_mode);
|
||||
|
|
|
@ -57,7 +57,7 @@
|
|||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/actuator_safety.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
|
@ -91,8 +91,8 @@ static int
|
|||
mc_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* declare and safely initialize all structs */
|
||||
struct vehicle_status_s state;
|
||||
memset(&state, 0, sizeof(state));
|
||||
struct vehicle_control_mode_s control_mode;
|
||||
memset(&control_mode, 0, sizeof(control_mode));
|
||||
struct actuator_safety_s safety;
|
||||
memset(&safety, 0, sizeof(safety));
|
||||
struct vehicle_attitude_s att;
|
||||
|
@ -116,7 +116,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
int param_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
|
||||
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
|
@ -181,7 +181,6 @@ mc_thread_main(int argc, char *argv[])
|
|||
} else if (ret == 0) {
|
||||
/* no return value, ignore */
|
||||
} else {
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[1].revents & POLLIN) {
|
||||
/* read from param to clear updated flag */
|
||||
|
@ -199,10 +198,10 @@ mc_thread_main(int argc, char *argv[])
|
|||
|
||||
/* get a local copy of system state */
|
||||
bool updated;
|
||||
orb_check(state_sub, &updated);
|
||||
orb_check(control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
|
||||
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
|
||||
}
|
||||
|
||||
orb_check(safety_sub, &updated);
|
||||
|
@ -227,9 +226,8 @@ mc_thread_main(int argc, char *argv[])
|
|||
/* get a local copy of the current sensor values */
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
|
||||
|
||||
|
||||
/** STEP 1: Define which input is the dominating control input */
|
||||
if (state.flag_control_offboard_enabled) {
|
||||
if (control_mode.flag_control_offboard_enabled) {
|
||||
/* offboard inputs */
|
||||
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
|
||||
rates_sp.roll = offboard_sp.p1;
|
||||
|
@ -252,13 +250,11 @@ mc_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
|
||||
} else if (state.flag_control_manual_enabled) {
|
||||
|
||||
if (state.flag_control_attitude_enabled) {
|
||||
|
||||
} else if (control_mode.flag_control_manual_enabled) {
|
||||
if (control_mode.flag_control_attitude_enabled) {
|
||||
/* initialize to current yaw if switching to manual or att control */
|
||||
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
|
||||
state.flag_control_manual_enabled != flag_control_manual_enabled ||
|
||||
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
|
||||
control_mode.flag_control_manual_enabled != flag_control_manual_enabled ||
|
||||
safety.armed != flag_armed) {
|
||||
att_sp.yaw_body = att.yaw;
|
||||
}
|
||||
|
@ -266,6 +262,8 @@ mc_thread_main(int argc, char *argv[])
|
|||
static bool rc_loss_first_time = true;
|
||||
|
||||
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
|
||||
/* XXX fix this */
|
||||
#if 0
|
||||
if (state.rc_signal_lost) {
|
||||
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
|
||||
param_get(failsafe_throttle_handle, &failsafe_throttle);
|
||||
|
@ -297,6 +295,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
rc_loss_first_time = false;
|
||||
|
||||
} else {
|
||||
#endif
|
||||
rc_loss_first_time = true;
|
||||
|
||||
att_sp.roll_body = manual.roll;
|
||||
|
@ -344,7 +343,9 @@ mc_thread_main(int argc, char *argv[])
|
|||
|
||||
att_sp.thrust = manual.throttle;
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
#if 0
|
||||
}
|
||||
#endif
|
||||
|
||||
/* STEP 2: publish the controller output */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
|
@ -378,7 +379,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
|
||||
if (state.flag_control_attitude_enabled) {
|
||||
if (control_mode.flag_control_attitude_enabled) {
|
||||
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
|
||||
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
|
||||
|
@ -405,11 +406,11 @@ mc_thread_main(int argc, char *argv[])
|
|||
multirotor_control_rates(&rates_sp, gyro, &actuators);
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
/* update state */
|
||||
flag_control_attitude_enabled = state.flag_control_attitude_enabled;
|
||||
flag_control_manual_enabled = state.flag_control_manual_enabled;
|
||||
flag_control_position_enabled = state.flag_control_position_enabled;
|
||||
flag_control_velocity_enabled = state.flag_control_velocity_enabled;
|
||||
/* update control_mode */
|
||||
flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
|
||||
flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
|
||||
flag_control_position_enabled = control_mode.flag_control_position_enabled;
|
||||
flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled;
|
||||
flag_armed = safety.armed;
|
||||
|
||||
perf_end(mc_loop_perf);
|
||||
|
@ -427,7 +428,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
|
||||
|
||||
close(att_sub);
|
||||
close(state_sub);
|
||||
close(control_mode_sub);
|
||||
close(manual_sub);
|
||||
close(actuator_pub);
|
||||
close(att_sp_pub);
|
||||
|
|
|
@ -98,6 +98,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s);
|
|||
#include "topics/vehicle_command.h"
|
||||
ORB_DEFINE(vehicle_command, struct vehicle_command_s);
|
||||
|
||||
#include "topics/vehicle_control_mode.h"
|
||||
ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s);
|
||||
|
||||
#include "topics/vehicle_local_position_setpoint.h"
|
||||
ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
|
||||
|
||||
|
|
|
@ -58,6 +58,7 @@ struct actuator_safety_s {
|
|||
bool armed; /**< Set to true if system is armed */
|
||||
bool ready_to_arm; /**< Set to true if system is ready to be armed */
|
||||
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
|
||||
bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */
|
||||
};
|
||||
|
||||
ORB_DECLARE(actuator_safety);
|
||||
|
|
|
@ -61,16 +61,11 @@
|
|||
*/
|
||||
struct vehicle_control_mode_s
|
||||
{
|
||||
/* use of a counter and timestamp recommended (but not necessary) */
|
||||
|
||||
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
|
||||
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
|
||||
|
||||
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
|
||||
|
||||
bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */
|
||||
bool flag_armed; /**< true is motors / actuators are armed */
|
||||
bool flag_safety_off; /**< true if safety is off */
|
||||
bool flag_system_emergency;
|
||||
bool flag_preflight_calibration;
|
||||
|
||||
|
@ -83,7 +78,7 @@ struct vehicle_control_mode_s
|
|||
bool flag_control_position_enabled; /**< true if position is controlled */
|
||||
|
||||
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
|
||||
bool failsave_highlevel;
|
||||
bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */
|
||||
};
|
||||
|
||||
/**
|
||||
|
@ -91,6 +86,6 @@ struct vehicle_control_mode_s
|
|||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(vehicle_status);
|
||||
ORB_DECLARE(vehicle_control_mode);
|
||||
|
||||
#endif
|
||||
|
|
|
@ -199,18 +199,18 @@ struct vehicle_status_s
|
|||
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
|
||||
|
||||
bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */
|
||||
bool flag_armed; /**< true is motors / actuators are armed */
|
||||
bool flag_safety_off; /**< true if safety is off */
|
||||
//bool flag_armed; /**< true is motors / actuators are armed */
|
||||
//bool flag_safety_off; /**< true if safety is off */
|
||||
bool flag_system_emergency;
|
||||
bool flag_preflight_calibration;
|
||||
|
||||
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
|
||||
bool flag_control_offboard_enabled; /**< true if offboard control input is on */
|
||||
bool flag_auto_enabled;
|
||||
bool flag_control_rates_enabled; /**< true if rates are stabilized */
|
||||
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
|
||||
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
|
||||
bool flag_control_position_enabled; /**< true if position is controlled */
|
||||
// bool flag_control_manual_enabled; /**< true if manual input is mixed in */
|
||||
// bool flag_control_offboard_enabled; /**< true if offboard control input is on */
|
||||
// bool flag_auto_enabled;
|
||||
// bool flag_control_rates_enabled; /**< true if rates are stabilized */
|
||||
// bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
|
||||
// bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
|
||||
// bool flag_control_position_enabled; /**< true if position is controlled */
|
||||
|
||||
// bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
|
||||
// bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
|
||||
|
|
Loading…
Reference in New Issue