From 86ed36579a51ea73688d9053346f95f2d06ffed5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Aug 2012 16:32:54 +0200 Subject: [PATCH] Updated ROMFS scrips, created new ardrone_interface to jointly use multirotor_att_control on all multirotors, including AR.Drone frames --- ROMFS/scripts/rc.PX4IO | 2 +- ROMFS/scripts/rc.PX4IOAR | 2 +- ROMFS/scripts/rc.sensors | 5 +- apps/ardrone_interface/Makefile | 42 ++ apps/ardrone_interface/ardrone_interface.c | 281 ++++++++++++ .../ardrone_interface/ardrone_motor_control.c | 433 ++++++++++++++++++ .../ardrone_interface/ardrone_motor_control.h | 79 ++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 8 files changed, 840 insertions(+), 5 deletions(-) create mode 100644 apps/ardrone_interface/Makefile create mode 100644 apps/ardrone_interface/ardrone_interface.c create mode 100644 apps/ardrone_interface/ardrone_motor_control.c create mode 100644 apps/ardrone_interface/ardrone_motor_control.h diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index e2f4fca84f..13c759db2d 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -18,7 +18,7 @@ sh /etc/init.d/rc.sensors # # Start MAVLink # -mavlink -d /dev/ttyS0 -b 57600 & +mavlink start -d /dev/ttyS0 -b 57600 # # Start the commander. diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index d3cf8b5067..2a771cac48 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -18,7 +18,7 @@ sh /etc/init.d/rc.sensors # # Start MAVLink # -mavlink -d /dev/ttyS0 -b 57600 & +mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 # diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/scripts/rc.sensors index 7515c1947e..b6b4c65cb4 100644 --- a/ROMFS/scripts/rc.sensors +++ b/ROMFS/scripts/rc.sensors @@ -8,13 +8,12 @@ # #ms5611 start +#mpu6000 start # # Start the sensor collection task. # -# XXX should be 'sensors start' -# -sensors & +sensors start # # Test sensor functionality diff --git a/apps/ardrone_interface/Makefile b/apps/ardrone_interface/Makefile new file mode 100644 index 0000000000..fea96082f6 --- /dev/null +++ b/apps/ardrone_interface/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build ardrone interface +# + +APPNAME = ardrone_interface +PRIORITY = SCHED_PRIORITY_MAX - 15 +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c new file mode 100644 index 0000000000..f354252757 --- /dev/null +++ b/apps/ardrone_interface/ardrone_interface.c @@ -0,0 +1,281 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ardrone_interface.c + * Implementation of AR.Drone 1.0 / 2.0 motor control interface. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ardrone_motor_control.h" + +__EXPORT int ardrone_interface_main(int argc, char *argv[]); + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int ardrone_interface_task; /**< Handle of deamon task / thread */ + +/** + * Mainloop of ardrone_interface. + */ +int ardrone_interface_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int ardrone_interface_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("ardrone_interface already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + ardrone_interface_task = task_create("ardrone_interface", SCHED_PRIORITY_MAX - 15, 4096, ardrone_interface_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + thread_running = true; + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tardrone_interface is running\n"); + } else { + printf("\tardrone_interface not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int ardrone_interface_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + printf("[ardrone_interface] Control started, taking over motors\n"); + + /* default values for arguments */ + char *ardrone_uart_name = "/dev/ttyS1"; + + /* File descriptors */ + int ardrone_write; + int gpios; + + enum { + CONTROL_MODE_RATES = 0, + CONTROL_MODE_ATTITUDE = 1, + } control_mode = CONTROL_MODE_ATTITUDE; + + char *commandline_usage = "\tusage: ardrone_interface -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n"; + + bool motor_test_mode = false; + + /* read commandline arguments */ + for (int i = 1; i < argc; i++) { + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set + if (argc > i + 1) { + ardrone_uart_name = argv[i + 1]; + } else { + printf(commandline_usage); + return ERROR; + } + + } else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) { + if (argc > i + 1) { + if (strcmp(argv[i + 1], "rates") == 0) { + control_mode = CONTROL_MODE_RATES; + + } else if (strcmp(argv[i + 1], "attitude") == 0) { + control_mode = CONTROL_MODE_ATTITUDE; + + } else { + printf(commandline_usage); + return ERROR; + } + + } else { + printf(commandline_usage); + return ERROR; + } + + } else if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) { + motor_test_mode = true; + } + } + + /* open uarts */ + printf("[ardrone_interface] AR.Drone UART is %s\n", ardrone_uart_name); + ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY); + if (ardrone_write < 0) { + fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + exit(ERROR); + } + + /* initialize motors */ + if (OK != ar_init_motors(ardrone_write, &gpios)) { + close(ardrone_write); + fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + exit(ERROR); + } + + /* Led animation */ + int counter = 0; + int led_counter = 0; + + /* declare and safely initialize all structs */ + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); + + /* subscribe to attitude, motor setpoints and system state */ + int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + + while (!thread_should_exit) { + + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of the actuator controls */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); + // if .. + ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode); + // } else { + // /* Silently lock down motor speeds to zero */ + // ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); + // } + + if (counter % 30 == 0) { + if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); + + if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); + + if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); + + if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); + + if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); + + if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); + + if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); + + if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); + + if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); + + if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); + + if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); + + if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); + + led_counter++; + + if (led_counter == 12) led_counter = 0; + } + + /* run at approximately 50 Hz */ + usleep(20000); + + // This is a hardcore debug code piece to validate + // the motor interface + // uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5}; + // ar_get_motor_packet(motorSpeedBuf, 20, 20, 20, 20); + // write(ardrone_write, motorSpeedBuf, 5); + // usleep(15000); + + counter++; + } + + /* close uarts */ + close(ardrone_write); + ar_multiplexing_deinit(gpios); + + printf("[ardrone_interface] ending now...\n"); + fflush(stdout); + + thread_running = false; + + return OK; +} + diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c new file mode 100644 index 0000000000..ad58939637 --- /dev/null +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -0,0 +1,433 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ardrone_motor_control.c + * Implementation of AR.Drone 1.0 / 2.0 motor control interface + */ + +#include +#include +#include +#include +#include +#include + +#include "ardrone_motor_control.h" + +static const unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2; +static const unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 }; + +typedef union { + uint16_t motor_value; + uint8_t bytes[2]; +} motor_union_t; + +/** + * @brief Generate the 8-byte motor set packet + * + * @return the number of bytes (8) + */ +void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) +{ + motor_buf[0] = 0x20; + motor_buf[1] = 0x00; + motor_buf[2] = 0x00; + motor_buf[3] = 0x00; + motor_buf[4] = 0x00; + /* + * {0x20, 0x00, 0x00, 0x00, 0x00}; + * 0x20 is start sign / motor command + */ + motor_union_t curr_motor; + uint16_t nineBitMask = 0x1FF; + + /* Set motor 1 */ + curr_motor.motor_value = (motor1 & nineBitMask) << 4; + motor_buf[0] |= curr_motor.bytes[1]; + motor_buf[1] |= curr_motor.bytes[0]; + + /* Set motor 2 */ + curr_motor.motor_value = (motor2 & nineBitMask) << 3; + motor_buf[1] |= curr_motor.bytes[1]; + motor_buf[2] |= curr_motor.bytes[0]; + + /* Set motor 3 */ + curr_motor.motor_value = (motor3 & nineBitMask) << 2; + motor_buf[2] |= curr_motor.bytes[1]; + motor_buf[3] |= curr_motor.bytes[0]; + + /* Set motor 4 */ + curr_motor.motor_value = (motor4 & nineBitMask) << 1; + motor_buf[3] |= curr_motor.bytes[1]; + motor_buf[4] |= curr_motor.bytes[0]; +} + +void ar_enable_broadcast(int fd) +{ + ar_select_motor(fd, 0); +} + +int ar_multiplexing_init() +{ + int fd; + + fd = open(GPIO_DEVICE_PATH, 0); + + if (fd < 0) { + printf("GPIO: open fail\n"); + return fd; + } + + /* deactivate all outputs */ + int ret = 0; + ret += ioctl(fd, GPIO_SET, motor_gpios); + + if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) { + printf("GPIO: output set fail\n"); + close(fd); + return -1; + } + + if (ret < 0) { + printf("GPIO: clearing pins fail\n"); + close(fd); + return -1; + } + + return fd; +} + +int ar_multiplexing_deinit(int fd) +{ + if (fd < 0) { + printf("GPIO: no valid descriptor\n"); + return fd; + } + + int ret = 0; + + /* deselect motor 1-4 */ + ret += ioctl(fd, GPIO_SET, motor_gpios); + + if (ret != 0) { + printf("GPIO: clear failed %d times\n", ret); + } + + if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) { + printf("GPIO: input set fail\n"); + return -1; + } + + close(fd); + + return ret; +} + +int ar_select_motor(int fd, uint8_t motor) +{ + int ret = 0; + unsigned long gpioset; + /* + * Four GPIOS: + * GPIO_EXT1 + * GPIO_EXT2 + * GPIO_UART2_CTS + * GPIO_UART2_RTS + */ + + /* select motor 0 to enable broadcast */ + if (motor == 0) { + /* select motor 1-4 */ + ret += ioctl(fd, GPIO_CLEAR, motor_gpios); + + } else { + /* deselect all */ + ret += ioctl(fd, GPIO_SET, motor_gpios); + + /* select reqested motor */ + ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); + + /* deselect all others */ + // gpioset = motor_gpios ^ motor_gpio[motor - 1]; + // ret += ioctl(fd, GPIO_SET, gpioset); + } + + return ret; +} + +int ar_init_motors(int ardrone_uart, int *gpios_pin) +{ + /* Initialize multiplexing */ + *gpios_pin = ar_multiplexing_init(); + + /* Write ARDrone commands on UART2 */ + uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40}; + uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0}; + + /* initialize all motors + * - select one motor at a time + * - configure motor + */ + int i; + int errcounter = 0; + + for (i = 1; i < 5; ++i) { + /* Initialize motors 1-4 */ + initbuf[3] = i; + errcounter += ar_select_motor(*gpios_pin, i); + + write(ardrone_uart, initbuf + 0, 1); + + /* sleep 400 ms */ + usleep(200000); + usleep(200000); + + write(ardrone_uart, initbuf + 1, 1); + /* wait 50 ms */ + usleep(50000); + + write(ardrone_uart, initbuf + 2, 1); + /* wait 50 ms */ + usleep(50000); + + write(ardrone_uart, initbuf + 3, 1); + /* wait 50 ms */ + usleep(50000); + + write(ardrone_uart, initbuf + 4, 1); + /* wait 50 ms */ + usleep(50000); + + /* enable multicast */ + write(ardrone_uart, multicastbuf + 0, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 1, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 2, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 3, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 4, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 5, 1); + /* wait 5 ms */ + usleep(50000); + } + + /* start the multicast part */ + errcounter += ar_select_motor(*gpios_pin, 0); + + if (errcounter != 0) { + fprintf(stderr, "[ar motors] init sequence incomplete, failed %d times", -errcounter); + fflush(stdout); + } + return errcounter; +} + +/* + * Sets the leds on the motor controllers, 1 turns led on, 0 off. + */ +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) +{ + /* + * 2 bytes are sent. The first 3 bits describe the command: 011 means led control + * the following 4 bits are the red leds for motor 4, 3, 2, 1 + * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1 + * the last bit is unknown. + * + * The packet is therefore: + * 011 rrrr 0000 gggg 0 + */ + uint8_t leds[2]; + leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); + leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); + write(ardrone_uart, leds, 2); +} + +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { + const unsigned int min_motor_interval = 20000; + static uint64_t last_motor_time = 0; + if (hrt_absolute_time() - last_motor_time > min_motor_interval) { + uint8_t buf[5] = {0}; + ar_get_motor_packet(buf, motor1, motor2, motor3, motor4); + int ret; + if ((ret = write(ardrone_fd, buf, sizeof(buf))) > 0) { + return OK; + } else { + return ret; + } + } else { + return -ERROR; + } +} + +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose) { + + float roll_control = actuators->control[0]; + float pitch_control = actuators->control[1]; + float yaw_control = actuators->control[2]; + float motor_thrust = actuators->control[3]; + + unsigned int motor_skip_counter = 0; + + const float min_thrust = 0.02f; /**< 2% minimum thrust */ + const float max_thrust = 1.0f; /**< 100% max thrust */ + const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ + + const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ + const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ + + /* initialize all fields to zero */ + uint16_t motor_pwm[4] = {0}; + float motor_calc[4] = {0}; + + float output_band = 0.0f; + float band_factor = 0.75f; + const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ + float yaw_factor = 1.0f; + + if (motor_thrust <= min_thrust) { + motor_thrust = min_thrust; + output_band = 0.0f; + + } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { + output_band = band_factor * (motor_thrust - min_thrust); + + } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { + output_band = band_factor * startpoint_full_control; + + } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { + output_band = band_factor * (max_thrust - motor_thrust); + } + + if (verbose && motor_skip_counter % 100 == 0) { + printf("1: mot1: %3.1f band: %3.1f r: %3.1f n: %3.1f y: %3.1f\n", (double)motor_thrust, (double)output_band, (double)roll_control, (double)pitch_control, (double)yaw_control); + } + + //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer + + // FRONT (MOTOR 1) + motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); + + // RIGHT (MOTOR 2) + motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); + + // BACK (MOTOR 3) + motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); + + // LEFT (MOTOR 4) + motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); + + // if we are not in the output band + if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band + && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band + && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band + && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { + + yaw_factor = 0.5f; + // FRONT (MOTOR 1) + motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor); + + // RIGHT (MOTOR 2) + motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor); + + // BACK (MOTOR 3) + motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor); + + // LEFT (MOTOR 4) + motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor); + } + + if (verbose && motor_skip_counter % 100 == 0) { + printf("2: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]); + } + + for (int i = 0; i < 4; i++) { + //check for limits + if (motor_calc[i] < motor_thrust - output_band) { + motor_calc[i] = motor_thrust - output_band; + } + + if (motor_calc[i] > motor_thrust + output_band) { + motor_calc[i] = motor_thrust + output_band; + } + } + + if (verbose && motor_skip_counter % 100 == 0) { + printf("3: band lim: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]); + } + + /* set the motor values */ + + /* scale up from 0..1 to 10..512) */ + motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); + + if (verbose && motor_skip_counter % 100 == 0) { + printf("4: scaled: m1: %d m2: %d m3: %d m4: %d\n", motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + } + + /* Keep motors spinning while armed and prevent overflows */ + + /* Failsafe logic - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; + motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; + motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; + motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; + + /* Failsafe logic - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512; + motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512; + motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512; + motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512; + + /* send motors via UART */ + if (verbose && motor_skip_counter % 100 == 0) printf("5: mot: %3.1f-%i-%i-%i-%i\n\n", (double)motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + + motor_skip_counter++; +} diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h new file mode 100644 index 0000000000..b8dd2abe40 --- /dev/null +++ b/apps/ardrone_interface/ardrone_motor_control.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ardrone_motor_control.h + * Definition of AR.Drone 1.0 / 2.0 motor control interface + */ + +#include +#include + +/** + * Generate the 5-byte motor set packet. + * + * @return the number of bytes (5) + */ +void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); + +/** + * Select a motor in the multiplexing. + */ +int ar_select_motor(int fd, uint8_t motor); + +void ar_enable_broadcast(int fd); + +int ar_multiplexing_init(void); +int ar_multiplexing_deinit(int fd); + +/** + * Write four motor commands to an already initialized port. + * + * Writing 0 stops a motor, values from 1-512 encode the full thrust range. + * on some motor controller firmware revisions a minimum value of 10 is + * required to spin the motors. + */ +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); + +/** + * Initialize the motors. + */ +int ar_init_motors(int ardrone_uart, int *gpios_pin); + +/** + * Set LED pattern. + */ +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); + +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose); diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index da119e14fc..b37bc386da 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -71,6 +71,7 @@ CONFIGURED_APPS += commander #CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors CONFIGURED_APPS += ardrone_control +CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += px4/attitude_estimator_bm CONFIGURED_APPS += fixedwing_control