From 36ed8bb97a5b3638f8d3678a7c49744d21ea2e19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Sep 2012 22:13:28 +0200 Subject: [PATCH] Removed old AR drone control stuff, outdated - replaced by multirotor_att and position control --- apps/ardrone_control/.context | 0 apps/ardrone_control/Makefile | 45 --- apps/ardrone_control/ardrone_control.c | 321 ----------------- apps/ardrone_control/ardrone_control.h | 12 - apps/ardrone_control/ardrone_motor_control.c | 299 ---------------- apps/ardrone_control/ardrone_motor_control.h | 76 ---- apps/ardrone_control/attitude_control.c | 355 ------------------- apps/ardrone_control/attitude_control.h | 61 ---- apps/ardrone_control/pid.c | 109 ------ apps/ardrone_control/pid.h | 40 --- apps/ardrone_control/position_control.c | 308 ---------------- apps/ardrone_control/position_control.h | 13 - apps/ardrone_control/rate_control.c | 320 ----------------- apps/ardrone_control/rate_control.h | 49 --- 14 files changed, 2008 deletions(-) delete mode 100644 apps/ardrone_control/.context delete mode 100644 apps/ardrone_control/Makefile delete mode 100644 apps/ardrone_control/ardrone_control.c delete mode 100644 apps/ardrone_control/ardrone_control.h delete mode 100644 apps/ardrone_control/ardrone_motor_control.c delete mode 100644 apps/ardrone_control/ardrone_motor_control.h delete mode 100644 apps/ardrone_control/attitude_control.c delete mode 100644 apps/ardrone_control/attitude_control.h delete mode 100644 apps/ardrone_control/pid.c delete mode 100644 apps/ardrone_control/pid.h delete mode 100644 apps/ardrone_control/position_control.c delete mode 100644 apps/ardrone_control/position_control.h delete mode 100644 apps/ardrone_control/rate_control.c delete mode 100644 apps/ardrone_control/rate_control.h diff --git a/apps/ardrone_control/.context b/apps/ardrone_control/.context deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile deleted file mode 100644 index 35fbe01d73..0000000000 --- a/apps/ardrone_control/Makefile +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# 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 uORB -# - -APPNAME = ardrone_control -PRIORITY = SCHED_PRIORITY_MAX - 15 -STACKSIZE = 4096 - -# explicit list of sources - not everything is built currently -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 deleted file mode 100644 index 72e8ae14ee..0000000000 --- a/apps/ardrone_control/ardrone_control.c +++ /dev/null @@ -1,321 +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 -#include "ardrone_control.h" -#include "attitude_control.h" -#include "rate_control.h" -#include "ardrone_motor_control.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -__EXPORT int ardrone_control_main(int argc, char *argv[]); - -// static void turn_xy_plane(const float_vect3 *vector, float yaw, -// float_vect3 *result); -// static void navi2body_xy_plane(const float_vect3 *vector, const float yaw, -// float_vect3 *result); - -// static 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++; -// } - -// static 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 -// } - -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"; - - /* 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_control -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_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) { - fprintf(stderr, "[ardrone_control] 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_control] 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 vehicle_attitude_s att; - 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)); - - /* 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 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)); - - while (1) { - - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - - 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 || - motor_test_mode) { - - 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(ardrone_write, &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) { - - /* 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); - - 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; - } - - } - - float roll_control, pitch_control, yaw_control, thrust_control; - - 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 */ - if (counter % 200 == 0) printf("[multirotor control] INVALID CONTROL MODE, locking down propulsion\n"); - ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); - } - } 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 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...\n"); - fflush(stdout); - return OK; -} - diff --git a/apps/ardrone_control/ardrone_control.h b/apps/ardrone_control/ardrone_control.h deleted file mode 100644 index 7f9567f861..0000000000 --- a/apps/ardrone_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/ardrone_control/ardrone_motor_control.c b/apps/ardrone_control/ardrone_motor_control.c deleted file mode 100644 index e9733a1a03..0000000000 --- a/apps/ardrone_control/ardrone_motor_control.c +++ /dev/null @@ -1,299 +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; -} - -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 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; - } -} diff --git a/apps/ardrone_control/ardrone_motor_control.h b/apps/ardrone_control/ardrone_motor_control.h deleted file mode 100644 index 2e6df0e694..0000000000 --- a/apps/ardrone_control/ardrone_motor_control.h +++ /dev/null @@ -1,76 +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 - - -/** - * 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); - -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/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c deleted file mode 100644 index 77e08a535c..0000000000 --- a/apps/ardrone_control/attitude_control.c +++ /dev/null @@ -1,355 +0,0 @@ -/**************************************************************************** - * - * 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 attitude_control.c - * Implementation of attitude controller - */ - -#include "attitude_control.h" -#include -#include -#include -#include -#include -#include "ardrone_motor_control.h" -#include -#include -#include "pid.h" -#include - -#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, - 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; -} - -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[MAX_MOTOR_COUNT] = {0}; - float motor_calc[MAX_MOTOR_COUNT] = {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_control/attitude_control.h b/apps/ardrone_control/attitude_control.h deleted file mode 100644 index e14d299662..0000000000 --- a/apps/ardrone_control/attitude_control.h +++ /dev/null @@ -1,61 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 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 attitude_control.h - * attitude control for multi rotors - */ - -#ifndef ATTITUDE_CONTROL_H_ -#define ATTITUDE_CONTROL_H_ - -#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); - -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/pid.c b/apps/ardrone_control/pid.c deleted file mode 100644 index 5ce05670e3..0000000000 --- a/apps/ardrone_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/ardrone_control/pid.h b/apps/ardrone_control/pid.h deleted file mode 100644 index a721c839c6..0000000000 --- a/apps/ardrone_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/ardrone_control/position_control.c b/apps/ardrone_control/position_control.c deleted file mode 100644 index 6ba1654595..0000000000 --- a/apps/ardrone_control/position_control.c +++ /dev/null @@ -1,308 +0,0 @@ -/**************************************************************************** - * 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 - * - * 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 "position_control.h" -#include -#include -#include -#include -#include -#include -#include -#include "pid.h" - -#ifndef FM_PI -#define FM_PI 3.1415926535897932384626433832795f -#endif - - - -#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) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - - const double radius_earth = 6371000.0; - - return radius_earth * c; -} - -float get_bearing_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - - 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; - } - - return theta; -} - -void control_position(void) -{ - static PID_t distance_controller; - - static int read_ret; - static global_data_position_t position_estimated; - - static uint16_t counter; - - static bool initialized; - static uint16_t pm_counter; - - static float lat_next; - static float lon_next; - - static float pitch_current; - - static float thrust_total; - - - 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], - global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], - PID_MODE_DERIVATIV_CALC, 150);//150 - -// 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 */ - /* Wait for new position estimate */ - do { - read_ret = read_lock_position(&position_estimated); - } while (read_ret != 0); - - lat_next = position_estimated.lat; - lon_next = position_estimated.lon; - - /* attitude initialization */ - global_data_lock(&global_data_attitude->access_conf); - pitch_current = global_data_attitude->pitch; - global_data_unlock(&global_data_attitude->access_conf); - - initialized = true; - } - - /* load new parameters with 10Hz */ - if (counter % 50 == 0) { - if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { - /* check whether new parameters are available */ - if (global_data_parameter_storage->counter > pm_counter) { - pid_set_parameters(&distance_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// -// 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; - printf("Position controller changed pid parameters\n"); - } - } - - global_data_unlock(&global_data_parameter_storage->access_conf); - } - - - /* Wait for new position estimate */ - do { - read_ret = read_lock_position(&position_estimated); - } while (read_ret != 0); - - /* Get next waypoint */ //TODO: add local copy - - if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { - lat_next = global_data_position_setpoint->x; - lon_next = global_data_position_setpoint->y; - global_data_unlock(&global_data_position_setpoint->access_conf); - } - - /* Get distance to waypoint */ - float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// if(counter % 5 == 0) -// printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - - /* Get bearing to waypoint (direction on earth surface to next waypoint) */ - float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - - if (counter % 5 == 0) - printf("bearing: %.4f\n", bearing); - - /* Calculate speed in direction of bearing (needed for controller) */ - float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// if(counter % 5 == 0) -// printf("speed_norm: %.4f\n", speed_norm); - float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - - /* Control Thrust in bearing direction */ - float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, - CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// if(counter % 5 == 0) -// printf("horizontal thrust: %.4f\n", horizontal_thrust); - - /* Get total thrust (from remote for now) */ - if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { - thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? - global_data_unlock(&global_data_rc_channels->access_conf); - } - - const float max_gas = 500.0f; - thrust_total *= max_gas / 20000.0f; //TODO: check this - thrust_total += max_gas / 2.0f; - - - if (horizontal_thrust > thrust_total) { - horizontal_thrust = thrust_total; - - } else if (horizontal_thrust < -thrust_total) { - horizontal_thrust = -thrust_total; - } - - - - //TODO: maybe we want to add a speed controller later... - - /* Calclulate thrust in east and north direction */ - float thrust_north = cosf(bearing) * horizontal_thrust; - float thrust_east = sinf(bearing) * horizontal_thrust; - - if (counter % 10 == 0) { - printf("thrust north: %.4f\n", thrust_north); - printf("thrust east: %.4f\n", thrust_east); - fflush(stdout); - } - - /* Get current attitude */ - if (0 == global_data_trylock(&global_data_attitude->access_conf)) { - pitch_current = global_data_attitude->pitch; - global_data_unlock(&global_data_attitude->access_conf); - } - - /* Get desired pitch & roll */ - float pitch_desired = 0.0f; - float roll_desired = 0.0f; - - if (thrust_total != 0) { - float pitch_fraction = -thrust_north / thrust_total; - float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - - if (roll_fraction < -1) { - roll_fraction = -1; - - } else if (roll_fraction > 1) { - roll_fraction = 1; - } - -// if(counter % 5 == 0) -// { -// printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// fflush(stdout); -// } - - pitch_desired = asinf(pitch_fraction); - 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); - - - counter++; -} diff --git a/apps/ardrone_control/position_control.h b/apps/ardrone_control/position_control.h deleted file mode 100644 index dbc0650e66..0000000000 --- a/apps/ardrone_control/position_control.h +++ /dev/null @@ -1,13 +0,0 @@ -/* - * position_control.h - * - * Created on: May 29, 2012 - * Author: thomasgubler - */ - -#ifndef POSITION_CONTROL_H_ -#define POSITION_CONTROL_H_ - -void control_position(void); - -#endif /* POSITION_CONTROL_H_ */ diff --git a/apps/ardrone_control/rate_control.c b/apps/ardrone_control/rate_control.c deleted file mode 100644 index 6c6b004e45..0000000000 --- a/apps/ardrone_control/rate_control.c +++ /dev/null @@ -1,320 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Tobias Naegeli - * - * 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 rate_control.c - * Implementation of attitude rate control - */ - -#include "rate_control.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(int ardrone_write, 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/ardrone_control/rate_control.h b/apps/ardrone_control/rate_control.h deleted file mode 100644 index 61bd7bf38b..0000000000 --- a/apps/ardrone_control/rate_control.h +++ /dev/null @@ -1,49 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Tobias Naegeli - * - * 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 rate_control.h - * Definition of attitude rate control - */ -#ifndef RATE_CONTROL_H_ -#define RATE_CONTROL_H_ - -#include -#include -#include - -void control_rates(int ardrone_write, struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints); - -#endif /* RATE_CONTROL_H_ */