dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Removed old AR drone control stuff, outdated - replaced by multirotor_att and position control

This commit is contained in:
Lorenz Meier 2012-09-07 22:13:28 +02:00
parent b67d7fc22a
commit 36ed8bb97a
14 changed files with 0 additions and 2008 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
apps/ardrone_control/.context
View File

45
apps/ardrone_control/Makefile
View File

@ -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

321
apps/ardrone_control/ardrone_control.c
View File

@ -1,321 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <math.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <termios.h>
#include <time.h>
#include <sys/prctl.h>
#include <arch/board/up_hrt.h>
#include "ardrone_control.h"
#include "attitude_control.h"
#include "rate_control.h"
#include "ardrone_motor_control.h"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/ardrone_control.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/ardrone_motors_setpoint.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
__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;
}

12
apps/ardrone_control/ardrone_control.h
View File

@ -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_ */

299
apps/ardrone_control/ardrone_motor_control.c
View File

@ -1,299 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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;
}
}

76
apps/ardrone_control/ardrone_motor_control.h
View File

@ -1,76 +0,0 @@
/****************************************************************************
* px4/ardrone_offboard_control.h
*
* Copyright (C) 2012 PX4 Autopilot Project. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 <nuttx/config.h>
#include <pthread.h>
#include <poll.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_gpio.h>
/**
* 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);

355
apps/ardrone_control/attitude_control.c
View File

@ -1,355 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* 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 <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include "ardrone_motor_control.h"
#include <float.h>
#include <math.h>
#include "pid.h"
#include <arch/board/up_hrt.h>
#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++;
}

61
apps/ardrone_control/attitude_control.h
View File

@ -1,61 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* 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 <uORB/uORB.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/ardrone_control.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
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_ */

109
apps/ardrone_control/pid.c
View File

@ -1,109 +0,0 @@
#include "pid.h"
#include <px4/attitude_estimator_bm/matrix.h> //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);
}

40
apps/ardrone_control/pid.h
View File

@ -1,40 +0,0 @@
/*
* pid.h
*
* Created on: May 29, 2012
* Author: thomasgubler
*/
#ifndef PID_H_
#define PID_H_
#include <stdint.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_ */

308
apps/ardrone_control/position_control.c
View File

@ -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 <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <stdbool.h>
#include <float.h>
#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++;
}

13
apps/ardrone_control/position_control.h
View File

@ -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_ */

320
apps/ardrone_control/rate_control.c
View File

@ -1,320 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Tobias Naegeli <nagelit@student.ethz.ch>
*
* 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 <arch/board/up_hrt.h>
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++;
}

49
apps/ardrone_control/rate_control.h
View File

@ -1,49 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Tobias Naegeli <nagelit@student.ethz.ch>
*
* 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 <uORB/uORB.h>
#include <uORB/topics/ardrone_motors_setpoint.h>
#include <uORB/topics/sensor_combined.h>
void control_rates(int ardrone_write, struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints);
#endif /* RATE_CONTROL_H_ */