fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 06:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated start script, checking commander mishaps

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-16 13:09:35 +02:00
parent e84d0f41fa
commit b30e443f28
3 changed files with 13 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/state_machine_helper.c
+5 -2
View File
@@ -111,7 +111,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
invalid_state = false;
mavlink_log_info(mavlink_fd, "[commander] Switched to PREFLIGHT state");
mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
@@ -161,6 +161,9 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
current_status->state_machine = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
}
if (invalid_state) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
}
}
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
@@ -476,7 +479,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->control_manual_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
printf("[commander] auto mode\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
apps/mavlink/mavlink.c
+4
View File
@@ -925,6 +925,10 @@ void handleMessage(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
}
/* create command */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}