dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fixed HIL mode switching, HIL works

This commit is contained in:
Lorenz Meier 2013-09-08 21:06:55 +02:00
parent 2e8b675c6c
commit d3ac8c9ff3
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/commander/commander.cpp
View File

@ -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));

2
src/modules/commander/state_machine_helper.cpp
View File

@ -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 {