diff options
Diffstat (limited to 'apps/multirotor_control/multirotor_control.c')
-rw-r--r-- | apps/multirotor_control/multirotor_control.c | 221 |
1 files changed, 115 insertions, 106 deletions
diff --git a/apps/multirotor_control/multirotor_control.c b/apps/multirotor_control/multirotor_control.c index 3b54dead7..2edc1ba84 100644 --- a/apps/multirotor_control/multirotor_control.c +++ b/apps/multirotor_control/multirotor_control.c @@ -34,6 +34,7 @@ /* * @file multirotor_control.c + * * Implementation of multirotor controllers */ @@ -46,8 +47,9 @@ #include <fcntl.h> #include <errno.h> #include <debug.h> -#include <termios.h> +#include <getopt.h> #include <time.h> +#include <poll.h> #include <sys/prctl.h> #include <arch/board/up_hrt.h> #include "multirotor_control.h" @@ -55,144 +57,151 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/ardrone_control.h> +#include <uORB/topics/actuator_controls.h> #include <uORB/topics/rc_channels.h> -#include <uORB/topics/sensor_combined.h> -__EXPORT int ardrone_control_main(int argc, char *argv[]); +#include <systemlib/perf_counter.h> -/**************************************************************************** - * Internal Definitions - ****************************************************************************/ +__EXPORT int multirotor_control_main(int argc, char *argv[]); -enum { +static enum { CONTROL_MODE_RATES = 0, CONTROL_MODE_ATTITUDE = 1, } control_mode; -/**************************************************************************** - * Private Data - ****************************************************************************/ -/*File descriptors */ -int ardrone_write; -int gpios; +static bool thread_should_exit; +static int mc_task; -bool position_control_thread_started; +static int +mc_thread_main(int argc, char *argv[]) +{ + /* structures */ + struct vehicle_status_s state; + struct vehicle_attitude_s att; + struct rc_channels_s rc; + struct actuator_controls_s actuators; + struct actuator_armed_s armed; -/**************************************************************************** - * pthread loops - ****************************************************************************/ -// static void *position_control_loop(void *arg) -// { -// struct vehicle_status_s *state = (struct vehicle_status_s *)arg; -// // Set thread name -// prctl(PR_SET_NAME, "ardrone pos ctrl", getpid()); - -// while (1) { -// if (state->state_machine == SYSTEM_STATE_AUTO) { -// // control_position(); //FIXME TODO XXX -// /* temporary 50 Hz execution */ -// usleep(20000); - -// } else { -// position_control_thread_started = false; -// break; -// } -// } - -// return NULL; -// } + /* subscribe to attitude, motor setpoints and system state */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int rc_sub = orb_subscribe(ORB_ID(rc_channels)); -/**************************************************************************** - * main - ****************************************************************************/ + /* rate-limit the attitude subscription to 200Hz to pace our loop */ + orb_set_interval(att_sub, 5); + struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + + /* publish actuator controls */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + actuators.control[i] = 0.0f; + int actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + armed.armed = true; + int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + + /* register the perf counter */ + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_control"); -int ardrone_control_main(int argc, char *argv[]) -{ /* welcome user */ - printf("[ardrone_control] Control started, taking over motors\n"); + printf("[multirotor_control] starting\n"); - /* default values for arguments */ - char *ardrone_uart_name = "/dev/ttyS1"; - control_mode = CONTROL_MODE_RATES; + while (!thread_should_exit) { - char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n"; + /* wait for a sensor update */ + poll(&fds, 1, -1); - /* read commandline arguments */ - int i; + perf_begin(mc_loop_perf); - for (i = 1; i < argc; i++) { - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set - if (argc > i + 1) { - ardrone_uart_name = argv[i + 1]; + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); - } else { - printf(commandline_usage); - return 0; - } + /* get a local copy of rc inputs */ + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - } else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) { - if (argc > i + 1) { - if (strcmp(argv[i + 1], "rates") == 0) { - control_mode = CONTROL_MODE_RATES; + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - } else if (strcmp(argv[i + 1], "attitude") == 0) { - control_mode = CONTROL_MODE_ATTITUDE; + /* run the attitude controller */ + multirotor_control_attitude(&rc, &att, &state, &actuators); - } else { - printf(commandline_usage); - return 0; - } + /* publish the result */ + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } else { - printf(commandline_usage); - return 0; - } - } + perf_end(mc_loop_perf); } - /* initialize motors */ - - int counter = 0; + printf("[multirotor_control] stopping\r\n"); - /* pthread for position control */ - // pthread_t position_control_thread; - // position_control_thread_started = false; + /* kill all outputs */ + armed.armed = false; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + actuators.control[i] = 0.0f; + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* structures */ - struct vehicle_status_s state; - struct vehicle_attitude_s att; - struct rc_channels_s rc; - struct sensor_combined_s raw; - /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + close(att_sub); + close(state_sub); + close(rc_sub); + close(actuator_pub); + close(armed_pub); - /* publish AR.Drone motor control state */ - // int ardrone_pub = orb_advertise(ORB_ID(ardrone_control), &ar_control); + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); - while (1) { - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of rc */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + fflush(stdout); + exit(0); +} + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: multirotor_control [-m <mode>] {start|stop}\n"); + fprintf(stderr, " <mode> is 'rates' or 'attitude'\n"); + exit(1); +} + +int multirotor_control_main(int argc, char *argv[]) +{ + int ch; + + control_mode = CONTROL_MODE_RATES; + + while ((ch = getopt(argc, argv, "m:")) != EOF) { + switch (ch) { + case 'm': + if (!strcmp(optarg, "rates")) { + control_mode = CONTROL_MODE_RATES; + } else if (!strcmp(optarg, "attitude")) { + control_mode = CONTROL_MODE_RATES; + } else { + usage("unrecognized -m value"); + } + default: + usage("unrecognized option"); + } + } + argc -= optind; + argv += optind; + + if (argc < 1) + usage("missing command"); - multirotor_control_attitude(&rc, &att, &state); + if (!strcmp(argv[1], "start")) { - /* run at approximately 200 Hz */ - usleep(5000); - counter++; + thread_should_exit = false; + mc_task = task_create("multirotor_attitude", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL); + exit(0); } - printf("[ardrone_control] ending now...\r\n"); - fflush(stdout); - return 0; -} + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + usage("unrecognised command"); + exit(1); +} |