diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-08 21:06:55 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-08 21:06:55 +0200 |
| commit | d3ac8c9ff31ac609d555613e552b38782a2b2490 (patch) | |
| tree | 4917b7388783df3b75cd0a28401e5058849aa4a4 | |
| parent | 2e8b675c6c5ddc3fd2db22a8e22a664b50ef18a9 (diff) | |
| download | px4-firmware-d3ac8c9ff31ac609d555613e552b38782a2b2490.tar.gz px4-firmware-d3ac8c9ff31ac609d555613e552b38782a2b2490.tar.bz2 px4-firmware-d3ac8c9ff31ac609d555613e552b38782a2b2490.zip | |
Fixed HIL mode switching, HIL works
| -rw-r--r-- | src/modules/commander/commander.cpp | 10 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 2 |
2 files changed, 10 insertions, 2 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 830ee9743..5eeb018fd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -343,6 +343,9 @@ void print_status() warnx("arming: %s", armed_str); } +static orb_advert_t control_mode_pub; +static orb_advert_t status_pub; + void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -356,6 +359,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + // TODO remove debug code //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ @@ -543,7 +550,6 @@ int commander_thread_main(int argc, char *argv[]) } /* Main state machine */ - orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value @@ -555,7 +561,7 @@ int commander_thread_main(int argc, char *argv[]) /* flags for control apps */ struct vehicle_control_mode_s control_mode; - orb_advert_t control_mode_pub; + /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cef10185..7d759b8d2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -502,6 +502,8 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_control_mode->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + // XXX also set lockdown here + ret = OK; } else { |