diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-11 19:45:32 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-11 19:45:32 +0200 |
commit | 18c6c620c048abba7cbd2530f80ea6d0207704b1 (patch) | |
tree | 9f8521489700a7de49c09f11ce297433730ebdef /apps/commander/commander.c | |
parent | 4eef4e186437c6b923df7b9dcffdc3723c411560 (diff) | |
download | px4-firmware-18c6c620c048abba7cbd2530f80ea6d0207704b1.tar.gz px4-firmware-18c6c620c048abba7cbd2530f80ea6d0207704b1.tar.bz2 px4-firmware-18c6c620c048abba7cbd2530f80ea6d0207704b1.zip |
Added manual control abstraction layer, reworked sensors and ardrone_control apps to use it instead of direct RC channels
Diffstat (limited to 'apps/commander/commander.c')
-rw-r--r-- | apps/commander/commander.c | 19 |
1 files changed, 12 insertions, 7 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index dc5647223..3663503d1 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -631,8 +631,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } - -static void *command_handling_loop(void *arg) //handles commands which come from the mavlink app +/** + * Handle commands sent by the ground control station via MAVLink. + */ +static void *command_handling_loop(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "commander cmd handler", getpid()); @@ -645,7 +647,7 @@ static void *command_handling_loop(void *arg) //handles commands which come fro struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } }; if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem */ + /* timeout, but this is no problem, silently ignore */ } else { /* got command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); @@ -785,7 +787,8 @@ int commander_main(int argc, char *argv[]) stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); if (stat_pub < 0) { - printf("[commander] ERROR: orb_advertise failed.\n"); + printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n"); + exit(ERROR); } /* make sure we are in preflight state */ @@ -1035,6 +1038,8 @@ int commander_main(int argc, char *argv[]) int16_t rc_yaw_scale = rc.chan[rc.function[YAW]].scale; int16_t rc_throttle_scale = rc.chan[rc.function[THROTTLE]].scale; int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; + /* Check the value of the rc channel of the mode switch */ + mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; /* check if left stick is in lower left position --> switch to standby state */ if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values @@ -1061,9 +1066,6 @@ int commander_main(int argc, char *argv[]) } //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); - /* Check the value of the rc channel of the mode switch */ - mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; - if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { update_state_machine_mode_manual(stat_pub, ¤t_status); @@ -1074,6 +1076,9 @@ int commander_main(int argc, char *argv[]) update_state_machine_mode_stabilized(stat_pub, ¤t_status); } + /* Publish RC signal */ + + /* handle the case where RC signal was regained */ if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); current_status.rc_signal_lost = false; |