Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
actuators_ardrone2_raw.c
Go to the documentation of this file.
1 /*
2  * Original Code from:
3  * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
4  *
5  * Adapated for Paparazzi by:
6  * Copyright (C) 2012 Dino Hensen <dino.hensen@gmail.com>
7  *
8  * This file is part of paparazzi.
9  *
10  * paparazzi is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2, or (at your option)
13  * any later version.
14  *
15  * paparazzi is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with paparazzi; see the file COPYING. If not, write to
22  * the Free Software Foundation, 59 Temple Place - Suite 330,
23  * Boston, MA 02111-1307, USA.
24  */
25 
31 #include "subsystems/actuators.h"
32 #include "actuators_ardrone2_raw.h"
33 #include "mcu_periph/gpio.h"
34 #include "led_hw.h"
35 #include "mcu_periph/sys_time.h"
36 #include "navdata.h" // for full_write
37 
38 #include <stdio.h> /* Standard input/output definitions */
39 #include <string.h> /* String function definitions */
40 #include <unistd.h> /* UNIX standard function definitions */
41 #include <fcntl.h> /* File control definitions */
42 #include <errno.h> /* Error number definitions */
43 #include <termios.h> /* POSIX terminal control definitions */
44 #include <stdint.h>
45 
58 #define ARDRONE_GPIO_PORT 0x32524
59 
60 #define ARDRONE_GPIO_PIN_MOTOR1 171
61 #define ARDRONE_GPIO_PIN_MOTOR2 172
62 #define ARDRONE_GPIO_PIN_MOTOR3 173
63 #define ARDRONE_GPIO_PIN_MOTOR4 174
64 
65 #define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP 175
66 #define ARDRONE_GPIO_PIN_IRQ_INPUT 176
67 
69 
70 static inline void actuators_ardrone_reset_flipflop(void)
71 {
74  int32_t stop = sys_time.nb_sec + 2;
75  while (sys_time.nb_sec < stop);
77 }
78 
79 
80 
82 {
83  led_hw_values = 0;
84 
85  //open mot port
86  actuator_ardrone2_raw_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY);
87  if (actuator_ardrone2_raw_fd == -1)
88  {
89  perror("open_port: Unable to open /dev/ttyO0 - ");
90  return;
91  }
92  fcntl(actuator_ardrone2_raw_fd, F_SETFL, 0); //read calls are non blocking
93  fcntl(actuator_ardrone2_raw_fd, F_GETFL, 0);
94 
95  //set port options
96  struct termios options;
97  //Get the current options for the port
98  tcgetattr(actuator_ardrone2_raw_fd, &options);
99  //Set the baud rates to 115200
100  cfsetispeed(&options, B115200);
101  cfsetospeed(&options, B115200);
102 
103  options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
104  options.c_iflag = 0; //clear input options
105  options.c_lflag=0; //clear local options
106  options.c_oflag &= ~OPOST; //clear output options (raw output)
107 
108  //Set the new options for the port
109  tcsetattr(actuator_ardrone2_raw_fd, TCSANOW, &options);
110 
111  //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
114 
115 
116  //all select lines active
125 
126  //configure motors
127  uint8_t reply[256];
128  for(int m=0;m<4;m++) {
130  actuators_ardrone_cmd(0xe0,reply,2);
131  if(reply[0]!=0xe0 || reply[1]!=0x00)
132  {
133  printf("motor%d cmd=0x%02x reply=0x%02x\n",m+1,(int)reply[0],(int)reply[1]);
134  }
135  actuators_ardrone_cmd(m+1,reply,1);
137  }
138 
139  //all select lines active
144 
145  //start multicast
146  actuators_ardrone_cmd(0xa0,reply,1);
147  actuators_ardrone_cmd(0xa0,reply,1);
148  actuators_ardrone_cmd(0xa0,reply,1);
149  actuators_ardrone_cmd(0xa0,reply,1);
150  actuators_ardrone_cmd(0xa0,reply,1);
151 
152  //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0
155 
156  // Left Red, Right Green
158 }
159 
160 int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) {
161  if (full_write(actuator_ardrone2_raw_fd, &cmd, 1) < 0)
162  {
163  perror("actuators_ardrone_cmd: write failed");
164  return -1;
165  }
166  return full_read(actuator_ardrone2_raw_fd, reply, replylen);
167 }
168 
169 #include "autopilot.h"
170 
173 {
174  static bool_t last_motor_on = FALSE;
175 
176  // Reset Flipflop sequence
177  static bool_t reset_flipflop_counter = 0;
178  if (reset_flipflop_counter > 0)
179  {
180  reset_flipflop_counter--;
181 
182  if (reset_flipflop_counter == 10)
183  {
184  // Reset flipflop
187  }
188  else if (reset_flipflop_counter == 1)
189  {
190  // Listen to IRQ again
192  }
193  return;
194  }
195 
196  // If a motor IRQ line is set
198  {
200  {
201  if (last_motor_on)
202  {
203  // Tell paparazzi that one motor has stalled
205  }
206  else
207  {
208  // Toggle Flipflop reset so motors can be re-enabled
209  reset_flipflop_counter = 20;
210  }
211 
212  }
213  }
214  last_motor_on = autopilot_motors_on;
215 
216 }
217 
218 #define BIT_NUMBER(VAL,BIT) (((VAL)>>BIT)&0x03)
219 
220 void actuators_ardrone_led_run(void);
222 {
223  static uint32_t previous_led_hw_values = 0x00;
224  if (previous_led_hw_values != led_hw_values)
225  {
226  previous_led_hw_values = led_hw_values;
228  }
229 }
230 
232 {
234  RunOnceEvery(100,actuators_ardrone_motor_status());
235 }
236 
242 {
243  uint8_t cmd[5];
244  cmd[0] = 0x20 | ((pwm0&0x1ff)>>4);
245  cmd[1] = ((pwm0&0x1ff)<<4) | ((pwm1&0x1ff)>>5);
246  cmd[2] = ((pwm1&0x1ff)<<3) | ((pwm2&0x1ff)>>6);
247  cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7);
248  cmd[4] = ((pwm3&0x1ff)<<1);
250  RunOnceEvery(20,actuators_ardrone_led_run());
251 }
252 
265 {
266  uint8_t cmd[2];
267 
268  led0 &= 0x03;
269  led1 &= 0x03;
270  led2 &= 0x03;
271  led3 &= 0x03;
272 
273  //printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3);
274 
275  cmd[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1);
276  cmd[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0);
277 
279 }
280 
282 {
284 }
unsigned short uint16_t
Definition: types.h:16
void actuators_ardrone_led_run(void)
void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3)
Write LED command cmd = 011rrrr0 000gggg0 (this is ardrone1 format, we need ardrone2 format) ...
static void actuators_ardrone_reset_flipflop(void)
bool_t autopilot_motors_on
Definition: autopilot.c:54
#define MOT_LEDRED
Some architecture independent helper functions for GPIOs.
void actuators_ardrone_init(void)
void gpio_set(uint32_t port, uint16_t pin)
Set a gpio output to high level.
void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint16_t pwm3)
Write motor speed command cmd = 001aaaaa aaaabbbb bbbbbccc ccccccdd ddddddd0.
uint32_t led_hw_values
void actuators_ardrone_commit(void)
int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen)
#define FALSE
Definition: imu_chimu.h:141
#define B115200
Definition: uart_arch.h:45
#define ARDRONE_GPIO_PIN_MOTOR4
Actuator driver for ardrone2-raw version.
void autopilot_set_motors_on(bool_t motors_on)
Definition: autopilot.c:474
int32_t actuators_pwm_values[ACTUATORS_PWM_NB]
Architecture independent timing functions.
#define BIT_NUMBER(VAL, BIT)
unsigned long uint32_t
Definition: types.h:18
Hardware independent API for actuators (servos, motor controllers).
signed long int32_t
Definition: types.h:19
void gpio_clear(uint32_t port, uint16_t pin)
Clear a gpio output to low level.
#define ARDRONE_GPIO_PIN_MOTOR3
#define ARDRONE_GPIO_PORT
unsigned char uint8_t
Definition: types.h:14
static void gpio_setup_output(uint32_t port, uint32_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.h:76
int actuator_ardrone2_raw_fd
Power consumption @ 11V all 4 motors running PWM A 0 0.2 80 1.3 100 1.5 150 2.0 190 2...
#define MOT_LEDGREEN
#define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP
void actuators_ardrone_close(void)
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:69
void actuators_ardrone_motor_status(void)
static void gpio_setup_input(uint32_t port, uint32_t gpios)
Setup one or more pins of the given GPIO port as inputs.
Definition: gpio_arch.h:88
#define ARDRONE_GPIO_PIN_MOTOR1
#define ARDRONE_GPIO_PIN_MOTOR2
uint16_t gpio_get(uint32_t gpioport, uint16_t gpios)
Read a gpio value.
#define ARDRONE_GPIO_PIN_IRQ_INPUT