aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/commander/commander.cpp18
-rw-r--r--src/modules/commander/commander_tests/state_machine_helper_test.cpp10
-rw-r--r--src/modules/commander/state_machine_helper.cpp13
-rw-r--r--src/modules/commander/state_machine_helper.h2
-rw-r--r--src/modules/uavcan/uavcan_main.cpp41
5 files changed, 41 insertions, 43 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2b528eef6..4f976546e 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -393,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
- arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
+ arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -1085,7 +1085,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
- if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
+ if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
@@ -1288,14 +1288,14 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
- arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
- arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1309,7 +1309,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
- arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1368,7 +1368,7 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
- arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1394,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
- arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+ arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -2156,7 +2156,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
- if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
+ if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@@ -2219,7 +2219,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
- arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+ arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 2e18c4284..08dda2fab 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
- transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
+ transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
MTT_ALL_NOT_VALID,
MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
- { "transition: MANUAL to AUTO_MISSION - global position valid",
- MTT_GLOBAL_POS_VALID,
+ { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
+ MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
- { "transition: AUTO_MISSION to MANUAL - global position valid",
- MTT_GLOBAL_POS_VALID,
+ { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
+ MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_LOITER - global position valid",
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 3c3d2f233..f8589d24b 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
};
transition_result_t
-arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
- const struct safety_s *safety, /// current safety settings
- arming_state_t new_arming_state, /// arming state requested
- struct actuator_armed_s *armed, /// current armed status
- const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
+arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
+ const struct safety_s *safety, ///< current safety settings
+ arming_state_t new_arming_state, ///< arming state requested
+ struct actuator_armed_s *armed, ///< current armed status
+ bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
+ const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
@@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
int prearm_ret = OK;
/* only perform the check if we have to */
- if (new_arming_state == ARMING_STATE_ARMED) {
+ if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index bb1b87e71..69ce8bbce 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -57,7 +57,7 @@ typedef enum {
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
- arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
+ arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 9d5404baf..4535b6d6a 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -253,7 +253,6 @@ int UavcanNode::run()
// XXX figure out the output count
_output_count = 2;
-
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
actuator_outputs_s outputs;
@@ -273,21 +272,23 @@ int UavcanNode::run()
_node.setStatusOk();
- while (!_task_should_exit) {
+ /*
+ * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
+ * Please note that with such multiplexing it is no longer possible to rely only on
+ * the value returned from poll() to detect whether actuator control has timed out or not.
+ * Instead, all ORB events need to be checked individually (see below).
+ */
+ _poll_fds_num = 0;
+ _poll_fds[_poll_fds_num] = ::pollfd();
+ _poll_fds[_poll_fds_num].fd = busevent_fd;
+ _poll_fds[_poll_fds_num].events = POLLIN;
+ _poll_fds_num += 1;
+ while (!_task_should_exit) {
+ // update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
- /*
- * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
- * Please note that with such multiplexing it is no longer possible to rely only on
- * the value returned from poll() to detect whether actuator control has timed out or not.
- * Instead, all ORB events need to be checked individually (see below).
- */
- _poll_fds[_poll_fds_num] = ::pollfd();
- _poll_fds[_poll_fds_num].fd = busevent_fd;
- _poll_fds[_poll_fds_num].events = POLLIN;
- _poll_fds_num += 1;
}
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
@@ -301,7 +302,7 @@ int UavcanNode::run()
} else {
// get controls for required topics
bool controls_updated = false;
- unsigned poll_id = 0;
+ unsigned poll_id = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
@@ -312,12 +313,7 @@ int UavcanNode::run()
}
}
- if (!controls_updated) {
- // timeout: no control data, switch to failsafe values
- // XXX trigger failsafe
- }
-
- //can we mix?
+ // can we mix?
if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
@@ -417,7 +413,8 @@ UavcanNode::subscribe()
// Subscribe/unsubscribe to required actuator control groups
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
- _poll_fds_num = 0;
+ // the first fd used by CAN
+ _poll_fds_num = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
@@ -523,8 +520,8 @@ UavcanNode::print_info()
warnx("not running, start first");
}
- warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
- warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
+ warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+ warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
}
/*
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-15 08:30:09 +0000