From 85bc4f683a4a88cc19a35e1147d19ac5b1106019 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Aug 2012 11:29:07 +0200 Subject: [PATCH 1/4] Cleaned up position control (WIP), moved PID structs (should become classes) to systemlib, added deamon app example --- apps/ardrone_control/ardrone_control_helper.c | 60 --- apps/ardrone_control/ardrone_control_helper.h | 21 - apps/examples/px4_deamon_app/Makefile | 42 ++ apps/examples/px4_deamon_app/px4_deamon_app.c | 130 +++++ .../multirotor_attitude_control.c | 45 +- apps/multirotor_control/pid.h | 40 -- apps/multirotor_position_control/Makefile | 9 +- .../ardrone_control.c | 290 ----------- .../ardrone_control.h | 12 - .../ardrone_control_helper.c | 60 --- .../ardrone_control_helper.h | 21 - .../ardrone_motor_control.c | 281 ----------- .../ardrone_motor_control.h | 61 --- .../attitude_control.c | 459 ------------------ .../attitude_control.h | 52 -- .../multirotor_position_control.c | 136 ++++++ apps/multirotor_position_control/pid.c | 109 ----- apps/multirotor_position_control/pid.h | 40 -- .../position_control.c | 81 +--- .../position_control.h | 47 +- .../rate_control.c | 320 ------------ apps/sensors/sensors.c | 2 +- .../pid}/pid.c | 43 +- .../rate_control.h => systemlib/pid/pid.h} | 48 +- nuttx/configs/px4fmu/nsh/appconfig | 4 + 25 files changed, 482 insertions(+), 1931 deletions(-) delete mode 100644 apps/ardrone_control/ardrone_control_helper.c delete mode 100644 apps/ardrone_control/ardrone_control_helper.h create mode 100644 apps/examples/px4_deamon_app/Makefile create mode 100644 apps/examples/px4_deamon_app/px4_deamon_app.c delete mode 100644 apps/multirotor_control/pid.h delete mode 100644 apps/multirotor_position_control/ardrone_control.c delete mode 100644 apps/multirotor_position_control/ardrone_control.h delete mode 100644 apps/multirotor_position_control/ardrone_control_helper.c delete mode 100644 apps/multirotor_position_control/ardrone_control_helper.h delete mode 100644 apps/multirotor_position_control/ardrone_motor_control.c delete mode 100644 apps/multirotor_position_control/ardrone_motor_control.h delete mode 100644 apps/multirotor_position_control/attitude_control.c delete mode 100644 apps/multirotor_position_control/attitude_control.h create mode 100644 apps/multirotor_position_control/multirotor_position_control.c delete mode 100644 apps/multirotor_position_control/pid.c delete mode 100644 apps/multirotor_position_control/pid.h delete mode 100644 apps/multirotor_position_control/rate_control.c rename apps/{multirotor_control => systemlib/pid}/pid.c (53%) rename apps/{multirotor_position_control/rate_control.h => systemlib/pid/pid.h} (61%) diff --git a/apps/ardrone_control/ardrone_control_helper.c b/apps/ardrone_control/ardrone_control_helper.c deleted file mode 100644 index c073119e09..0000000000 --- a/apps/ardrone_control/ardrone_control_helper.c +++ /dev/null @@ -1,60 +0,0 @@ -#include "ardrone_control_helper.h" -#include -#include -#include -#include - -// int read_sensors_raw(sensors_raw_t *sensors_raw) -// { -// static int ret; -// ret = global_data_wait(&global_data_sensors_raw->access_conf_rate_full); - -// if (ret == 0) { -// memcpy(sensors_raw->gyro_raw, global_data_sensors_raw->gyro_raw, sizeof(sensors_raw->gyro_raw)); -// // printf("Timestamp %d\n", &global_data_sensors_raw->timestamp); - -// } else { -// printf("Controller timeout, no new sensor values available\n"); -// } - -// global_data_unlock(&global_data_sensors_raw->access_conf_rate_full); -// return ret; -// } - -// int read_attitude(global_data_attitude_t *attitude) -// { - -// static int ret; -// ret = global_data_wait(&global_data_attitude->access_conf); - -// if (ret == 0) { -// memcpy(&attitude->roll, &global_data_attitude->roll, sizeof(global_data_attitude->roll)); -// memcpy(&attitude->pitch, &global_data_attitude->pitch, sizeof(global_data_attitude->pitch)); -// memcpy(&attitude->yaw, &global_data_attitude->yaw, sizeof(global_data_attitude->yaw)); -// memcpy(&attitude->rollspeed, &global_data_attitude->rollspeed, sizeof(global_data_attitude->rollspeed)); -// memcpy(&attitude->pitchspeed, &global_data_attitude->pitchspeed, sizeof(global_data_attitude->pitchspeed)); -// memcpy(&attitude->yawspeed, &global_data_attitude->yawspeed, sizeof(global_data_attitude->yawspeed)); - -// } else { -// printf("Controller timeout, no new attitude values available\n"); -// } - -// global_data_unlock(&global_data_attitude->access_conf); - - - -// return ret; -// } - -// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint) -// { - -// if (0 == global_data_trylock(&global_data_quad_motors_setpoint->access_conf)) { //TODO: check if trylock is the right choice, maybe only lock? -// rate_setpoint->motor_front_nw = global_data_quad_motors_setpoint->motor_front_nw; -// rate_setpoint->motor_right_ne = global_data_quad_motors_setpoint->motor_right_ne; -// rate_setpoint->motor_back_se = global_data_quad_motors_setpoint->motor_back_se; -// rate_setpoint->motor_left_sw = global_data_quad_motors_setpoint->motor_left_sw; - -// global_data_unlock(&global_data_quad_motors_setpoint->access_conf); -// } -// } diff --git a/apps/ardrone_control/ardrone_control_helper.h b/apps/ardrone_control/ardrone_control_helper.h deleted file mode 100644 index 22eebe986d..0000000000 --- a/apps/ardrone_control/ardrone_control_helper.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * ardrone_control_helper.h - * - * Created on: May 15, 2012 - * Author: thomasgubler - */ - -#ifndef ARDRONE_CONTROL_HELPER_H_ -#define ARDRONE_CONTROL_HELPER_H_ - -#include - -// typedef struct { -// int16_t gyro_raw[3]; // l3gd20 -// } sensors_raw_t; - -// /* Copy quad_motors_setpoint values from global memory to private variables */ //TODO: change this once the new mavlink message for rates is available -// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint); - - -#endif /* ARDRONE_CONTROL_HELPER_H_ */ diff --git a/apps/examples/px4_deamon_app/Makefile b/apps/examples/px4_deamon_app/Makefile new file mode 100644 index 0000000000..e4c169872b --- /dev/null +++ b/apps/examples/px4_deamon_app/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Basic example application +# + +APPNAME = px4_deamon_app +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c new file mode 100644 index 0000000000..e17bf478af --- /dev/null +++ b/apps/examples/px4_deamon_app/px4_deamon_app.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Example User + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_deamon_app.c + * Deamon application example for PX4 autopilot + */ + +#include +#include +#include + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +__EXPORT int px4_deamon_app_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int mavlink_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +int mavlink_thread_main(int argc, char *argv[]) { + + printf("[deamon] starting\n"); + + while (true) { + printf("Hello Deamon!\n"); + sleep(1); + if (thread_should_exit) break; + } + + printf("[deamon] exiting.\n"); + + return 0; +} + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int px4_deamon_app_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("deamon already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + mavlink_task = task_create("deamon", SCHED_PRIORITY_DEFAULT, 4096, deamon_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + thread_running = true; + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tdeamon app is running\n"); + } else { + printf("\tdeamon app not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} diff --git a/apps/multirotor_control/multirotor_attitude_control.c b/apps/multirotor_control/multirotor_attitude_control.c index 4391c39577..5d298ab543 100644 --- a/apps/multirotor_control/multirotor_attitude_control.c +++ b/apps/multirotor_control/multirotor_attitude_control.c @@ -49,41 +49,40 @@ #include #include #include -#include //TODO: move matrix.h to somewhere else? #include #include -#include "pid.h" #include +#include extern int multirotor_write; extern int gpios; #define CONTROL_PID_ATTITUDE_INTERVAL 5e-3 -void turn_xy_plane(const float_vect3 *vector, float yaw, - float_vect3 *result) -{ - //turn clockwise - static uint16_t counter; +// void turn_xy_plane(const float_vect3 *vector, float yaw, +// float_vect3 *result) +// { +// //turn clockwise +// static uint16_t counter; - result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y); - result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y); - result->z = vector->z; //leave direction normal to xy-plane untouched +// result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y); +// result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y); +// result->z = vector->z; //leave direction normal to xy-plane untouched - counter++; -} +// counter++; +// } -void navi2body_xy_plane(const float_vect3 *vector, const float yaw, - float_vect3 *result) -{ - turn_xy_plane(vector, yaw, result); -// result->x = vector->x; -// result->y = vector->y; -// result->z = vector->z; - // result->x = cos(yaw) * vector->x + sin(yaw) * vector->y; - // result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y; - // result->z = vector->z; //leave direction normal to xy-plane untouched -} +// void navi2body_xy_plane(const float_vect3 *vector, const float yaw, +// float_vect3 *result) +// { +// turn_xy_plane(vector, yaw, result); +// // result->x = vector->x; +// // result->y = vector->y; +// // result->z = vector->z; +// // result->x = cos(yaw) * vector->x + sin(yaw) * vector->y; +// // result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y; +// // result->z = vector->z; //leave direction normal to xy-plane untouched +// } void multirotor_control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, diff --git a/apps/multirotor_control/pid.h b/apps/multirotor_control/pid.h deleted file mode 100644 index a721c839c6..0000000000 --- a/apps/multirotor_control/pid.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * pid.h - * - * Created on: May 29, 2012 - * Author: thomasgubler - */ - -#ifndef PID_H_ -#define PID_H_ - -#include - -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC 0 -/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 1 - -typedef struct { - float kp; - float ki; - float kd; - float intmax; - float sp; - float integral; - float error_previous; - uint8_t mode; - uint8_t plot_i; - uint8_t count; - uint8_t saturated; -} PID_t; - -void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i); -void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); -//void pid_set(PID_t *pid, float sp); -float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); - - - -#endif /* PID_H_ */ diff --git a/apps/multirotor_position_control/Makefile b/apps/multirotor_position_control/Makefile index 23e2d49fb4..f95a4b50a3 100644 --- a/apps/multirotor_position_control/Makefile +++ b/apps/multirotor_position_control/Makefile @@ -32,14 +32,11 @@ ############################################################################ # -# Makefile to build uORB +# Makefile to build multirotor position control # -APPNAME = ardrone_control -PRIORITY = SCHED_PRIORITY_MAX - 15 +APPNAME = multirotor_position_control +PRIORITY = SCHED_PRIORITY_MAX - 25 STACKSIZE = 2048 -# explicit list of sources - not everything is built currently -CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c - include $(APPDIR)/mk/app.mk diff --git a/apps/multirotor_position_control/ardrone_control.c b/apps/multirotor_position_control/ardrone_control.c deleted file mode 100644 index 89983724e1..0000000000 --- a/apps/multirotor_position_control/ardrone_control.c +++ /dev/null @@ -1,290 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file Implementation of AR.Drone 1.0 / 2.0 control interface - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "ardrone_control.h" -#include "attitude_control.h" -#include "rate_control.h" -#include "ardrone_motor_control.h" -#include "position_control.h" -#include -#include -#include -#include -#include -#include -#include - -#include "ardrone_control_helper.h" - -__EXPORT int ardrone_control_main(int argc, char *argv[]); - -/**************************************************************************** - * Internal Definitions - ****************************************************************************/ - - -enum { - CONTROL_MODE_RATES = 0, - CONTROL_MODE_ATTITUDE = 1, -} control_mode; - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/*File descriptors */ -int ardrone_write; -int gpios; - -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[]) -{ - /* 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_ATTITUDE; - - 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; - } - } - } - - /* open uarts */ - printf("[ardrone_control] AR.Drone UART is %s\n", ardrone_uart_name); - ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY); - if (ardrone_write < 0) { - printf("[ardrone_control] Failed opening AR.Drone UART, exiting.\n"); - exit(ERROR); - } - - /* initialize motors */ - ar_init_motors(ardrone_write, &gpios); - int counter = 0; - - /* Led animation */ - int led_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 ardrone_control_s ar_control; - struct manual_control_setpoint_s manual; - struct sensor_combined_s raw; - struct ardrone_motors_setpoint_s setpoint; - - /* 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 manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); - - /* publish AR.Drone motor control state */ - int ardrone_pub = orb_advertise(ORB_ID(ardrone_control), &ar_control); - - while (1) { - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - - if (state.state_machine == SYSTEM_STATE_AUTO) { - if (false == position_control_thread_started) { - pthread_attr_t position_control_thread_attr; - pthread_attr_init(&position_control_thread_attr); - pthread_attr_setstacksize(&position_control_thread_attr, 2048); - pthread_create(&position_control_thread, &position_control_thread_attr, position_control_loop, &state); - position_control_thread_started = true; - } - - control_attitude(0, 0, 0, 0, &att, &state, ardrone_pub, &ar_control); - - //No check for remote sticks to disarm in auto mode, land/disarm with ground station - - } else if (state.state_machine == SYSTEM_STATE_MANUAL) { - if (control_mode == CONTROL_MODE_RATES) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint); - control_rates(&raw, &setpoint); - - } else if (control_mode == CONTROL_MODE_ATTITUDE) { - - // XXX Add failsafe logic for RC loss situations - /* hardcore, last-resort safety checking */ - //if (status->rc_signal_lost) { - - control_attitude(manual.roll, manual.pitch, manual.yaw, - manual.throttle, &att, &state, ardrone_pub, &ar_control); - } - - } else { - - } - - if (counter % 30 == 0) { - if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); - - if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); - - if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); - - if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); - - if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); - - if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); - - if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); - - if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); - - if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); - - if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); - - if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); - - if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); - - led_counter++; - - if (led_counter == 12) led_counter = 0; - } - - /* run at approximately 200 Hz */ - usleep(5000); - - // This is a hardcore debug code piece to validate - // the motor interface - // uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5}; - // ar_get_motor_packet(motorSpeedBuf, 20, 20, 20, 20); - // write(ardrone_write, motorSpeedBuf, 5); - // usleep(15000); - - counter++; - } - - /* close uarts */ - close(ardrone_write); - ar_multiplexing_deinit(gpios); - - printf("[ardrone_control] ending now...\r\n"); - fflush(stdout); - return 0; -} - diff --git a/apps/multirotor_position_control/ardrone_control.h b/apps/multirotor_position_control/ardrone_control.h deleted file mode 100644 index 7f9567f861..0000000000 --- a/apps/multirotor_position_control/ardrone_control.h +++ /dev/null @@ -1,12 +0,0 @@ -/* - * ardrone_control.h - * - * Created on: Mar 23, 2012 - * Author: thomasgubler - */ - -#ifndef ARDRONE_CONTROL_H_ -#define ARDRONE_CONTROL_H_ - - -#endif /* ARDRONE_CONTROL_H_ */ diff --git a/apps/multirotor_position_control/ardrone_control_helper.c b/apps/multirotor_position_control/ardrone_control_helper.c deleted file mode 100644 index c073119e09..0000000000 --- a/apps/multirotor_position_control/ardrone_control_helper.c +++ /dev/null @@ -1,60 +0,0 @@ -#include "ardrone_control_helper.h" -#include -#include -#include -#include - -// int read_sensors_raw(sensors_raw_t *sensors_raw) -// { -// static int ret; -// ret = global_data_wait(&global_data_sensors_raw->access_conf_rate_full); - -// if (ret == 0) { -// memcpy(sensors_raw->gyro_raw, global_data_sensors_raw->gyro_raw, sizeof(sensors_raw->gyro_raw)); -// // printf("Timestamp %d\n", &global_data_sensors_raw->timestamp); - -// } else { -// printf("Controller timeout, no new sensor values available\n"); -// } - -// global_data_unlock(&global_data_sensors_raw->access_conf_rate_full); -// return ret; -// } - -// int read_attitude(global_data_attitude_t *attitude) -// { - -// static int ret; -// ret = global_data_wait(&global_data_attitude->access_conf); - -// if (ret == 0) { -// memcpy(&attitude->roll, &global_data_attitude->roll, sizeof(global_data_attitude->roll)); -// memcpy(&attitude->pitch, &global_data_attitude->pitch, sizeof(global_data_attitude->pitch)); -// memcpy(&attitude->yaw, &global_data_attitude->yaw, sizeof(global_data_attitude->yaw)); -// memcpy(&attitude->rollspeed, &global_data_attitude->rollspeed, sizeof(global_data_attitude->rollspeed)); -// memcpy(&attitude->pitchspeed, &global_data_attitude->pitchspeed, sizeof(global_data_attitude->pitchspeed)); -// memcpy(&attitude->yawspeed, &global_data_attitude->yawspeed, sizeof(global_data_attitude->yawspeed)); - -// } else { -// printf("Controller timeout, no new attitude values available\n"); -// } - -// global_data_unlock(&global_data_attitude->access_conf); - - - -// return ret; -// } - -// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint) -// { - -// if (0 == global_data_trylock(&global_data_quad_motors_setpoint->access_conf)) { //TODO: check if trylock is the right choice, maybe only lock? -// rate_setpoint->motor_front_nw = global_data_quad_motors_setpoint->motor_front_nw; -// rate_setpoint->motor_right_ne = global_data_quad_motors_setpoint->motor_right_ne; -// rate_setpoint->motor_back_se = global_data_quad_motors_setpoint->motor_back_se; -// rate_setpoint->motor_left_sw = global_data_quad_motors_setpoint->motor_left_sw; - -// global_data_unlock(&global_data_quad_motors_setpoint->access_conf); -// } -// } diff --git a/apps/multirotor_position_control/ardrone_control_helper.h b/apps/multirotor_position_control/ardrone_control_helper.h deleted file mode 100644 index 22eebe986d..0000000000 --- a/apps/multirotor_position_control/ardrone_control_helper.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * ardrone_control_helper.h - * - * Created on: May 15, 2012 - * Author: thomasgubler - */ - -#ifndef ARDRONE_CONTROL_HELPER_H_ -#define ARDRONE_CONTROL_HELPER_H_ - -#include - -// typedef struct { -// int16_t gyro_raw[3]; // l3gd20 -// } sensors_raw_t; - -// /* Copy quad_motors_setpoint values from global memory to private variables */ //TODO: change this once the new mavlink message for rates is available -// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint); - - -#endif /* ARDRONE_CONTROL_HELPER_H_ */ diff --git a/apps/multirotor_position_control/ardrone_motor_control.c b/apps/multirotor_position_control/ardrone_motor_control.c deleted file mode 100644 index f13427eea7..0000000000 --- a/apps/multirotor_position_control/ardrone_motor_control.c +++ /dev/null @@ -1,281 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file ardrone_motor_control.c - * Implementation of AR.Drone 1.0 / 2.0 motor control interface - */ - - - -#include "ardrone_motor_control.h" - -static const unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2; -static const unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 }; - -typedef union { - uint16_t motor_value; - uint8_t bytes[2]; -} motor_union_t; - -/** - * @brief Generate the 8-byte motor set packet - * - * @return the number of bytes (8) - */ -void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) -{ - motor_buf[0] = 0x20; - motor_buf[1] = 0x00; - motor_buf[2] = 0x00; - motor_buf[3] = 0x00; - motor_buf[4] = 0x00; - /* - * {0x20, 0x00, 0x00, 0x00, 0x00}; - * 0x20 is start sign / motor command - */ - motor_union_t curr_motor; - uint16_t nineBitMask = 0x1FF; - - /* Set motor 1 */ - curr_motor.motor_value = (motor1 & nineBitMask) << 4; - motor_buf[0] |= curr_motor.bytes[1]; - motor_buf[1] |= curr_motor.bytes[0]; - - /* Set motor 2 */ - curr_motor.motor_value = (motor2 & nineBitMask) << 3; - motor_buf[1] |= curr_motor.bytes[1]; - motor_buf[2] |= curr_motor.bytes[0]; - - /* Set motor 3 */ - curr_motor.motor_value = (motor3 & nineBitMask) << 2; - motor_buf[2] |= curr_motor.bytes[1]; - motor_buf[3] |= curr_motor.bytes[0]; - - /* Set motor 4 */ - curr_motor.motor_value = (motor4 & nineBitMask) << 1; - motor_buf[3] |= curr_motor.bytes[1]; - motor_buf[4] |= curr_motor.bytes[0]; -} - -void ar_enable_broadcast(int fd) -{ - ar_select_motor(fd, 0); -} - -int ar_multiplexing_init() -{ - int fd; - - fd = open(GPIO_DEVICE_PATH, 0); - - if (fd < 0) { - printf("GPIO: open fail\n"); - return fd; - } - - /* deactivate all outputs */ - int ret = 0; - ret += ioctl(fd, GPIO_SET, motor_gpios); - - if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) { - printf("GPIO: output set fail\n"); - close(fd); - return -1; - } - - if (ret < 0) { - printf("GPIO: clearing pins fail\n"); - close(fd); - return -1; - } - - return fd; -} - -int ar_multiplexing_deinit(int fd) -{ - if (fd < 0) { - printf("GPIO: no valid descriptor\n"); - return fd; - } - - int ret = 0; - - /* deselect motor 1-4 */ - ret += ioctl(fd, GPIO_SET, motor_gpios); - - if (ret != 0) { - printf("GPIO: clear failed %d times\n", ret); - } - - if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) { - printf("GPIO: input set fail\n"); - return -1; - } - - close(fd); - - return ret; -} - -int ar_select_motor(int fd, uint8_t motor) -{ - int ret = 0; - unsigned long gpioset; - /* - * Four GPIOS: - * GPIO_EXT1 - * GPIO_EXT2 - * GPIO_UART2_CTS - * GPIO_UART2_RTS - */ - - /* select motor 0 to enable broadcast */ - if (motor == 0) { - /* select motor 1-4 */ - ret += ioctl(fd, GPIO_CLEAR, motor_gpios); - - } else { - /* deselect all */ - ret += ioctl(fd, GPIO_SET, motor_gpios); - - /* select reqested motor */ - ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); - - /* deselect all others */ - // gpioset = motor_gpios ^ motor_gpio[motor - 1]; - // ret += ioctl(fd, GPIO_SET, gpioset); - } - - return ret; -} - -void ar_init_motors(int ardrone_uart, int *gpios_pin) -{ - /* Initialize multiplexing */ - *gpios_pin = ar_multiplexing_init(); - - /* Write ARDrone commands on UART2 */ - uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40}; - uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0}; - - /* initialize all motors - * - select one motor at a time - * - configure motor - */ - int i; - int errcounter = 0; - - for (i = 1; i < 5; ++i) { - /* Initialize motors 1-4 */ - initbuf[3] = i; - errcounter += ar_select_motor(*gpios_pin, i); - - write(ardrone_uart, initbuf + 0, 1); - - /* sleep 400 ms */ - usleep(200000); - usleep(200000); - - write(ardrone_uart, initbuf + 1, 1); - /* wait 50 ms */ - usleep(50000); - - write(ardrone_uart, initbuf + 2, 1); - /* wait 50 ms */ - usleep(50000); - - write(ardrone_uart, initbuf + 3, 1); - /* wait 50 ms */ - usleep(50000); - - write(ardrone_uart, initbuf + 4, 1); - /* wait 50 ms */ - usleep(50000); - - /* enable multicast */ - write(ardrone_uart, multicastbuf + 0, 1); - /* wait 1 ms */ - usleep(1000); - - write(ardrone_uart, multicastbuf + 1, 1); - /* wait 1 ms */ - usleep(1000); - - write(ardrone_uart, multicastbuf + 2, 1); - /* wait 1 ms */ - usleep(1000); - - write(ardrone_uart, multicastbuf + 3, 1); - /* wait 1 ms */ - usleep(1000); - - write(ardrone_uart, multicastbuf + 4, 1); - /* wait 1 ms */ - usleep(1000); - - write(ardrone_uart, multicastbuf + 5, 1); - /* wait 5 ms */ - usleep(50000); - } - - /* start the multicast part */ - errcounter += ar_select_motor(*gpios_pin, 0); - - if (errcounter != 0) { - printf("AR: init sequence incomplete, failed %d times", -errcounter); - fflush(stdout); - } -} - -/* - * Sets the leds on the motor controllers, 1 turns led on, 0 off. - */ -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) -{ - /* - * 2 bytes are sent. The first 3 bits describe the command: 011 means led control - * the following 4 bits are the red leds for motor 4, 3, 2, 1 - * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1 - * the last bit is unknown. - * - * The packet is therefore: - * 011 rrrr 0000 gggg 0 - */ - uint8_t leds[2]; - leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); - leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); - write(ardrone_uart, leds, 2); -} diff --git a/apps/multirotor_position_control/ardrone_motor_control.h b/apps/multirotor_position_control/ardrone_motor_control.h deleted file mode 100644 index bf41e3f3b4..0000000000 --- a/apps/multirotor_position_control/ardrone_motor_control.h +++ /dev/null @@ -1,61 +0,0 @@ -/**************************************************************************** - * px4/ardrone_offboard_control.h - * - * Copyright (C) 2012 PX4 Autopilot Project. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include - - -/** - * @brief Generate the 8-byte motor set packet - * - * @return the number of bytes (8) - */ -void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); - -int ar_select_motor(int fd, uint8_t motor); - -void ar_enable_broadcast(int fd); - -int ar_multiplexing_init(void); -int ar_multiplexing_deinit(int fd); - -void ar_init_motors(int ardrone_uart, int *gpios_uart); - -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); - diff --git a/apps/multirotor_position_control/attitude_control.c b/apps/multirotor_position_control/attitude_control.c deleted file mode 100644 index 8bcd33ede6..0000000000 --- a/apps/multirotor_position_control/attitude_control.c +++ /dev/null @@ -1,459 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file Implementation of attitude controller - */ - -#include "attitude_control.h" -#include -#include -#include -#include -#include -#include //TODO: move matrix.h to somewhere else? -#include "ardrone_motor_control.h" -#include -#include -#include "pid.h" -#include - -extern int ardrone_write; -extern int gpios; - -#define CONTROL_PID_ATTITUDE_INTERVAL 5e-3f - -void turn_xy_plane(const float_vect3 *vector, float yaw, - float_vect3 *result); -void navi2body_xy_plane(const float_vect3 *vector, const float yaw, - float_vect3 *result); - -void turn_xy_plane(const float_vect3 *vector, float yaw, - float_vect3 *result) -{ - //turn clockwise - static uint16_t counter; - - result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y); - result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y); - result->z = vector->z; //leave direction normal to xy-plane untouched - - counter++; -} - -void navi2body_xy_plane(const float_vect3 *vector, const float yaw, - float_vect3 *result) -{ - turn_xy_plane(vector, yaw, result); -// result->x = vector->x; -// result->y = vector->y; -// result->z = vector->z; - // result->x = cos(yaw) * vector->x + sin(yaw) * vector->y; - // result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y; - // result->z = vector->z; //leave direction normal to xy-plane untouched -} - -void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control) -{ - static int motor_skip_counter = 0; - - static PID_t yaw_pos_controller; - static PID_t yaw_speed_controller; - static PID_t nick_controller; - static PID_t roll_controller; - - const float min_thrust = 0.02f; /**< 2% minimum thrust */ - const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ - - const float min_gas = min_thrust * scaling; - const float max_gas = max_thrust * scaling; - - static uint16_t motor_pwm[4] = {0, 0, 0, 0}; - static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f}; -// static float remote_control_weight_z = 1; -// static float position_control_weight_z = 0; - - static float pid_yawpos_lim; - static float pid_yawspeed_lim; - static float pid_att_lim; - - static bool initialized; - - static float_vect3 attitude_setpoint_navigationframe_from_positioncontroller; - static commander_state_machine_t current_state; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - - pid_init(&yaw_pos_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU], - PID_MODE_DERIVATIV_CALC, 154); - - pid_init(&yaw_speed_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU], - PID_MODE_DERIVATIV_CALC, 155); - - pid_init(&nick_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], - PID_MODE_DERIVATIV_SET, 156); - - pid_init(&roll_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], - PID_MODE_DERIVATIV_SET, 157); - - pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; - pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; - pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; - - //TODO: true initialization? get gps while on ground? - attitude_setpoint_navigationframe_from_positioncontroller.x = 0.0f; - attitude_setpoint_navigationframe_from_positioncontroller.y = 0.0f; - attitude_setpoint_navigationframe_from_positioncontroller.z = 0.0f; - - initialized = true; - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 50 == 0) { - pid_set_parameters(&yaw_pos_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]); - - pid_set_parameters(&yaw_speed_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]); - - pid_set_parameters(&nick_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); - - pid_set_parameters(&roll_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); - - pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; - pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; - pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; - } - - current_state = status->state_machine; - float_vect3 attitude_setpoint_bodyframe = {.x = 0.0f, .y = 0.0f, .z = 0.0f}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller - - if (current_state == SYSTEM_STATE_AUTO) { - - attitude_setpoint_navigationframe_from_positioncontroller.x = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[0]; - attitude_setpoint_navigationframe_from_positioncontroller.y = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[1]; - attitude_setpoint_navigationframe_from_positioncontroller.z = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[2]; - - float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z; - - // don't turn around the wrong side (only works if yaw angle is between +- 180 degree) - if (yaw_e > M_PI_F) { - yaw_e -= 2.0f * M_PI_F; - } - - if (yaw_e < -M_PI_F) { - yaw_e += 2.0f * M_PI_F; - } - - attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0.0f, yaw_e, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); - - - /* limit control output */ - if (attitude_setpoint_navigationframe_from_positioncontroller.z > pid_yawpos_lim) { - attitude_setpoint_navigationframe_from_positioncontroller.z = pid_yawpos_lim; - yaw_pos_controller.saturated = 1; - } - - if (attitude_setpoint_navigationframe_from_positioncontroller.z < -pid_yawpos_lim) { - attitude_setpoint_navigationframe_from_positioncontroller.z = -pid_yawpos_lim; - yaw_pos_controller.saturated = 1; - } - - //transform attitude setpoint from position controller from navi to body frame on xy_plane - float_vect3 attitude_setpoint_bodyframe_from_positioncontroller; - navi2body_xy_plane(&attitude_setpoint_navigationframe_from_positioncontroller, att->yaw , &attitude_setpoint_bodyframe_from_positioncontroller); //yaw angle= att->yaw - //now everything is in body frame - - - //TODO: here we decide which input (position controller or ppm) we use. For now we have only the ppm, this should be decided dpending on the state machione (manula or auto) ppm should always overwrite auto (?) - attitude_setpoint_bodyframe.x = attitude_setpoint_bodyframe_from_positioncontroller.x; - attitude_setpoint_bodyframe.y = attitude_setpoint_bodyframe_from_positioncontroller.y; - attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z; - - } else if (current_state == SYSTEM_STATE_MANUAL) { - attitude_setpoint_bodyframe.x = -roll * M_PI_F / 8.0f; - attitude_setpoint_bodyframe.y = -pitch * M_PI_F / 8.0f; - attitude_setpoint_bodyframe.z = -yaw * M_PI_F; - } - - /* add an attitude offset which needs to be estimated somewhere */ - attitude_setpoint_bodyframe.x += global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET]; - attitude_setpoint_bodyframe.y += global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET]; - - /*Calculate Controllers*/ - //control Nick - float nick_control = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL); - //control Roll - float roll_control = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL); - //control Yaw Speed - float yaw_rate_control = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); //attitude_setpoint_bodyframe.z is yaw speed! - - /* - * compensate the vertical loss of thrust - * when thrust plane has an angle. - * start with a factor of 1.0 (no change) - */ - float zcompensation = 1.0f; - - if (fabsf(att->roll) > 1.0f) { - zcompensation *= 1.85081571768f; - - } else { - zcompensation *= 1.0f / cosf(att->roll); - } - - if (fabsf(att->pitch) > 1.0f) { - zcompensation *= 1.85081571768f; - - } else { - zcompensation *= 1.0f / cosf(att->pitch); - } - - // use global_data.position_control_output.z and mix parameter global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] - // to compute thrust for Z position control - // - // float motor_thrust = min_gas + - // ( ( 1 - global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] ) * ( max_gas - min_gas ) * global_data.gas_remote * zcompensation ) - // + ( global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] * ( max_gas - min_gas ) * controlled_thrust * zcompensation ); - //calculate the basic thrust - - - - float motor_thrust = 0; - - // FLYING MODES - if (current_state == SYSTEM_STATE_MANUAL) { - motor_thrust = thrust; - - } else if (current_state == SYSTEM_STATE_GROUND_READY || - current_state == SYSTEM_STATE_STABILIZED || - current_state == SYSTEM_STATE_AUTO || - current_state == SYSTEM_STATE_MISSION_ABORT) { - motor_thrust = thrust; //TODO - - } else if (current_state == SYSTEM_STATE_EMCY_LANDING) { - motor_thrust = thrust; //TODO - - } else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) { - /* immediately cut off motors */ - motor_thrust = 0; - - } else { - /* limit motor throttle to zero for an unknown mode */ - motor_thrust = 0; - } - - printf("t1:%1.3f "); - - /* compensate thrust vector for roll / pitch contributions */ - motor_thrust *= zcompensation; - - printf("t2:%1.3f\n"); - - /* limit yaw rate output */ - if (yaw_rate_control > pid_yawspeed_lim) { - yaw_rate_control = pid_yawspeed_lim; - yaw_speed_controller.saturated = 1; - } - - if (yaw_rate_control < -pid_yawspeed_lim) { - yaw_rate_control = -pid_yawspeed_lim; - yaw_speed_controller.saturated = 1; - } - - if (nick_control > pid_att_lim) { - nick_control = pid_att_lim; - nick_controller.saturated = 1; - } - - if (nick_control < -pid_att_lim) { - nick_control = -pid_att_lim; - nick_controller.saturated = 1; - } - - - if (roll_control > pid_att_lim) { - roll_control = pid_att_lim; - roll_controller.saturated = 1; - } - - if (roll_control < -pid_att_lim) { - roll_control = -pid_att_lim; - roll_controller.saturated = 1; - } - - /* Emit controller values */ - ar_control->setpoint_thrust_cast = motor_thrust; - ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x; - ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y; - ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z; - ar_control->attitude_control_output[0] = roll_control; - ar_control->attitude_control_output[1] = nick_control; - ar_control->attitude_control_output[2] = yaw_rate_control; - ar_control->zcompensation = zcompensation; - orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control); - - float output_band = 0.f; - float band_factor = 0.75f; - const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ - float yaw_factor = 1.0f; - - if (motor_thrust <= min_thrust) { - motor_thrust = min_thrust; - output_band = 0.f; - - } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { - output_band = band_factor * (motor_thrust - min_thrust); - - } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * startpoint_full_control; - - } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * (max_thrust - motor_thrust); - } - - //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer - - // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control); - - // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control); - - // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control); - - // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control); - - // if we are not in the output band - if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band - && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band - && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band - && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { - - yaw_factor = 0.5f; - // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control * yaw_factor); - - // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control * yaw_factor); - - // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control * yaw_factor); - - // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control * yaw_factor); - } - - for (int i = 0; i < 4; i++) { - //check for limits - if (motor_calc[i] < motor_thrust - output_band) { - motor_calc[i] = motor_thrust - output_band; - } - - if (motor_calc[i] > motor_thrust + output_band) { - motor_calc[i] = motor_thrust + output_band; - } - } - - /* set the motor values */ - - /* scale up from 0..1 to 10..512) */ - motor_pwm[0] = (uint16_t) motor_calc[0] * ((float)max_gas - min_gas) + min_gas; - motor_pwm[1] = (uint16_t) motor_calc[1] * ((float)max_gas - min_gas) + min_gas; - motor_pwm[2] = (uint16_t) motor_calc[2] * ((float)max_gas - min_gas) + min_gas; - motor_pwm[3] = (uint16_t) motor_calc[3] * ((float)max_gas - min_gas) + min_gas; - - /* Keep motors spinning while armed and prevent overflows */ - - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; - motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; - motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; - motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; - - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512; - motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512; - motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512; - motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512; - - /* send motors via UART */ - if (motor_skip_counter % 5 == 0) { - uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5}; - ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); - write(ardrone_write, motorSpeedBuf, 5); - } - - motor_skip_counter++; - -} diff --git a/apps/multirotor_position_control/attitude_control.h b/apps/multirotor_position_control/attitude_control.h deleted file mode 100644 index a93a511bae..0000000000 --- a/apps/multirotor_position_control/attitude_control.h +++ /dev/null @@ -1,52 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file attitude control for quadrotors */ - -#ifndef ATTITUDE_CONTROL_H_ -#define ATTITUDE_CONTROL_H_ - -#include -#include -#include -#include -#include - -void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att, - const struct vehicle_status_s *status, int ardrone_pub, - struct ardrone_control_s *ar_control); - -#endif /* ATTITUDE_CONTROL_H_ */ diff --git a/apps/multirotor_position_control/multirotor_position_control.c b/apps/multirotor_position_control/multirotor_position_control.c new file mode 100644 index 0000000000..52930fd831 --- /dev/null +++ b/apps/multirotor_position_control/multirotor_position_control.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Implementation of AR.Drone 1.0 / 2.0 control interface + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ardrone_control.h" +#include "attitude_control.h" +#include "rate_control.h" +#include "ardrone_motor_control.h" +#include "position_control.h" +#include +#include +#include +#include + +__EXPORT int multirotor_position_control_main(int argc, char *argv[]); + +static bool thread_should_exit; +static bool thread_running = false; +static int mpc_task; + +static int +mpc_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + printf("[multirotor pos ctrl] Control started, taking over position control\n"); + + /* structures */ + struct vehicle_status_s state; + struct vehicle_attitude_s att; + //struct vehicle_global_position_setpoint_s global_pos_sp; + struct vehicle_local_position_setpoint_s local_pos_sp; + struct vehicle_local_position_s local_pos; + struct manual_control_setpoint_s manual; + struct vehicle_attitude_setpoint_s att_sp; + + /* 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 manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + + /* publish attitude setpoint */ + int att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + while (1) { + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of local position */ + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + /* get a local copy of local position setpoint */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + + if (state.state_machine == SYSTEM_STATE_AUTO) { + position_control(&state, &manual, &att, &local_pos, &local_pos_sp, &att_sp); + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } else if (state.state_machine == SYSTEM_STATE_STABILIZE) { + /* set setpoint to current position */ + // XXX select pos reset channel on remote + /* reset setpoint to current position (position hold) */ + // if (1 == 2) { + // local_pos_sp.x = local_pos.x; + // local_pos_sp.y = local_pos.y; + // local_pos_sp.z = local_pos.z; + // local_pos_sp.yaw = att.yaw; + // } + } + + /* run at approximately 50 Hz */ + usleep(20000); + + counter++; + } + + /* close uarts */ + close(ardrone_write); + ar_multiplexing_deinit(gpios); + + printf("[ardrone_control] ending now...\r\n"); + fflush(stdout); + return 0; +} + diff --git a/apps/multirotor_position_control/pid.c b/apps/multirotor_position_control/pid.c deleted file mode 100644 index 5ce05670e3..0000000000 --- a/apps/multirotor_position_control/pid.c +++ /dev/null @@ -1,109 +0,0 @@ -#include "pid.h" - -#include //TODO: move matrix.h to somewhere else? - -void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - uint8_t mode, uint8_t plot_i) -{ - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; - pid->mode = mode; - pid->plot_i = plot_i; - pid->count = 0; - pid->saturated = 0; - - pid->sp = 0; - pid->error_previous = 0; - pid->integral = 0; -} -void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) -{ - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; - // pid->mode = mode; - - // pid->sp = 0; - // pid->error_previous = 0; - // pid->integral = 0; -} - -//void pid_set(PID_t *pid, float sp) -//{ -// pid->sp = sp; -// pid->error_previous = 0; -// pid->integral = 0; -//} - -/** - * - * @param pid - * @param val - * @param dt - * @return - */ -float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) -{ - /* error = setpoint - actual_position - integral = integral + (error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + (Ki*integral) + (Kd*derivative) - previous_error = error - wait(dt) - goto start - */ - - float i, d; - pid->sp = sp; - float error = pid->sp - val; - - if (pid->saturated && (pid->integral * error > 0)) { - //Output is saturated and the integral would get bigger (positive or negative) - i = pid->integral; - - //Reset saturation. If we are still saturated this will be set again at output limit check. - pid->saturated = 0; - - } else { - i = pid->integral + (error * dt); - } - - // Anti-Windup. Needed if we don't use the saturation above. - if (pid->intmax != 0.0) { - if (i > pid->intmax) { - pid->integral = pid->intmax; - - } else if (i < -pid->intmax) { - - pid->integral = -pid->intmax; - - } else { - pid->integral = i; - } - - //Send Controller integrals - // Disabled because of new possibilities with debug_vect. - // Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens - // if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1)) - // { - // mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral); - // } - } - - if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / dt; - - } else if (pid->mode == PID_MODE_DERIVATIV_SET) { - d = -val_dot; - - } else { - d = 0; - } - - pid->error_previous = error; - - return (error * pid->kp) + (i * pid->ki) + (d * pid->kd); -} diff --git a/apps/multirotor_position_control/pid.h b/apps/multirotor_position_control/pid.h deleted file mode 100644 index a721c839c6..0000000000 --- a/apps/multirotor_position_control/pid.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * pid.h - * - * Created on: May 29, 2012 - * Author: thomasgubler - */ - -#ifndef PID_H_ -#define PID_H_ - -#include - -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC 0 -/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 1 - -typedef struct { - float kp; - float ki; - float kd; - float intmax; - float sp; - float integral; - float error_previous; - uint8_t mode; - uint8_t plot_i; - uint8_t count; - uint8_t saturated; -} PID_t; - -void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i); -void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); -//void pid_set(PID_t *pid, float sp); -float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); - - - -#endif /* PID_H_ */ diff --git a/apps/multirotor_position_control/position_control.c b/apps/multirotor_position_control/position_control.c index 6ba1654595..fd731a33de 100644 --- a/apps/multirotor_position_control/position_control.c +++ b/apps/multirotor_position_control/position_control.c @@ -1,8 +1,10 @@ /**************************************************************************** - * ardrone_control/position_control.c * - * Copyright (C) 2008, 2012 Thomas Gubler, Julian Oes, Lorenz Meier. All rights reserved. - * Author: Based on the pixhawk quadrotor controller + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -14,7 +16,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -33,7 +35,11 @@ * ****************************************************************************/ -#include "position_control.h" +/** + * @file multirotor_position_control.c + * Implementation of the position control for a multirotor VTOL + */ + #include #include #include @@ -41,41 +47,11 @@ #include #include #include -#include "pid.h" +#include -#ifndef FM_PI -#define FM_PI 3.1415926535897932384626433832795f -#endif +#include "multirotor_position_control.h" - - -#define CONTROL_PID_POSITION_INTERVAL 0.020 - -int read_lock_position(global_data_position_t *position) -{ - static int ret; - ret = global_data_wait(&global_data_position->access_conf); - - if (ret == 0) { - memcpy(&position->lat, &global_data_position->lat, sizeof(global_data_position->lat)); - memcpy(&position->lon, &global_data_position->lon, sizeof(global_data_position->lon)); - memcpy(&position->alt, &global_data_position->alt, sizeof(global_data_position->alt)); - memcpy(&position->relative_alt, &global_data_position->relative_alt, sizeof(global_data_position->relative_alt)); - memcpy(&position->vx, &global_data_position->vx, sizeof(global_data_position->vx)); - memcpy(&position->vy, &global_data_position->vy, sizeof(global_data_position->vy)); - memcpy(&position->vz, &global_data_position->vz, sizeof(global_data_position->vz)); - memcpy(&position->hdg, &global_data_position->hdg, sizeof(global_data_position->hdg)); - - - } else { - printf("Controller timeout, no new position values available\n"); - } - - global_data_unlock(&global_data_position->access_conf); - return ret; -} - -float get_distance_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next) +float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now / 180.0 * M_PI; double lon_now_rad = lon_now / 180.0 * M_PI; @@ -94,7 +70,7 @@ float get_distance_to_next_waypoint(float lat_now, float lon_now, float lat_next return radius_earth * c; } -float get_bearing_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next) +float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now / 180.0 * M_PI; double lon_now_rad = lon_now / 180.0 * M_PI; @@ -104,17 +80,20 @@ float get_bearing_to_next_waypoint(float lat_now, float lon_now, float lat_next, double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - + /* conscious mix of double and float trig function to maximize speed and efficiency */ float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - if (theta < 0) { - theta = theta + 2 * FM_PI; + // XXX wrapping check is incomplete + if (theta < 0.0f) { + theta = theta + 2.0f * M_PI_F; } return theta; } -void control_position(void) +void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, + const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, + const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) { static PID_t distance_controller; @@ -136,8 +115,6 @@ void control_position(void) if (initialized == false) { - global_data_lock(&global_data_parameter_storage->access_conf); - pid_init(&distance_controller, global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], @@ -148,10 +125,6 @@ void control_position(void) // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - pm_counter = global_data_parameter_storage->counter; - - global_data_unlock(&global_data_parameter_storage->access_conf); - thrust_total = 0.0f; /* Position initialization */ @@ -295,14 +268,8 @@ void control_position(void) roll_desired = asinf(roll_fraction); } - /*Broadcast desired angles */ - global_data_lock(&global_data_ardrone_control->access_conf); - global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[0] = roll_desired; - global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[1] = pitch_desired; - global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[2] = bearing; //TODO: add yaw setpoint - global_data_unlock(&global_data_ardrone_control->access_conf); - global_data_broadcast(&global_data_ardrone_control->access_conf); - + att_sp.roll = roll_desired; + att_sp.pitch = pitch_desired; counter++; } diff --git a/apps/multirotor_position_control/position_control.h b/apps/multirotor_position_control/position_control.h index dbc0650e66..5ba59362e3 100644 --- a/apps/multirotor_position_control/position_control.h +++ b/apps/multirotor_position_control/position_control.h @@ -1,13 +1,50 @@ -/* - * position_control.h +/**************************************************************************** * - * Created on: May 29, 2012 - * Author: thomasgubler + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file multirotor_position_control.h + * Definition of the position control for a multirotor VTOL */ #ifndef POSITION_CONTROL_H_ #define POSITION_CONTROL_H_ -void control_position(void); +void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, + const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, + const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); #endif /* POSITION_CONTROL_H_ */ diff --git a/apps/multirotor_position_control/rate_control.c b/apps/multirotor_position_control/rate_control.c deleted file mode 100644 index 4abba62556..0000000000 --- a/apps/multirotor_position_control/rate_control.c +++ /dev/null @@ -1,320 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file Implementation of attitude rate control - */ - -#include "rate_control.h" -#include "ardrone_control_helper.h" -#include "ardrone_motor_control.h" -#include - -extern int ardrone_write; -extern int gpios; - -typedef struct { - uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration - uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration - uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration - uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration - uint8_t target_system; ///< System ID of the system that should set these motor commands -} quad_motors_setpoint_t; - - -void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints) -{ - static quad_motors_setpoint_t actuators_desired; - //static quad_motors_setpoint_t quad_motors_setpoint_desired; - - static int16_t outputBand = 0; - -// static uint16_t control_counter; - static hrt_abstime now_time; - static hrt_abstime last_time; - - static float setpointXrate; - static float setpointYrate; - static float setpointZrate; - - static float setpointRateCast[3]; - static float Kp; -// static float Ki; - static float setpointThrustCast; - static float startpointFullControll; - static float maxThrustSetpoints; - - static float gyro_filtered[3]; - static float gyro_filtered_offset[3]; - static float gyro_alpha; - static float gyro_alpha_offset; -// static float errXrate; - static float attRatesScaled[3]; - - static uint16_t offsetCnt; -// static float antiwindup; - static int motor_skip_counter; - - static int read_ret; - - static bool initialized; - - if (initialized == false) { - initialized = true; - - /* Read sensors for initial values */ - - gyro_filtered_offset[0] = 0.00026631611f * (float)raw->gyro_raw[0]; - gyro_filtered_offset[1] = 0.00026631611f * (float)raw->gyro_raw[1]; - gyro_filtered_offset[2] = 0.00026631611f * (float)raw->gyro_raw[2]; - - gyro_filtered[0] = 0.00026631611f * (float)raw->gyro_raw[0]; - gyro_filtered[1] = 0.00026631611f * (float)raw->gyro_raw[1]; - gyro_filtered[2] = 0.00026631611f * (float)raw->gyro_raw[2]; - - outputBand = 0; - startpointFullControll = 150.0f; - maxThrustSetpoints = 511.0f; - //Kp=60; - //Kp=40.0f; - //Kp=45; - Kp = 30.0f; -// Ki=0.0f; -// antiwindup=50.0f; - } - - /* Get setpoint */ - - - //Rate Controller - setpointRateCast[0] = -((float)setpoints->motor_right_ne - 9999.0f) * 0.01f / 180.0f * 3.141f; - setpointRateCast[1] = -((float)setpoints->motor_front_nw - 9999.0f) * 0.01f / 180.0f * 3.141f; - setpointRateCast[2] = 0; //-((float)setpoints->motor_back_se-9999.0f)*0.01f; - //Ki=actuatorDesired.motorRight_NE*0.001f; - setpointThrustCast = setpoints->motor_left_sw; - - attRatesScaled[0] = 0.000317603994f * (float)raw->gyro_raw[0]; - attRatesScaled[1] = 0.000317603994f * (float)raw->gyro_raw[1]; - attRatesScaled[2] = 0.000317603994f * (float)raw->gyro_raw[2]; - - //filtering of the gyroscope values - - //compute filter coefficient alpha - - //gyro_alpha=0.005/(2.0f*3.1415f*200.0f+0.005f); - //gyro_alpha=0.009; - gyro_alpha = 0.09f; - gyro_alpha_offset = 0.001f; - //gyro_alpha=0.001; - //offset estimation and filtering - offsetCnt++; - uint8_t i; - - for (i = 0; i < 3; i++) { - if (offsetCnt < 5000) { - gyro_filtered_offset[i] = attRatesScaled[i] * gyro_alpha_offset + gyro_filtered_offset[i] * (1 - gyro_alpha_offset); - } - - gyro_filtered[i] = 1.0f * ((attRatesScaled[i] - gyro_filtered_offset[i]) * gyro_alpha + gyro_filtered[i] * (1 - gyro_alpha)) - 0 * setpointRateCast[i]; - } - - // //START DEBUG - // /* write filtered values to global_data_attitude */ - // global_data_attitude->rollspeed = gyro_filtered[0]; - // global_data_attitude->pitchspeed = gyro_filtered[1]; - // global_data_attitude->yawspeed = gyro_filtered[2]; - // //END DEBUG - - //rate controller - - //X-axis - setpointXrate = -Kp * (setpointRateCast[0] - gyro_filtered[0]); - //Y-axis - setpointYrate = -Kp * (setpointRateCast[1] - gyro_filtered[1]); - //Z-axis - setpointZrate = -Kp * (setpointRateCast[2] - gyro_filtered[2]); - - - - - //Mixing - if (setpointThrustCast <= 0) { - setpointThrustCast = 0; - outputBand = 0; - } - - if ((setpointThrustCast < startpointFullControll) && (setpointThrustCast > 0)) { - outputBand = 0.75f * setpointThrustCast; - } - - if ((setpointThrustCast >= startpointFullControll) && (setpointThrustCast < maxThrustSetpoints - 0.75f * startpointFullControll)) { - outputBand = 0.75f * startpointFullControll; - } - - if (setpointThrustCast >= maxThrustSetpoints - 0.75f * startpointFullControll) { - setpointThrustCast = 0.75f * startpointFullControll; - outputBand = 0.75f * startpointFullControll; - } - - actuators_desired.motor_front_nw = setpointThrustCast + (setpointXrate + setpointYrate + setpointZrate); - actuators_desired.motor_right_ne = setpointThrustCast + (-setpointXrate + setpointYrate - setpointZrate); - actuators_desired.motor_back_se = setpointThrustCast + (-setpointXrate - setpointYrate + setpointZrate); - actuators_desired.motor_left_sw = setpointThrustCast + (setpointXrate - setpointYrate - setpointZrate); - - - if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) > (setpointThrustCast + outputBand)) { - actuators_desired.motor_front_nw = setpointThrustCast + outputBand; - } - - if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) < (setpointThrustCast - outputBand)) { - actuators_desired.motor_front_nw = setpointThrustCast - outputBand; - } - - if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) > (setpointThrustCast + outputBand)) { - actuators_desired.motor_right_ne = setpointThrustCast + outputBand; - } - - if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) < (setpointThrustCast - outputBand)) { - actuators_desired.motor_right_ne = setpointThrustCast - outputBand; - } - - if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) > (setpointThrustCast + outputBand)) { - actuators_desired.motor_back_se = setpointThrustCast + outputBand; - } - - if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) < (setpointThrustCast - outputBand)) { - actuators_desired.motor_back_se = setpointThrustCast - outputBand; - } - - if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) > (setpointThrustCast + outputBand)) { - actuators_desired.motor_left_sw = setpointThrustCast + outputBand; - } - - if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) < (setpointThrustCast - outputBand)) { - actuators_desired.motor_left_sw = setpointThrustCast - outputBand; - } - - //printf("%lu,%lu,%lu,%lu\n",actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw); - - if (motor_skip_counter % 5 == 0) { - uint8_t motorSpeedBuf[5]; - ar_get_motor_packet(motorSpeedBuf, actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw); -// uint8_t* motorSpeedBuf = ar_get_motor_packet(1, 1, 1, 1); -// if(motor_skip_counter %50 == 0) -// { -// if(0==actuators_desired.motor_front_nw || 0 == actuators_desired.motor_right_ne || 0 == actuators_desired.motor_back_se || 0 == actuators_desired.motor_left_sw) -// printf("Motors set: %u, %u, %u, %u\n", actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw); -// printf("input: %u\n", setpoints->motor_front_nw); -// printf("Roll casted desired: %f, Pitch casted desired: %f, Yaw casted desired: %f\n", setpointRateCast[0], setpointRateCast[1], setpointRateCast[2]); -// } - write(ardrone_write, motorSpeedBuf, 5); -// motor_skip_counter = 0; - } - - motor_skip_counter++; - - //START DEBUG -// global_data_lock(&global_data_ardrone_control->access_conf); -// global_data_ardrone_control->timestamp = hrt_absolute_time(); -// global_data_ardrone_control->gyro_scaled[0] = attRatesScaled[0]; -// global_data_ardrone_control->gyro_scaled[1] = attRatesScaled[1]; -// global_data_ardrone_control->gyro_scaled[2] = attRatesScaled[2]; -// global_data_ardrone_control->gyro_filtered[0] = gyro_filtered[0]; -// global_data_ardrone_control->gyro_filtered[1] = gyro_filtered[1]; -// global_data_ardrone_control->gyro_filtered[2] = gyro_filtered[2]; -// global_data_ardrone_control->gyro_filtered_offset[0] = gyro_filtered_offset[0]; -// global_data_ardrone_control->gyro_filtered_offset[1] = gyro_filtered_offset[1]; -// global_data_ardrone_control->gyro_filtered_offset[2] = gyro_filtered_offset[2]; -// global_data_ardrone_control->setpoint_rate_cast[0] = setpointRateCast[0]; -// global_data_ardrone_control->setpoint_rate_cast[1] = setpointRateCast[1]; -// global_data_ardrone_control->setpoint_rate_cast[2] = setpointRateCast[2]; -// global_data_ardrone_control->setpoint_thrust_cast = setpointThrustCast; -// global_data_ardrone_control->setpoint_rate[0] = setpointXrate; -// global_data_ardrone_control->setpoint_rate[1] = setpointYrate; -// global_data_ardrone_control->setpoint_rate[2] = setpointZrate; -// global_data_ardrone_control->motor_front_nw = actuators_desired.motor_front_nw; -// global_data_ardrone_control->motor_right_ne = actuators_desired.motor_right_ne; -// global_data_ardrone_control->motor_back_se = actuators_desired.motor_back_se; -// global_data_ardrone_control->motor_left_sw = actuators_desired.motor_left_sw; -// global_data_unlock(&global_data_ardrone_control->access_conf); -// global_data_broadcast(&global_data_ardrone_control->access_conf); - //END DEBUG - - - -// gettimeofday(&tv, NULL); -// now = ((uint32_t)tv.tv_sec) * 1000 + tv.tv_usec/1000; -// time_elapsed = now - last_run; -// if (time_elapsed*1000 > CONTROL_LOOP_USLEEP) -// { -// sleep_time = (int32_t)CONTROL_LOOP_USLEEP - ((int32_t)time_elapsed*1000 - (int32_t)CONTROL_LOOP_USLEEP); -// -// if(motor_skip_counter %500 == 0) -// { -// printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run); -// } -// } -// -// if (sleep_time <= 0) -// { -// printf("WARNING: CPU Overload!\n"); -// printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run); -// usleep(CONTROL_LOOP_USLEEP); -// } -// else -// { -// usleep(sleep_time); -// } -// last_run = now; -// -// now_time = hrt_absolute_time(); -// if(control_counter % 500 == 0) -// { -// printf("Now: %lu\n",(unsigned long)now_time); -// printf("Last: %lu\n",(unsigned long)last_time); -// printf("Difference: %lu\n", (unsigned long)(now_time - last_time)); -// printf("now seconds: %lu\n", (unsigned long)(now_time / 1000000)); -// } -// last_time = now_time; -// -// now_time = hrt_absolute_time() / 1000000; -// if(now_time - last_time > 0) -// { -// printf("Counter: %ld\n",control_counter); -// last_time = now_time; -// control_counter = 0; -// } -// control_counter++; -} diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 564ee4f8c4..77b7b9c47d 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -966,7 +966,7 @@ int sensors_main(int argc, char *argv[]) if (!thread_running) { printf("sensors app not started\n"); } else { - printf("stopping sensors app"); + printf("stopping sensors app\n"); thread_should_exit = true; } exit(0); diff --git a/apps/multirotor_control/pid.c b/apps/systemlib/pid/pid.c similarity index 53% rename from apps/multirotor_control/pid.c rename to apps/systemlib/pid/pid.c index 5ce05670e3..a24657a411 100644 --- a/apps/multirotor_control/pid.c +++ b/apps/systemlib/pid/pid.c @@ -1,6 +1,45 @@ -#include "pid.h" +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ -#include //TODO: move matrix.h to somewhere else? +/** + * @file pid.c + * Implementation of generic PID control interface + */ + +#include "pid.h" void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i) diff --git a/apps/multirotor_position_control/rate_control.h b/apps/systemlib/pid/pid.h similarity index 61% rename from apps/multirotor_position_control/rate_control.h rename to apps/systemlib/pid/pid.h index a69745d9b4..d62843ed4a 100644 --- a/apps/multirotor_position_control/rate_control.h +++ b/apps/systemlib/pid/pid.h @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier + * Author: @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,16 +34,41 @@ * ****************************************************************************/ -/* - * @file Definition of attitude rate control +/** + * @file pid.h + * Definition of generic PID control interface */ -#ifndef RATE_CONTROL_H_ -#define RATE_CONTROL_H_ -#include -#include -#include +#ifndef PID_H_ +#define PID_H_ -void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints); +#include -#endif /* RATE_CONTROL_H_ */ +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error + * val_dot in pid_calculate() will be ignored */ +#define PID_MODE_DERIVATIV_CALC 0 +/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ +#define PID_MODE_DERIVATIV_SET 1 + +typedef struct { + float kp; + float ki; + float kd; + float intmax; + float sp; + float integral; + float error_previous; + uint8_t mode; + uint8_t plot_i; + uint8_t count; + uint8_t saturated; +} PID_t; + +void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i); +void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); +//void pid_set(PID_t *pid, float sp); +float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); + + + +#endif /* PID_H_ */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 4f0c5093f5..01b67cfef4 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -56,6 +56,10 @@ CONFIGURED_APPS += systemcmds/mixer # https://pixhawk.ethz.ch/px4/dev/hello_sky # CONFIGURED_APPS += examples/px4_simple_app +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/deamon +CONFIGURED_APPS += examples/px4_deamon_app + CONFIGURED_APPS += uORB ifeq ($(CONFIG_MAVLINK),y) From dae0b922f166b1c6af79ecce85b3eb00dde22654 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Aug 2012 15:52:59 +0200 Subject: [PATCH 2/4] Added deamon example, reworked / merged multirotor attitude control. Ready for AR.Drone interface changes and integration tests --- apps/ardrone_control/Makefile | 2 +- apps/ardrone_control/ardrone_control.c | 8 +- apps/ardrone_control/attitude_control.c | 20 +- apps/ardrone_control/attitude_control.h | 6 +- apps/ardrone_control/rate_control.c | 1 - apps/examples/px4_deamon_app/px4_deamon_app.c | 8 +- apps/mavlink/mavlink.c | 59 ++- .../Makefile | 5 +- .../multirotor_att_control_main.c} | 109 +++-- .../multirotor_attitude_control.c | 223 +++++++++ .../multirotor_attitude_control.h | 28 +- .../multirotor_attitude_control.c | 456 ------------------ apps/multirotor_control/multirotor_control.h | 12 - .../Makefile | 2 +- .../multirotor_pos_control.c} | 6 +- .../position_control.c | 0 .../position_control.h | 0 apps/sensors/sensors.c | 2 +- apps/systemlib/Makefile | 3 +- apps/systemlib/pid/pid.c | 6 +- apps/systemlib/pid/pid.h | 6 +- nuttx/configs/px4fmu/nsh/appconfig | 2 +- 22 files changed, 399 insertions(+), 565 deletions(-) rename apps/{multirotor_control => multirotor_att_control}/Makefile (91%) rename apps/{multirotor_control/multirotor_control.c => multirotor_att_control/multirotor_att_control_main.c} (60%) create mode 100644 apps/multirotor_att_control/multirotor_attitude_control.c rename apps/{multirotor_control => multirotor_att_control}/multirotor_attitude_control.h (67%) delete mode 100644 apps/multirotor_control/multirotor_attitude_control.c delete mode 100644 apps/multirotor_control/multirotor_control.h rename apps/{multirotor_position_control => multirotor_pos_control}/Makefile (97%) rename apps/{multirotor_position_control/multirotor_position_control.c => multirotor_pos_control/multirotor_pos_control.c} (95%) rename apps/{multirotor_position_control => multirotor_pos_control}/position_control.c (100%) rename apps/{multirotor_position_control => multirotor_pos_control}/position_control.h (100%) diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile index a6d471567b..35fbe01d73 100644 --- a/apps/ardrone_control/Makefile +++ b/apps/ardrone_control/Makefile @@ -40,6 +40,6 @@ PRIORITY = SCHED_PRIORITY_MAX - 15 STACKSIZE = 4096 # explicit list of sources - not everything is built currently -CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c +CSRCS = ardrone_control.c ardrone_motor_control.c rate_control.c attitude_control.c pid.c include $(APPDIR)/mk/app.mk diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c index bc5598f9c5..72e8ae14ee 100644 --- a/apps/ardrone_control/ardrone_control.c +++ b/apps/ardrone_control/ardrone_control.c @@ -62,6 +62,7 @@ #include #include #include +#include __EXPORT int ardrone_control_main(int argc, char *argv[]); @@ -181,6 +182,8 @@ int ardrone_control_main(int argc, char *argv[]) memset(&raw, 0, sizeof(raw)); struct ardrone_motors_setpoint_s setpoint; memset(&setpoint, 0, sizeof(setpoint)); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -251,9 +254,8 @@ int ardrone_control_main(int argc, char *argv[]) float roll_control, pitch_control, yaw_control, thrust_control; - multirotor_control_attitude(&att_sp, &att, &state, motor_test_mode, - &roll_control, &pitch_control, &yaw_control, &thrust_control); - ardrone_mixing_and_output(ardrone_write, roll_control, pitch_control, yaw_control, thrust_control, motor_test_mode); + multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode); + ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode); } else { /* invalid mode, complain */ diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c index d31e36e332..77e08a535c 100644 --- a/apps/ardrone_control/attitude_control.c +++ b/apps/ardrone_control/attitude_control.c @@ -56,8 +56,9 @@ #define MAX_MOTOR_COUNT 16 -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, - bool verbose, float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output) +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, + struct actuator_controls_s *actuators, bool verbose) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -218,13 +219,18 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s roll_controller.saturated = 1; } - *roll_output = roll_control; - *pitch_output = pitch_control; - *yaw_output = yaw_rate_control; - *thrust_output = motor_thrust; + actuators->control[0] = roll_control; + actuators->control[1] = pitch_control; + actuators->control[2] = yaw_rate_control; + actuators->control[3] = motor_thrust; } -void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose) { +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose) { + + float roll_control = actuators->control[0]; + float pitch_control = actuators->control[1]; + float yaw_control = actuators->control[2]; + float motor_thrust = actuators->control[3]; unsigned int motor_skip_counter = 0; diff --git a/apps/ardrone_control/attitude_control.h b/apps/ardrone_control/attitude_control.h index 56f52dabec..e14d299662 100644 --- a/apps/ardrone_control/attitude_control.h +++ b/apps/ardrone_control/attitude_control.h @@ -51,11 +51,11 @@ #include #include #include +#include void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - const struct vehicle_status_s *status, bool verbose, - float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output); + const struct vehicle_status_s *status, struct actuator_controls_s *actuators, bool verbose); -void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose); +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose); #endif /* ATTITUDE_CONTROL_H_ */ diff --git a/apps/ardrone_control/rate_control.c b/apps/ardrone_control/rate_control.c index df20cd1ab8..6c6b004e45 100644 --- a/apps/ardrone_control/rate_control.c +++ b/apps/ardrone_control/rate_control.c @@ -39,7 +39,6 @@ */ #include "rate_control.h" -#include "ardrone_control_helper.h" #include "ardrone_motor_control.h" #include diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c index e17bf478af..91e60a8f90 100644 --- a/apps/examples/px4_deamon_app/px4_deamon_app.c +++ b/apps/examples/px4_deamon_app/px4_deamon_app.c @@ -53,20 +53,20 @@ __EXPORT int px4_deamon_app_main(int argc, char *argv[]); /** * Mainloop of deamon. */ -int mavlink_thread_main(int argc, char *argv[]); +int px4_deamon_thread_main(int argc, char *argv[]); /** * Print the correct usage. */ static void usage(const char *reason); -int mavlink_thread_main(int argc, char *argv[]) { +int px4_deamon_thread_main(int argc, char *argv[]) { printf("[deamon] starting\n"); while (true) { printf("Hello Deamon!\n"); - sleep(1); + sleep(10); if (thread_should_exit) break; } @@ -106,7 +106,7 @@ int px4_deamon_app_main(int argc, char *argv[]) } thread_should_exit = false; - mavlink_task = task_create("deamon", SCHED_PRIORITY_DEFAULT, 4096, deamon_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_create("deamon", SCHED_PRIORITY_DEFAULT, 4096, px4_deamon_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; exit(0); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index b6fdc1a3e9..47eab6be87 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -130,6 +130,7 @@ static int pub_hil_global_pos = -1; static int ardrone_motors_pub = -1; static int cmd_pub = -1; static int sensor_sub = -1; +static int att_sub = -1; static int global_pos_sub = -1; static int local_pos_sub = -1; static int flow_pub = -1; @@ -412,31 +413,40 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t */ static void *receiveloop(void *arg) { + int uart_fd = *((int*)arg); + + const int timeout = 1000; uint8_t ch; mavlink_message_t msg; prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); - while (1) { + while (!thread_should_exit) { if (mavlink_exit_requested) break; - /* blocking read on next byte */ - int nread = read(uart, &ch, 1); + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + if (poll(fds, 1, timeout) > 0) { + /* non-blocking read until buffer is empty */ + int nread = 0; - if (nread > 0 && mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char - /* handle generic messages and commands */ - handleMessage(&msg); + do { + nread = read(uart_fd, &ch, 1); - /* Handle packet with waypoint component */ - mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); + if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* handle generic messages and commands */ + handleMessage(&msg); - /* Handle packet with parameter component */ - mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + /* Handle packet with waypoint component */ + mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); + + /* Handle packet with parameter component */ + mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + } + } while (nread > 0); } - } return NULL; @@ -455,6 +465,10 @@ static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) /* senser sub triggers RAW IMU */ orb_set_interval(sensor_sub, min_interval); break; + case MAVLINK_MSG_ID_ATTITUDE: + /* attitude sub triggers attitude */ + orb_set_interval(att_sub, 100); + break; default: /* not found */ ret = ERROR; @@ -499,15 +513,13 @@ static void *uorb_receiveloop(void *arg) /* --- SENSORS RAW VALUE --- */ /* subscribe to ORB for sensors raw */ sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - orb_set_interval(sensor_sub, 100); /* 10Hz updates */ fds[fdsc_count].fd = sensor_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE VALUE --- */ /* subscribe to ORB for attitude */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(att_sub, 100); /* 10Hz updates */ + att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); fds[fdsc_count].fd = att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -611,9 +623,9 @@ static void *uorb_receiveloop(void *arg) * set up poll to block for new data, * wait for a maximum of 1000 ms (1 second) */ - const int timeout = 5000; + const int timeout = 1000; - while (1) { + while (!thread_should_exit) { if (mavlink_exit_requested) break; int poll_ret = poll(fds, fdsc_count, timeout); @@ -1271,7 +1283,7 @@ int mavlink_thread_main(int argc, char *argv[]) if (uart < 0) { printf("[mavlink] FAILED to open %s, terminating.\n", uart_name); - return -1; + return ERROR; } /* Flush UART */ @@ -1289,7 +1301,7 @@ int mavlink_thread_main(int argc, char *argv[]) pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); pthread_attr_setstacksize(&receiveloop_attr, 2048); - pthread_create(&receive_thread, &receiveloop_attr, receiveloop, NULL); + pthread_create(&receive_thread, &receiveloop_attr, receiveloop, &uart); pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); @@ -1305,23 +1317,28 @@ int mavlink_thread_main(int argc, char *argv[]) /* all subscriptions are now active, set up initial guess about rate limits */ if (baudrate >= 921600) { - /* 1000 Hz / 1 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1); + /* set no limit */ + /* 500 Hz / 2 ms */ + //set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 2); } else if (baudrate >= 460800) { /* 250 Hz / 4 ms */ set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 4); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 4); } else if (baudrate >= 115200) { /* 50 Hz / 20 ms */ set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 20); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 20); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 100); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); } - while (1) { + while (!thread_should_exit) { if (mavlink_exit_requested) break; diff --git a/apps/multirotor_control/Makefile b/apps/multirotor_att_control/Makefile similarity index 91% rename from apps/multirotor_control/Makefile rename to apps/multirotor_att_control/Makefile index 7ae1d472a3..03cf33e434 100644 --- a/apps/multirotor_control/Makefile +++ b/apps/multirotor_att_control/Makefile @@ -35,11 +35,8 @@ # Makefile to build uORB # -APPNAME = multirotor_control +APPNAME = multirotor_att_control PRIORITY = SCHED_PRIORITY_MAX - 15 STACKSIZE = 2048 -# explicit list of sources - not everything is built currently -CSRCS = multirotor_control.c multirotor_attitude_control.c pid.c - include $(APPDIR)/mk/app.mk diff --git a/apps/multirotor_control/multirotor_control.c b/apps/multirotor_att_control/multirotor_att_control_main.c similarity index 60% rename from apps/multirotor_control/multirotor_control.c rename to apps/multirotor_att_control/multirotor_att_control_main.c index 2edc1ba84f..baee507b2f 100644 --- a/apps/multirotor_control/multirotor_control.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Author: @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,9 +33,9 @@ ****************************************************************************/ /* - * @file multirotor_control.c + * @file multirotor_att_control_main.c * - * Implementation of multirotor controllers + * Implementation of multirotor attitude control main loop. */ #include @@ -49,20 +49,25 @@ #include #include #include +#include #include #include #include -#include "multirotor_control.h" -#include "multirotor_attitude_control.h" #include #include #include +#include +#include +#include +#include +#include #include -#include #include -__EXPORT int multirotor_control_main(int argc, char *argv[]); +#include "multirotor_attitude_control.h" + +__EXPORT int multirotor_att_control_main(int argc, char *argv[]); static enum { @@ -77,17 +82,35 @@ static int mc_task; static int mc_thread_main(int argc, char *argv[]) { + bool motor_test_mode = false; + /* structures */ + /* declare and safely initialize all structs */ struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); struct vehicle_attitude_s att; - struct rc_channels_s rc; + memset(&att, 0, sizeof(att)); + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + struct ardrone_motors_setpoint_s setpoint; + memset(&setpoint, 0, sizeof(setpoint)); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); + 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 att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + // int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); /* rate-limit the attitude subscription to 200Hz to pace our loop */ orb_set_interval(att_sub, 5); @@ -101,29 +124,55 @@ mc_thread_main(int argc, char *argv[]) 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"); + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); /* welcome user */ - printf("[multirotor_control] starting\n"); + printf("[multirotor_att_control] starting\n"); while (!thread_should_exit) { - /* wait for a sensor update */ - poll(&fds, 1, -1); + /* wait for a sensor update, check for exit condition every 500 ms */ + poll(&fds, 1, 500); perf_begin(mc_loop_perf); - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - - /* get a local copy of rc inputs */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of attitude setpoint */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - /* run the attitude controller */ - multirotor_control_attitude(&rc, &att, &state, &actuators); + att_sp.roll_body = -manual.roll * M_PI_F / 8.0f; + att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f; + att_sp.yaw_body = -manual.yaw * M_PI_F; + if (motor_test_mode) { + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.3f; + } else { + if (state.state_machine == SYSTEM_STATE_MANUAL || + state.state_machine == SYSTEM_STATE_GROUND_READY || + state.state_machine == SYSTEM_STATE_STABILIZED || + state.state_machine == SYSTEM_STATE_AUTO || + state.state_machine == SYSTEM_STATE_MISSION_ABORT || + state.state_machine == SYSTEM_STATE_EMCY_LANDING) { + att_sp.thrust = manual.throttle; + + } else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) { + /* immediately cut off motors */ + att_sp.thrust = 0.0f; + + } else { + /* limit motor throttle to zero for an unknown mode */ + att_sp.thrust = 0.0f; + } + + } + + multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode); + //ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode); /* publish the result */ orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -131,7 +180,7 @@ mc_thread_main(int argc, char *argv[]) perf_end(mc_loop_perf); } - printf("[multirotor_control] stopping\r\n"); + printf("[multirotor att control] stopping.\n"); /* kill all outputs */ armed.armed = false; @@ -143,7 +192,7 @@ mc_thread_main(int argc, char *argv[]) close(att_sub); close(state_sub); - close(rc_sub); + close(manual_sub); close(actuator_pub); close(armed_pub); @@ -159,16 +208,17 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: multirotor_control [-m ] {start|stop}\n"); + fprintf(stderr, "usage: multirotor_att_control [-m ] {start|stop}\n"); fprintf(stderr, " is 'rates' or 'attitude'\n"); exit(1); } -int multirotor_control_main(int argc, char *argv[]) +int multirotor_att_control_main(int argc, char *argv[]) { int ch; control_mode = CONTROL_MODE_RATES; + unsigned int optioncount = 0; while ((ch = getopt(argc, argv, "m:")) != EOF) { switch (ch) { @@ -180,12 +230,13 @@ int multirotor_control_main(int argc, char *argv[]) } else { usage("unrecognized -m value"); } + optioncount += 2; default: usage("unrecognized option"); } } - argc -= optind; - argv += optind; + argc -= optioncount; + argv += optioncount; if (argc < 1) usage("missing command"); @@ -193,7 +244,7 @@ int multirotor_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { thread_should_exit = false; - mc_task = task_create("multirotor_attitude", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL); + mc_task = task_create("multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL); exit(0); } @@ -202,6 +253,6 @@ int multirotor_control_main(int argc, char *argv[]) exit(0); } - usage("unrecognised command"); + usage("unrecognized command"); exit(1); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c new file mode 100644 index 0000000000..367a0c3e15 --- /dev/null +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * @author Julian Oes + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file multirotor_attitude_control.c + * Implementation of attitude controller + */ + +#include "multirotor_attitude_control.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, + struct actuator_controls_s *actuators, bool verbose) +{ + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + static int motor_skip_counter = 0; + + static PID_t yaw_pos_controller; + static PID_t yaw_speed_controller; + static PID_t pitch_controller; + static PID_t roll_controller; + + static float pid_yawpos_lim; + static float pid_yawspeed_lim; + static float pid_att_lim; + + static bool initialized = false; + + /* initialize the pid controllers when the function is called for the first time */ + if (initialized == false) { + + pid_init(&yaw_pos_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU], + PID_MODE_DERIVATIV_CALC, 154); + + pid_init(&yaw_speed_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU], + PID_MODE_DERIVATIV_CALC, 155); + + pid_init(&pitch_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], + PID_MODE_DERIVATIV_SET, 156); + + pid_init(&roll_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], + PID_MODE_DERIVATIV_SET, 157); + + pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; + pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; + pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; + + initialized = true; + } + + /* load new parameters with lower rate */ + if (motor_skip_counter % 50 == 0) { + pid_set_parameters(&yaw_pos_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]); + + pid_set_parameters(&yaw_speed_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]); + + pid_set_parameters(&pitch_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); + + pid_set_parameters(&roll_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); + + pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; + pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; + pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; + } + + /*Calculate Controllers*/ + //control Nick + float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET], + att->pitch, att->pitchspeed, deltaT); + //control Roll + float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET], + att->roll, att->rollspeed, deltaT); + //control Yaw Speed + float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); //attitude_setpoint_bodyframe.z is yaw speed! + + /* + * compensate the vertical loss of thrust + * when thrust plane has an angle. + * start with a factor of 1.0 (no change) + */ + float zcompensation = 1.0f; + + if (fabsf(att->roll) > 1.0f) { + zcompensation *= 1.85081571768f; + + } else { + zcompensation *= 1.0f / cosf(att->roll); + } + + if (fabsf(att->pitch) > 1.0f) { + zcompensation *= 1.85081571768f; + + } else { + zcompensation *= 1.0f / cosf(att->pitch); + } + + float motor_thrust = 0.0f; + + // FLYING MODES + motor_thrust = att_sp->thrust; + + //printf("mot0: %3.1f\n", motor_thrust); + + /* compensate thrust vector for roll / pitch contributions */ + motor_thrust *= zcompensation; + + /* limit yaw rate output */ + if (yaw_rate_control > pid_yawspeed_lim) { + yaw_rate_control = pid_yawspeed_lim; + yaw_speed_controller.saturated = 1; + } + + if (yaw_rate_control < -pid_yawspeed_lim) { + yaw_rate_control = -pid_yawspeed_lim; + yaw_speed_controller.saturated = 1; + } + + if (pitch_control > pid_att_lim) { + pitch_control = pid_att_lim; + pitch_controller.saturated = 1; + } + + if (pitch_control < -pid_att_lim) { + pitch_control = -pid_att_lim; + pitch_controller.saturated = 1; + } + + + if (roll_control > pid_att_lim) { + roll_control = pid_att_lim; + roll_controller.saturated = 1; + } + + if (roll_control < -pid_att_lim) { + roll_control = -pid_att_lim; + roll_controller.saturated = 1; + } + + actuators->control[0] = roll_control; + actuators->control[1] = pitch_control; + actuators->control[2] = yaw_rate_control; + actuators->control[3] = motor_thrust; +} diff --git a/apps/multirotor_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h similarity index 67% rename from apps/multirotor_control/multirotor_attitude_control.h rename to apps/multirotor_att_control/multirotor_attitude_control.h index c3aa470413..c3caa75981 100644 --- a/apps/multirotor_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -1,9 +1,12 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * + * Author: @author Thomas Gubler + * @author Julian Oes + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,22 +37,25 @@ * ****************************************************************************/ -/** +/* * @file multirotor_attitude_control.h - * - * attitude control for multirotors + * Attitude control for multi rotors. */ -#ifndef ATTITUDE_CONTROL_H_ -#define ATTITUDE_CONTROL_H_ +#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_ +#define MULTIROTOR_ATTITUDE_CONTROL_H_ #include #include #include +#include +#include #include #include -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); +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + const struct vehicle_status_s *status, struct actuator_controls_s *actuators, bool verbose); -#endif /* ATTITUDE_CONTROL_H_ */ +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose); + +#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/apps/multirotor_control/multirotor_attitude_control.c b/apps/multirotor_control/multirotor_attitude_control.c deleted file mode 100644 index 5d298ab543..0000000000 --- a/apps/multirotor_control/multirotor_attitude_control.c +++ /dev/null @@ -1,456 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file multirotor_attitude_control.c - * - * Implementation of multirotor attitude controller. - */ - -#include "multirotor_attitude_control.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -extern int multirotor_write; -extern int gpios; - -#define CONTROL_PID_ATTITUDE_INTERVAL 5e-3 - -// void turn_xy_plane(const float_vect3 *vector, float yaw, -// float_vect3 *result) -// { -// //turn clockwise -// static uint16_t counter; - -// result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y); -// result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y); -// result->z = vector->z; //leave direction normal to xy-plane untouched - -// counter++; -// } - -// void navi2body_xy_plane(const float_vect3 *vector, const float yaw, -// float_vect3 *result) -// { -// turn_xy_plane(vector, yaw, result); -// // result->x = vector->x; -// // result->y = vector->y; -// // result->z = vector->z; -// // result->x = cos(yaw) * vector->x + sin(yaw) * vector->y; -// // result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y; -// // result->z = vector->z; //leave direction normal to xy-plane untouched -// } - -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; - - static PID_t yaw_pos_controller; - static PID_t yaw_speed_controller; - static PID_t nick_controller; - static PID_t roll_controller; - - // XXXM - static const float min_gas = 1; - static const float max_gas = 512; - - - static uint16_t motor_pwm[4] = {0, 0, 0, 0}; - static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f}; -// static float remote_control_weight_z = 1; -// static float position_control_weight_z = 0; - - static float pid_yawpos_lim; - static float pid_yawspeed_lim; - static float pid_att_lim; - - static bool initialized; - - // static float_vect3 attitude_setpoint_navigationframe_from_positioncontroller; - - // static hrt_abstime now_time; - // static hrt_abstime last_time; - - static commander_state_machine_t current_state; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - - pid_init(&yaw_pos_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU], - PID_MODE_DERIVATIV_CALC, 154); - - pid_init(&yaw_speed_controller, - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU], - PID_MODE_DERIVATIV_CALC, 155); - - pid_init(&nick_controller, - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], - PID_MODE_DERIVATIV_SET, 156); - - pid_init(&roll_controller, - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], - PID_MODE_DERIVATIV_SET, 157); - - pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; - pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; - pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; - - // //TODO: true initialization? get gps while on ground? - // attitude_setpoint_navigationframe_from_positioncontroller.x = 0.0f; - // attitude_setpoint_navigationframe_from_positioncontroller.y = 0.0f; - // attitude_setpoint_navigationframe_from_positioncontroller.z = 0.0f; - - // last_time = 0; - initialized = true; - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 50 == 0) { - pid_set_parameters(&yaw_pos_controller, - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]); - - pid_set_parameters(&yaw_speed_controller, - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]); - - pid_set_parameters(&nick_controller, - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); - - pid_set_parameters(&roll_controller, - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); - - pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; - pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; - pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; - } - - current_state = status->state_machine; - float_vect3 attitude_setpoint_bodyframe = {0.0f, 0.0f, 0.0f}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller - - if (current_state == SYSTEM_STATE_AUTO) { - - // attitude_setpoint_navigationframe_from_positioncontroller.x = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[0]; - // attitude_setpoint_navigationframe_from_positioncontroller.y = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[1]; - // attitude_setpoint_navigationframe_from_positioncontroller.z = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[2]; - - // float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z; - - // // don't turn around the wrong side (only works if yaw angle is between +- 180 degree) - // if (yaw_e > M_PI) { - // yaw_e -= 2.0f * M_PI; - // } - - // if (yaw_e < -M_PI) { - // yaw_e += 2.0f * M_PI; - // } - - // attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL); - - - // /* limit control output */ - // if (attitude_setpoint_navigationframe_from_positioncontroller.z > pid_yawpos_lim) { - // attitude_setpoint_navigationframe_from_positioncontroller.z = pid_yawpos_lim; - // yaw_pos_controller.saturated = 1; - // } - - // if (attitude_setpoint_navigationframe_from_positioncontroller.z < -pid_yawpos_lim) { - // attitude_setpoint_navigationframe_from_positioncontroller.z = -pid_yawpos_lim; - // yaw_pos_controller.saturated = 1; - // } - - // //transform attitude setpoint from position controller from navi to body frame on xy_plane - // float_vect3 attitude_setpoint_bodyframe_from_positioncontroller; - // navi2body_xy_plane(&attitude_setpoint_navigationframe_from_positioncontroller, att->yaw , &attitude_setpoint_bodyframe_from_positioncontroller); //yaw angle= att->yaw - // //now everything is in body frame - - - // //TODO: here we decide which input (position controller or ppm) we use. For now we have only the ppm, this should be decided dpending on the state machione (manula or auto) ppm should always overwrite auto (?) - // attitude_setpoint_bodyframe.x = attitude_setpoint_bodyframe_from_positioncontroller.x; - // attitude_setpoint_bodyframe.y = attitude_setpoint_bodyframe_from_positioncontroller.y; - // attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z; - - } else if (current_state == SYSTEM_STATE_MANUAL) { - attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * 3.14159265 / 8.0f; - attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * 3.14159265 / 8.0f; - attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * 3.14159265; - } - - /* add an attitude offset which needs to be estimated somewhere */ - attitude_setpoint_bodyframe.x += global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET]; - attitude_setpoint_bodyframe.y += global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET]; - - /*Calculate Controllers*/ - //control Nick - float nick = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL); - //control Roll - float roll = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL); - //control Yaw Speed - float yaw = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0, CONTROL_PID_ATTITUDE_INTERVAL); //attitude_setpoint_bodyframe.z is yaw speed! - - //compensation to keep force in z-direction - float zcompensation; - - if (fabs(att->roll) > 0.5f) { - zcompensation = 1.13949393f; - - } else { - zcompensation = 1.0f / cosf(att->roll); - } - - if (fabs(att->pitch) > 0.5f) { - zcompensation *= 1.13949393f; - - } else { - zcompensation *= 1.0f / cosf(att->pitch); - } - - // use global_data.position_control_output.z and mix parameter global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] - // to compute thrust for Z position control - // - // float motor_thrust = min_gas + - // ( ( 1 - global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] ) * ( max_gas - min_gas ) * global_data.gas_remote * zcompensation ) - // + ( global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] * ( max_gas - min_gas ) * controlled_thrust * zcompensation ); - //calculate the basic thrust - - - - float motor_thrust = 0; - - // FLYING MODES - if (current_state == SYSTEM_STATE_MANUAL) { - motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; - - } else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) { - motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; //TODO - - } else if (current_state == SYSTEM_STATE_EMCY_LANDING) { - motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; //TODO - - } else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) { - motor_thrust = 0.0f; //immediately cut off thrust! - - } else { - motor_thrust = 0.0f; // Motor thrust must be zero in any other mode! - } - - // Convertion to motor-step units - motor_thrust *= zcompensation; - motor_thrust *= max_gas / 20000.0f; //TODO: check this - motor_thrust += (max_gas - min_gas) / 2.f; - - // XXXM - // RC channels: assumed to be -10000 to +10000 - - - - //limit control output - //yawspeed - if (yaw > pid_yawspeed_lim) { - yaw = pid_yawspeed_lim; - yaw_speed_controller.saturated = 1; - } - - if (yaw < -pid_yawspeed_lim) { - yaw = -pid_yawspeed_lim; - yaw_speed_controller.saturated = 1; - } - - if (nick > pid_att_lim) { - nick = pid_att_lim; - nick_controller.saturated = 1; - } - - if (nick < -pid_att_lim) { - nick = -pid_att_lim; - nick_controller.saturated = 1; - } - - - if (roll > pid_att_lim) { - roll = pid_att_lim; - roll_controller.saturated = 1; - } - - if (roll < -pid_att_lim) { - roll = -pid_att_lim; - roll_controller.saturated = 1; - } - - // /* Emit controller values */ - // ar_control->setpoint_thrust_cast = motor_thrust; - // ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x; - // ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y; - // ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z; - // ar_control->attitude_control_output[0] = roll; - // ar_control->attitude_control_output[1] = nick; - // ar_control->attitude_control_output[2] = yaw; - // ar_control->zcompensation = zcompensation; - // orb_publish(ORB_ID(multirotor_control), multirotor_pub, ar_control); - - static float output_band = 0.f; - static float band_factor = 0.75f; - static float startpoint_full_control = 150.0f; //TODO - static float yaw_factor = 1.0f; - - - - - - - // MIXING AND THRUST LIMITING START - - - - if (motor_thrust <= min_gas) { - motor_thrust = min_gas; - output_band = 0.f; - - } else if (motor_thrust < startpoint_full_control && motor_thrust > min_gas) { - output_band = band_factor * (motor_thrust - min_gas); - - } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_gas - band_factor * startpoint_full_control) { - output_band = band_factor * startpoint_full_control; - - } else if (motor_thrust >= max_gas - band_factor * startpoint_full_control) { - output_band = band_factor * (max_gas - motor_thrust); - } - - //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer - - // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw); - - // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw); - - // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw); - - // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw); - - // if we are not in the output band - if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band - && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band - && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band - && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { - - yaw_factor = 0.5f; - // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw * yaw_factor); - - // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw * yaw_factor); - - // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw * yaw_factor); - - // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw * yaw_factor); - } - - uint8_t i; - - for (i = 0; i < 4; i++) { - //check for limits - if (motor_calc[i] < motor_thrust - output_band) { - motor_calc[i] = motor_thrust - output_band; - } - - if (motor_calc[i] > motor_thrust + output_band) { - motor_calc[i] = motor_thrust + output_band; - } - } - - // Write out actual thrust - - // XXXM - motor_pwm[0] = (uint16_t) motor_calc[0]; - motor_pwm[1] = (uint16_t) motor_calc[1]; - motor_pwm[2] = (uint16_t) motor_calc[2]; - motor_pwm[3] = (uint16_t) motor_calc[3]; - - //SEND MOTOR COMMANDS - - // XXXM - - motor_skip_counter++; - -// now_time = hrt_absolute_time() / 1000000; -// if(now_time - last_time > 0) -// { -// printf("Counter: %ld\n",control_counter); -// last_time = now_time; -// control_counter = 0; -// } -} diff --git a/apps/multirotor_control/multirotor_control.h b/apps/multirotor_control/multirotor_control.h deleted file mode 100644 index cd779d3691..0000000000 --- a/apps/multirotor_control/multirotor_control.h +++ /dev/null @@ -1,12 +0,0 @@ -/** - * @file multirotor_control.h - * - * Created on: Mar 23, 2012 - * Author: thomasgubler - */ - -#ifndef multirotor_CONTROL_H_ -#define multirotor_CONTROL_H_ - - -#endif /* multirotor_CONTROL_H_ */ diff --git a/apps/multirotor_position_control/Makefile b/apps/multirotor_pos_control/Makefile similarity index 97% rename from apps/multirotor_position_control/Makefile rename to apps/multirotor_pos_control/Makefile index f95a4b50a3..c88c854356 100644 --- a/apps/multirotor_position_control/Makefile +++ b/apps/multirotor_pos_control/Makefile @@ -35,7 +35,7 @@ # Makefile to build multirotor position control # -APPNAME = multirotor_position_control +APPNAME = multirotor_pos_control PRIORITY = SCHED_PRIORITY_MAX - 25 STACKSIZE = 2048 diff --git a/apps/multirotor_position_control/multirotor_position_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c similarity index 95% rename from apps/multirotor_position_control/multirotor_position_control.c rename to apps/multirotor_pos_control/multirotor_pos_control.c index 52930fd831..ff32fb4607 100644 --- a/apps/multirotor_position_control/multirotor_position_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -59,7 +59,7 @@ #include #include -__EXPORT int multirotor_position_control_main(int argc, char *argv[]); +__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); static bool thread_should_exit; static bool thread_running = false; @@ -69,7 +69,7 @@ static int mpc_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor pos ctrl] Control started, taking over position control\n"); + printf("[multirotor pos control] Control started, taking over position control\n"); /* structures */ struct vehicle_status_s state; @@ -129,7 +129,7 @@ mpc_thread_main(int argc, char *argv[]) close(ardrone_write); ar_multiplexing_deinit(gpios); - printf("[ardrone_control] ending now...\r\n"); + printf("[multirotor pos control] ending now...\r\n"); fflush(stdout); return 0; } diff --git a/apps/multirotor_position_control/position_control.c b/apps/multirotor_pos_control/position_control.c similarity index 100% rename from apps/multirotor_position_control/position_control.c rename to apps/multirotor_pos_control/position_control.c diff --git a/apps/multirotor_position_control/position_control.h b/apps/multirotor_pos_control/position_control.h similarity index 100% rename from apps/multirotor_position_control/position_control.h rename to apps/multirotor_pos_control/position_control.h diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 77b7b9c47d..6a06a786f1 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -433,7 +433,7 @@ int sensors_thread_main(int argc, char *argv[]) thread_running = true; - while (1) { + while (!thread_should_exit) { pthread_mutex_lock(&sensors_read_ready_mutex); struct timespec time_to_wait = {0, 0}; diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index 172eef979d..e1a6db598f 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -43,7 +43,8 @@ CSRCS = err.c \ # XXX this really should be a CONFIG_* test # ifeq ($(TARGET),px4fmu) -CSRCS += systemlib.c +CSRCS += systemlib.c \ + pid/pid.c endif include $(APPDIR)/mk/app.mk diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index a24657a411..807373c158 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -41,7 +41,7 @@ #include "pid.h" -void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i) { pid->kp = kp; @@ -57,7 +57,7 @@ void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->error_previous = 0; pid->integral = 0; } -void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) +__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) { pid->kp = kp; pid->ki = ki; @@ -84,7 +84,7 @@ void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) * @param dt * @return */ -float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) { /* error = setpoint - actual_position integral = integral + (error*dt) diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index d62843ed4a..83bf09b59f 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -64,10 +64,10 @@ typedef struct { uint8_t saturated; } PID_t; -void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i); -void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i); +__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); //void pid_set(PID_t *pid, float sp); -float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 01b67cfef4..da119e14fc 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -71,7 +71,7 @@ CONFIGURED_APPS += commander #CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors CONFIGURED_APPS += ardrone_control -CONFIGURED_APPS += multirotor_control +CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += px4/attitude_estimator_bm CONFIGURED_APPS += fixedwing_control CONFIGURED_APPS += mix_and_link From 86ed36579a51ea73688d9053346f95f2d06ffed5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Aug 2012 16:32:54 +0200 Subject: [PATCH 3/4] Updated ROMFS scrips, created new ardrone_interface to jointly use multirotor_att_control on all multirotors, including AR.Drone frames --- ROMFS/scripts/rc.PX4IO | 2 +- ROMFS/scripts/rc.PX4IOAR | 2 +- ROMFS/scripts/rc.sensors | 5 +- apps/ardrone_interface/Makefile | 42 ++ apps/ardrone_interface/ardrone_interface.c | 281 ++++++++++++ .../ardrone_interface/ardrone_motor_control.c | 433 ++++++++++++++++++ .../ardrone_interface/ardrone_motor_control.h | 79 ++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 8 files changed, 840 insertions(+), 5 deletions(-) create mode 100644 apps/ardrone_interface/Makefile create mode 100644 apps/ardrone_interface/ardrone_interface.c create mode 100644 apps/ardrone_interface/ardrone_motor_control.c create mode 100644 apps/ardrone_interface/ardrone_motor_control.h diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index e2f4fca84f..13c759db2d 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -18,7 +18,7 @@ sh /etc/init.d/rc.sensors # # Start MAVLink # -mavlink -d /dev/ttyS0 -b 57600 & +mavlink start -d /dev/ttyS0 -b 57600 # # Start the commander. diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index d3cf8b5067..2a771cac48 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -18,7 +18,7 @@ sh /etc/init.d/rc.sensors # # Start MAVLink # -mavlink -d /dev/ttyS0 -b 57600 & +mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 # diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/scripts/rc.sensors index 7515c1947e..b6b4c65cb4 100644 --- a/ROMFS/scripts/rc.sensors +++ b/ROMFS/scripts/rc.sensors @@ -8,13 +8,12 @@ # #ms5611 start +#mpu6000 start # # Start the sensor collection task. # -# XXX should be 'sensors start' -# -sensors & +sensors start # # Test sensor functionality diff --git a/apps/ardrone_interface/Makefile b/apps/ardrone_interface/Makefile new file mode 100644 index 0000000000..fea96082f6 --- /dev/null +++ b/apps/ardrone_interface/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build ardrone interface +# + +APPNAME = ardrone_interface +PRIORITY = SCHED_PRIORITY_MAX - 15 +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c new file mode 100644 index 0000000000..f354252757 --- /dev/null +++ b/apps/ardrone_interface/ardrone_interface.c @@ -0,0 +1,281 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ardrone_interface.c + * Implementation of AR.Drone 1.0 / 2.0 motor control interface. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ardrone_motor_control.h" + +__EXPORT int ardrone_interface_main(int argc, char *argv[]); + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int ardrone_interface_task; /**< Handle of deamon task / thread */ + +/** + * Mainloop of ardrone_interface. + */ +int ardrone_interface_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int ardrone_interface_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("ardrone_interface already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + ardrone_interface_task = task_create("ardrone_interface", SCHED_PRIORITY_MAX - 15, 4096, ardrone_interface_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + thread_running = true; + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tardrone_interface is running\n"); + } else { + printf("\tardrone_interface not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int ardrone_interface_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + printf("[ardrone_interface] Control started, taking over motors\n"); + + /* default values for arguments */ + char *ardrone_uart_name = "/dev/ttyS1"; + + /* File descriptors */ + int ardrone_write; + int gpios; + + enum { + CONTROL_MODE_RATES = 0, + CONTROL_MODE_ATTITUDE = 1, + } control_mode = CONTROL_MODE_ATTITUDE; + + char *commandline_usage = "\tusage: ardrone_interface -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n"; + + bool motor_test_mode = false; + + /* read commandline arguments */ + for (int 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 ERROR; + } + + } 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 ERROR; + } + + } else { + printf(commandline_usage); + return ERROR; + } + + } else if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) { + motor_test_mode = true; + } + } + + /* open uarts */ + printf("[ardrone_interface] AR.Drone UART is %s\n", ardrone_uart_name); + ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY); + if (ardrone_write < 0) { + fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + exit(ERROR); + } + + /* initialize motors */ + if (OK != ar_init_motors(ardrone_write, &gpios)) { + close(ardrone_write); + fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + exit(ERROR); + } + + /* Led animation */ + int counter = 0; + int led_counter = 0; + + /* declare and safely initialize all structs */ + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); + + /* subscribe to attitude, motor setpoints and system state */ + int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + + while (!thread_should_exit) { + + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of the actuator controls */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); + // if .. + ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode); + // } else { + // /* Silently lock down motor speeds to zero */ + // ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); + // } + + if (counter % 30 == 0) { + if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); + + if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); + + if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); + + if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); + + if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); + + if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); + + if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); + + if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); + + if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); + + if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); + + if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); + + if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); + + led_counter++; + + if (led_counter == 12) led_counter = 0; + } + + /* run at approximately 50 Hz */ + usleep(20000); + + // This is a hardcore debug code piece to validate + // the motor interface + // uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5}; + // ar_get_motor_packet(motorSpeedBuf, 20, 20, 20, 20); + // write(ardrone_write, motorSpeedBuf, 5); + // usleep(15000); + + counter++; + } + + /* close uarts */ + close(ardrone_write); + ar_multiplexing_deinit(gpios); + + printf("[ardrone_interface] ending now...\n"); + fflush(stdout); + + thread_running = false; + + return OK; +} + diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c new file mode 100644 index 0000000000..ad58939637 --- /dev/null +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -0,0 +1,433 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ardrone_motor_control.c + * Implementation of AR.Drone 1.0 / 2.0 motor control interface + */ + +#include +#include +#include +#include +#include +#include + +#include "ardrone_motor_control.h" + +static const unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2; +static const unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 }; + +typedef union { + uint16_t motor_value; + uint8_t bytes[2]; +} motor_union_t; + +/** + * @brief Generate the 8-byte motor set packet + * + * @return the number of bytes (8) + */ +void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) +{ + motor_buf[0] = 0x20; + motor_buf[1] = 0x00; + motor_buf[2] = 0x00; + motor_buf[3] = 0x00; + motor_buf[4] = 0x00; + /* + * {0x20, 0x00, 0x00, 0x00, 0x00}; + * 0x20 is start sign / motor command + */ + motor_union_t curr_motor; + uint16_t nineBitMask = 0x1FF; + + /* Set motor 1 */ + curr_motor.motor_value = (motor1 & nineBitMask) << 4; + motor_buf[0] |= curr_motor.bytes[1]; + motor_buf[1] |= curr_motor.bytes[0]; + + /* Set motor 2 */ + curr_motor.motor_value = (motor2 & nineBitMask) << 3; + motor_buf[1] |= curr_motor.bytes[1]; + motor_buf[2] |= curr_motor.bytes[0]; + + /* Set motor 3 */ + curr_motor.motor_value = (motor3 & nineBitMask) << 2; + motor_buf[2] |= curr_motor.bytes[1]; + motor_buf[3] |= curr_motor.bytes[0]; + + /* Set motor 4 */ + curr_motor.motor_value = (motor4 & nineBitMask) << 1; + motor_buf[3] |= curr_motor.bytes[1]; + motor_buf[4] |= curr_motor.bytes[0]; +} + +void ar_enable_broadcast(int fd) +{ + ar_select_motor(fd, 0); +} + +int ar_multiplexing_init() +{ + int fd; + + fd = open(GPIO_DEVICE_PATH, 0); + + if (fd < 0) { + printf("GPIO: open fail\n"); + return fd; + } + + /* deactivate all outputs */ + int ret = 0; + ret += ioctl(fd, GPIO_SET, motor_gpios); + + if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) { + printf("GPIO: output set fail\n"); + close(fd); + return -1; + } + + if (ret < 0) { + printf("GPIO: clearing pins fail\n"); + close(fd); + return -1; + } + + return fd; +} + +int ar_multiplexing_deinit(int fd) +{ + if (fd < 0) { + printf("GPIO: no valid descriptor\n"); + return fd; + } + + int ret = 0; + + /* deselect motor 1-4 */ + ret += ioctl(fd, GPIO_SET, motor_gpios); + + if (ret != 0) { + printf("GPIO: clear failed %d times\n", ret); + } + + if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) { + printf("GPIO: input set fail\n"); + return -1; + } + + close(fd); + + return ret; +} + +int ar_select_motor(int fd, uint8_t motor) +{ + int ret = 0; + unsigned long gpioset; + /* + * Four GPIOS: + * GPIO_EXT1 + * GPIO_EXT2 + * GPIO_UART2_CTS + * GPIO_UART2_RTS + */ + + /* select motor 0 to enable broadcast */ + if (motor == 0) { + /* select motor 1-4 */ + ret += ioctl(fd, GPIO_CLEAR, motor_gpios); + + } else { + /* deselect all */ + ret += ioctl(fd, GPIO_SET, motor_gpios); + + /* select reqested motor */ + ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); + + /* deselect all others */ + // gpioset = motor_gpios ^ motor_gpio[motor - 1]; + // ret += ioctl(fd, GPIO_SET, gpioset); + } + + return ret; +} + +int ar_init_motors(int ardrone_uart, int *gpios_pin) +{ + /* Initialize multiplexing */ + *gpios_pin = ar_multiplexing_init(); + + /* Write ARDrone commands on UART2 */ + uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40}; + uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0}; + + /* initialize all motors + * - select one motor at a time + * - configure motor + */ + int i; + int errcounter = 0; + + for (i = 1; i < 5; ++i) { + /* Initialize motors 1-4 */ + initbuf[3] = i; + errcounter += ar_select_motor(*gpios_pin, i); + + write(ardrone_uart, initbuf + 0, 1); + + /* sleep 400 ms */ + usleep(200000); + usleep(200000); + + write(ardrone_uart, initbuf + 1, 1); + /* wait 50 ms */ + usleep(50000); + + write(ardrone_uart, initbuf + 2, 1); + /* wait 50 ms */ + usleep(50000); + + write(ardrone_uart, initbuf + 3, 1); + /* wait 50 ms */ + usleep(50000); + + write(ardrone_uart, initbuf + 4, 1); + /* wait 50 ms */ + usleep(50000); + + /* enable multicast */ + write(ardrone_uart, multicastbuf + 0, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 1, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 2, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 3, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 4, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 5, 1); + /* wait 5 ms */ + usleep(50000); + } + + /* start the multicast part */ + errcounter += ar_select_motor(*gpios_pin, 0); + + if (errcounter != 0) { + fprintf(stderr, "[ar motors] init sequence incomplete, failed %d times", -errcounter); + fflush(stdout); + } + return errcounter; +} + +/* + * Sets the leds on the motor controllers, 1 turns led on, 0 off. + */ +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) +{ + /* + * 2 bytes are sent. The first 3 bits describe the command: 011 means led control + * the following 4 bits are the red leds for motor 4, 3, 2, 1 + * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1 + * the last bit is unknown. + * + * The packet is therefore: + * 011 rrrr 0000 gggg 0 + */ + uint8_t leds[2]; + leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); + leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); + write(ardrone_uart, leds, 2); +} + +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { + const unsigned int min_motor_interval = 20000; + static uint64_t last_motor_time = 0; + if (hrt_absolute_time() - last_motor_time > min_motor_interval) { + uint8_t buf[5] = {0}; + ar_get_motor_packet(buf, motor1, motor2, motor3, motor4); + int ret; + if ((ret = write(ardrone_fd, buf, sizeof(buf))) > 0) { + return OK; + } else { + return ret; + } + } else { + return -ERROR; + } +} + +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose) { + + float roll_control = actuators->control[0]; + float pitch_control = actuators->control[1]; + float yaw_control = actuators->control[2]; + float motor_thrust = actuators->control[3]; + + unsigned int motor_skip_counter = 0; + + const float min_thrust = 0.02f; /**< 2% minimum thrust */ + const float max_thrust = 1.0f; /**< 100% max thrust */ + const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ + + const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ + const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ + + /* initialize all fields to zero */ + uint16_t motor_pwm[4] = {0}; + float motor_calc[4] = {0}; + + float output_band = 0.0f; + float band_factor = 0.75f; + const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ + float yaw_factor = 1.0f; + + if (motor_thrust <= min_thrust) { + motor_thrust = min_thrust; + output_band = 0.0f; + + } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { + output_band = band_factor * (motor_thrust - min_thrust); + + } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { + output_band = band_factor * startpoint_full_control; + + } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { + output_band = band_factor * (max_thrust - motor_thrust); + } + + if (verbose && motor_skip_counter % 100 == 0) { + printf("1: mot1: %3.1f band: %3.1f r: %3.1f n: %3.1f y: %3.1f\n", (double)motor_thrust, (double)output_band, (double)roll_control, (double)pitch_control, (double)yaw_control); + } + + //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer + + // FRONT (MOTOR 1) + motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); + + // RIGHT (MOTOR 2) + motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); + + // BACK (MOTOR 3) + motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); + + // LEFT (MOTOR 4) + motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); + + // if we are not in the output band + if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band + && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band + && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band + && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { + + yaw_factor = 0.5f; + // FRONT (MOTOR 1) + motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor); + + // RIGHT (MOTOR 2) + motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor); + + // BACK (MOTOR 3) + motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor); + + // LEFT (MOTOR 4) + motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor); + } + + if (verbose && motor_skip_counter % 100 == 0) { + printf("2: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]); + } + + for (int i = 0; i < 4; i++) { + //check for limits + if (motor_calc[i] < motor_thrust - output_band) { + motor_calc[i] = motor_thrust - output_band; + } + + if (motor_calc[i] > motor_thrust + output_band) { + motor_calc[i] = motor_thrust + output_band; + } + } + + if (verbose && motor_skip_counter % 100 == 0) { + printf("3: band lim: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]); + } + + /* set the motor values */ + + /* scale up from 0..1 to 10..512) */ + motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); + + if (verbose && motor_skip_counter % 100 == 0) { + printf("4: scaled: m1: %d m2: %d m3: %d m4: %d\n", motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + } + + /* Keep motors spinning while armed and prevent overflows */ + + /* Failsafe logic - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; + motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; + motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; + motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; + + /* Failsafe logic - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512; + motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512; + motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512; + motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512; + + /* send motors via UART */ + if (verbose && motor_skip_counter % 100 == 0) printf("5: mot: %3.1f-%i-%i-%i-%i\n\n", (double)motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + + motor_skip_counter++; +} diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h new file mode 100644 index 0000000000..b8dd2abe40 --- /dev/null +++ b/apps/ardrone_interface/ardrone_motor_control.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ardrone_motor_control.h + * Definition of AR.Drone 1.0 / 2.0 motor control interface + */ + +#include +#include + +/** + * Generate the 5-byte motor set packet. + * + * @return the number of bytes (5) + */ +void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); + +/** + * Select a motor in the multiplexing. + */ +int ar_select_motor(int fd, uint8_t motor); + +void ar_enable_broadcast(int fd); + +int ar_multiplexing_init(void); +int ar_multiplexing_deinit(int fd); + +/** + * Write four motor commands to an already initialized port. + * + * Writing 0 stops a motor, values from 1-512 encode the full thrust range. + * on some motor controller firmware revisions a minimum value of 10 is + * required to spin the motors. + */ +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); + +/** + * Initialize the motors. + */ +int ar_init_motors(int ardrone_uart, int *gpios_pin); + +/** + * Set LED pattern. + */ +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); + +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose); diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index da119e14fc..b37bc386da 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -71,6 +71,7 @@ CONFIGURED_APPS += commander #CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors CONFIGURED_APPS += ardrone_control +CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += px4/attitude_estimator_bm CONFIGURED_APPS += fixedwing_control From 9b239bc00197a862bca985f67457f8c47d163eb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Aug 2012 16:43:51 +0200 Subject: [PATCH 4/4] fixed typo, removed deamon example from default build --- apps/ardrone_interface/ardrone_interface.c | 4 ++-- nuttx/configs/px4fmu/nsh/appconfig | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index f354252757..e781d46130 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -79,7 +79,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d ]\n\n"); exit(1); } @@ -271,7 +271,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) close(ardrone_write); ar_multiplexing_deinit(gpios); - printf("[ardrone_interface] ending now...\n"); + printf("[ardrone_interface] ending now...\n\n"); fflush(stdout); thread_running = false; diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index b37bc386da..5032437964 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -58,7 +58,7 @@ CONFIGURED_APPS += systemcmds/mixer # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/deamon -CONFIGURED_APPS += examples/px4_deamon_app +# CONFIGURED_APPS += examples/px4_deamon_app CONFIGURED_APPS += uORB