diff --git a/apps/multirotor_control/multirotor_attitude_control.c b/apps/multirotor_control/multirotor_attitude_control.c index 5a4960d60b..4391c39577 100644 --- a/apps/multirotor_control/multirotor_attitude_control.c +++ b/apps/multirotor_control/multirotor_attitude_control.c @@ -38,7 +38,8 @@ ****************************************************************************/ /** - * @file multirotor_control.c + * @file multirotor_attitude_control.c + * * Implementation of multirotor attitude controller. */ @@ -54,7 +55,7 @@ #include "pid.h" #include -extern int ardrone_write; +extern int multirotor_write; extern int gpios; #define CONTROL_PID_ATTITUDE_INTERVAL 5e-3 @@ -84,7 +85,10 @@ void navi2body_xy_plane(const float_vect3 *vector, const float yaw, // result->z = vector->z; //leave direction normal to xy-plane untouched } -void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control) +void multirotor_control_attitude(const struct rc_channels_s *rc, + const struct vehicle_attitude_s *att, + const struct vehicle_status_s *status, + struct actuator_controls_s *actuators) { static int motor_skip_counter = 0; @@ -352,7 +356,7 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit // ar_control->attitude_control_output[1] = nick; // ar_control->attitude_control_output[2] = yaw; // ar_control->zcompensation = zcompensation; - // orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control); + // orb_publish(ORB_ID(multirotor_control), multirotor_pub, ar_control); static float output_band = 0.f; static float band_factor = 0.75f; diff --git a/apps/multirotor_control/multirotor_attitude_control.h b/apps/multirotor_control/multirotor_attitude_control.h index 6f66926c0b..c3aa470413 100644 --- a/apps/multirotor_control/multirotor_attitude_control.h +++ b/apps/multirotor_control/multirotor_attitude_control.h @@ -34,7 +34,11 @@ * ****************************************************************************/ -/* @file attitude control for quadrotors */ +/** + * @file multirotor_attitude_control.h + * + * attitude control for multirotors + */ #ifndef ATTITUDE_CONTROL_H_ #define ATTITUDE_CONTROL_H_ @@ -42,11 +46,10 @@ #include #include #include -#include #include +#include -void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, - const struct vehicle_status_s *status, int ardrone_pub, - struct ardrone_control_s *ar_control); +void multirotor_control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, + const struct vehicle_status_s *status, struct actuator_controls_s *actuators); #endif /* ATTITUDE_CONTROL_H_ */ diff --git a/apps/multirotor_control/multirotor_control.c b/apps/multirotor_control/multirotor_control.c index 3b54dead74..2edc1ba84f 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 #include #include -#include +#include #include +#include #include #include #include "multirotor_control.h" @@ -55,144 +57,151 @@ #include #include #include -#include +#include #include -#include -__EXPORT int ardrone_control_main(int argc, char *argv[]); +#include -/**************************************************************************** - * 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; - -/**************************************************************************** - * 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; -// } - -/**************************************************************************** - * main - ****************************************************************************/ - -int ardrone_control_main(int argc, char *argv[]) +static int +mc_thread_main(int argc, char *argv[]) { - /* welcome user */ - printf("[ardrone_control] Control started, taking over motors\n"); - - /* default values for arguments */ - char *ardrone_uart_name = "/dev/ttyS1"; - control_mode = CONTROL_MODE_RATES; - - char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n"; - - /* read commandline arguments */ - int i; - - 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]; - - } else { - printf(commandline_usage); - return 0; - } - - } 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; - - } else if (strcmp(argv[i + 1], "attitude") == 0) { - control_mode = CONTROL_MODE_ATTITUDE; - - } else { - printf(commandline_usage); - return 0; - } - - } else { - printf(commandline_usage); - return 0; - } - } - } - - /* initialize motors */ - - int counter = 0; - - /* pthread for position control */ - // pthread_t position_control_thread; - // position_control_thread_started = false; - /* structures */ struct vehicle_status_s state; struct vehicle_attitude_s att; struct rc_channels_s rc; - struct sensor_combined_s raw; + struct actuator_controls_s actuators; + struct actuator_armed_s armed; /* 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)); - /* publish AR.Drone motor control state */ - // int ardrone_pub = orb_advertise(ORB_ID(ardrone_control), &ar_control); + /* 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"); + + /* welcome user */ + printf("[multirotor_control] starting\n"); + + while (!thread_should_exit) { + + /* wait for a sensor update */ + poll(&fds, 1, -1); + + perf_begin(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 */ + + /* get a local copy of rc inputs */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - multirotor_control_attitude(&rc, &att, &state); + /* run the attitude controller */ + multirotor_control_attitude(&rc, &att, &state, &actuators); - /* run at approximately 200 Hz */ - usleep(5000); - counter++; + /* publish the result */ + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + perf_end(mc_loop_perf); } - printf("[ardrone_control] ending now...\r\n"); + printf("[multirotor_control] stopping\r\n"); + + /* 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); + + + close(att_sub); + close(state_sub); + close(rc_sub); + close(actuator_pub); + close(armed_pub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + fflush(stdout); - return 0; + exit(0); } +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: multirotor_control [-m ] {start|stop}\n"); + fprintf(stderr, " 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"); + + if (!strcmp(argv[1], "start")) { + + thread_should_exit = false; + mc_task = task_create("multirotor_attitude", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + usage("unrecognised command"); + exit(1); +} diff --git a/apps/multirotor_control/multirotor_control.h b/apps/multirotor_control/multirotor_control.h index 7f9567f861..cd779d3691 100644 --- a/apps/multirotor_control/multirotor_control.h +++ b/apps/multirotor_control/multirotor_control.h @@ -1,12 +1,12 @@ -/* - * ardrone_control.h +/** + * @file multirotor_control.h * * Created on: Mar 23, 2012 * Author: thomasgubler */ -#ifndef ARDRONE_CONTROL_H_ -#define ARDRONE_CONTROL_H_ +#ifndef multirotor_CONTROL_H_ +#define multirotor_CONTROL_H_ -#endif /* ARDRONE_CONTROL_H_ */ +#endif /* multirotor_CONTROL_H_ */