Browse Source

commander: moved offboard bools into status_flags

The offboard status bools were not used anywhere but in the commander.
Therefore they are now moved to the local status_flags topic.
sbg
Julian Oes 9 years ago
parent
commit
23df992cc5
  1. 6
      msg/vehicle_status.msg
  2. 20
      src/modules/commander/commander.cpp
  3. 6
      src/modules/commander/state_machine_helper.cpp
  4. 4
      src/modules/commander/state_machine_helper.h

6
msg/vehicle_status.msg

@ -95,12 +95,6 @@ bool gps_failure_cmd # Set to true if a gps failure mode is commanded @@ -95,12 +95,6 @@ bool gps_failure_cmd # Set to true if a gps failure mode is commanded
bool mission_failure # Set to true if mission could not continue/finish
bool barometer_failure # Set to true if a barometer failure is detected
bool offboard_control_signal_found_once
bool offboard_control_signal_lost
bool offboard_control_signal_weak
uint64 offboard_control_signal_lost_interval # interval in microseconds without an offboard control message
bool offboard_control_set_by_command # true if the offboard mode was set by a mavlink command and should not be overridden by RC
# see SYS_STATUS mavlink message for the following
uint32 onboard_control_sensors_present
uint32 onboard_control_sensors_enabled

20
src/modules/commander/commander.cpp

@ -984,18 +984,18 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s @@ -984,18 +984,18 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
status_local->offboard_control_set_by_command = false;
status_flags.offboard_control_set_by_command = false;
} else {
/* Set flag that offboard was set via command, main state is not overridden by rc */
status_local->offboard_control_set_by_command = true;
status_flags.offboard_control_set_by_command = true;
}
} else {
/* If the mavlink command is used to enable or disable offboard control:
* switch back to previous mode when disabling */
res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev, &status_flags);
status_local->offboard_control_set_by_command = false;
status_flags.offboard_control_set_by_command = false;
}
}
break;
@ -1237,12 +1237,12 @@ int commander_thread_main(int argc, char *argv[]) @@ -1237,12 +1237,12 @@ int commander_thread_main(int argc, char *argv[])
status.failsafe = false;
/* neither manual nor offboard control commands have been received */
status.offboard_control_signal_found_once = false;
status_flags.offboard_control_signal_found_once = false;
status.rc_signal_found_once = false;
/* mark all signals lost as long as they haven't been found */
status.rc_signal_lost = true;
status.offboard_control_signal_lost = true;
status_flags.offboard_control_signal_lost = true;
status.data_link_lost = true;
status_flags.condition_system_prearm_error_reported = false;
@ -1625,14 +1625,14 @@ int commander_thread_main(int argc, char *argv[]) @@ -1625,14 +1625,14 @@ int commander_thread_main(int argc, char *argv[])
if (offboard_control_mode.timestamp != 0 &&
offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
if (status.offboard_control_signal_lost) {
status.offboard_control_signal_lost = false;
if (status_flags.offboard_control_signal_lost) {
status_flags.offboard_control_signal_lost = false;
status_changed = true;
}
} else {
if (!status.offboard_control_signal_lost) {
status.offboard_control_signal_lost = true;
if (!status_flags.offboard_control_signal_lost) {
status_flags.offboard_control_signal_lost = true;
status_changed = true;
}
}
@ -2883,7 +2883,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -2883,7 +2883,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
// XXX this should not be necessary any more, we should be able to
// just delete this and respond to mode switches
/* if offboard is set already by a mavlink command, abort */
if (status.offboard_control_set_by_command) {
if (status_flags.offboard_control_set_by_command) {
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags);
}

6
src/modules/commander/state_machine_helper.cpp

@ -399,7 +399,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta @@ -399,7 +399,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */
if (!status->offboard_control_signal_lost) {
if (!status_flags->offboard_control_signal_lost) {
ret = TRANSITION_CHANGED;
}
@ -853,11 +853,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en @@ -853,11 +853,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */
if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {
status->failsafe = true;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
} else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {
status->failsafe = true;
if (status_flags->condition_local_position_valid) {

4
src/modules/commander/state_machine_helper.h

@ -75,6 +75,10 @@ struct status_flags_s { @@ -75,6 +75,10 @@ struct status_flags_s {
bool circuit_breaker_engaged_enginefailure_check;
bool circuit_breaker_engaged_gpsfailure_check;
bool cb_usb;
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;
bool offboard_control_set_by_command; // true if the offboard mode was set by a mavlink command and should not be overridden by RC
};
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);

Loading…
Cancel
Save