diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-30 17:23:10 +0100 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-30 17:23:10 +0100 |
commit | f64872b9b305df51bad14c4cbf5519e3e4b95030 (patch) | |
tree | 67c3d4d000b262baf97543310dd34e02e2bce15d /src/modules/commander | |
parent | 13a5b5b4a3d8e487fb836e14185d61c03493ef0a (diff) | |
parent | 2f267fbf439d867d47b6772471d0f203036eefa2 (diff) | |
download | px4-firmware-f64872b9b305df51bad14c4cbf5519e3e4b95030.tar.gz px4-firmware-f64872b9b305df51bad14c4cbf5519e3e4b95030.tar.bz2 px4-firmware-f64872b9b305df51bad14c4cbf5519e3e4b95030.zip |
Merge branch 'master' into beta
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 53 |
1 files changed, 46 insertions, 7 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 901f91911..d318c7017 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,6 +153,11 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static struct vehicle_status_s status; +static struct actuator_armed_s armed; +static struct safety_s safety; +static struct vehicle_control_mode_s control_mode; + /* tasks waiting for low prio thread */ typedef enum { LOW_PRIO_TASK_NONE = 0, @@ -211,6 +216,11 @@ void print_reject_arm(const char *msg); void print_status(); +int arm(); +int disarm(); + +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -236,7 +246,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2088, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -276,6 +286,16 @@ int commander_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "arm")) { + arm(); + exit(0); + } + + if (!strcmp(argv[1], "disarm")) { + disarm(); + exit(0); + } + usage("unrecognized command"); exit(1); } @@ -342,6 +362,30 @@ void print_status() static orb_advert_t status_pub; +int arm() +{ + int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + } else { + return 1; + } +} + +int disarm() +{ + int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + } else { + return 1; + } +} + bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -556,11 +600,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } -static struct vehicle_status_s status; -static struct vehicle_control_mode_s control_mode; -static struct actuator_armed_s armed; -static struct safety_s safety; - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -672,7 +711,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2992); + pthread_attr_setstacksize(&commander_low_prio_attr, 1728); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); |