diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-16 13:09:35 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-16 13:09:35 +0200 |
commit | b30e443f282981cbbf836cb087e0d1e0aeb72496 (patch) | |
tree | 4369de57a672b815038c4607f169eb6e3ebce2d9 /apps | |
parent | e84d0f41fa17e3e83ff3d58861ee572570604c19 (diff) | |
download | px4-firmware-b30e443f282981cbbf836cb087e0d1e0aeb72496.tar.gz px4-firmware-b30e443f282981cbbf836cb087e0d1e0aeb72496.tar.bz2 px4-firmware-b30e443f282981cbbf836cb087e0d1e0aeb72496.zip |
Updated start script, checking commander mishaps
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/state_machine_helper.c | 7 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 4 |
2 files changed, 9 insertions, 2 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a62b1437a..b44d2150c 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 66b3865cc..dc8b2a2c8 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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); } |