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 | |
| parent | e84d0f41fa17e3e83ff3d58861ee572570604c19 (diff) | |
| download | px4-firmware-b30e443f282981cbbf836cb087e0d1e0aeb72496.tar.gz px4-firmware-b30e443f282981cbbf836cb087e0d1e0aeb72496.tar.bz2 px4-firmware-b30e443f282981cbbf836cb087e0d1e0aeb72496.zip | |
Updated start script, checking commander mishaps
| -rw-r--r-- | ROMFS/scripts/rc.PX4IOAR | 7 | ||||
| -rw-r--r-- | apps/commander/state_machine_helper.c | 7 | ||||
| -rw-r--r-- | apps/mavlink/mavlink.c | 4 |
3 files changed, 13 insertions, 5 deletions
diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index d626ca213..d3cf8b506 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -19,6 +19,7 @@ sh /etc/init.d/rc.sensors # Start MAVLink # mavlink -d /dev/ttyS0 -b 57600 & +usleep 5000 # # Start the commander. @@ -40,7 +41,7 @@ attitude_estimator_bm & # # XXX arguments? # -px4fmu start +#fmu mode_ # # Fire up the AR.Drone controller. @@ -54,12 +55,12 @@ ardrone_control -d /dev/ttyS1 -m attitude & # # XXX this should not need to be backgrounded # -gps -d /dev/ttyS3 -m all & +#gps -d /dev/ttyS3 -m all & # # Start logging to microSD if we can # -sh /etc/init.d/rc.logging +#sh /etc/init.d/rc.logging # # startup is done; we don't want the shell because we 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); } |