Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
actuators.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 
32 #include "actuators.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 
70 
71 static inline void actuators_ardrone_reset_flipflop(void)
72 {
75  uint32_t stop = sys_time.nb_sec + 2;
76  while (sys_time.nb_sec < stop);
78 }
79 
80 
81 
83 {
84  led_hw_values = 0;
85 
86  //open mot port
87  actuator_ardrone2_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY);
88  if (actuator_ardrone2_fd == -1) {
89  perror("open_port: Unable to open /dev/ttyO0 - ");
90  return;
91  }
92  fcntl(actuator_ardrone2_fd, F_SETFL, 0); //read calls are non blocking
93  fcntl(actuator_ardrone2_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_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_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  printf("motor%d cmd=0x%02x reply=0x%02x\n", m + 1, (int)reply[0], (int)reply[1]);
133  }
134  actuators_ardrone_cmd(m + 1, reply, 1);
136  }
137 
138  //all select lines active
143 
144  //start multicast
145  actuators_ardrone_cmd(0xa0, reply, 1);
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 
151  //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0
154 
155  // Left Red, Right Green
157 }
158 
159 int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen)
160 {
161  if (full_write(actuator_ardrone2_fd, &cmd, 1) < 0) {
162  perror("actuators_ardrone_cmd: write failed");
163  return -1;
164  }
165  return full_read(actuator_ardrone2_fd, reply, replylen);
166 }
167 
168 #include "autopilot.h"
169 
172 {
173  static bool last_motor_on = false;
174 
175  // Reset Flipflop sequence
176  static uint8_t reset_flipflop_counter = 0;
177  if (reset_flipflop_counter > 0) {
178  reset_flipflop_counter--;
179 
180  if (reset_flipflop_counter == 10) {
181  // Reset flipflop
184  } else if (reset_flipflop_counter == 1) {
185  // Listen to IRQ again
187  }
188  return;
189  }
190 
191  // If a motor IRQ line is set
193  if (autopilot_get_motors_on()) {
194  if (last_motor_on) {
195  // Tell paparazzi that one motor has stalled
197  } else {
198  // Toggle Flipflop reset so motors can be re-enabled
199  reset_flipflop_counter = 20;
200  }
201 
202  }
203  }
204  last_motor_on = autopilot_get_motors_on();
205 
206 }
207 
208 #define BIT_NUMBER(VAL,BIT) (((VAL)>>BIT)&0x03)
209 
210 void actuators_ardrone_led_run(void);
212 {
213  static uint32_t previous_led_hw_values = 0x00;
214  if (previous_led_hw_values != led_hw_values) {
215  previous_led_hw_values = led_hw_values;
218  }
219 }
220 
222 {
225  RunOnceEvery(100, actuators_ardrone_motor_status());
226 }
227 
233 {
234  uint8_t cmd[5];
235  cmd[0] = 0x20 | ((pwm0 & 0x1ff) >> 4);
236  cmd[1] = ((pwm0 & 0x1ff) << 4) | ((pwm1 & 0x1ff) >> 5);
237  cmd[2] = ((pwm1 & 0x1ff) << 3) | ((pwm2 & 0x1ff) >> 6);
238  cmd[3] = ((pwm2 & 0x1ff) << 2) | ((pwm3 & 0x1ff) >> 7);
239  cmd[4] = ((pwm3 & 0x1ff) << 1);
241  RunOnceEvery(20, actuators_ardrone_led_run());
242 }
243 
256 {
257  uint8_t cmd[2];
258 
259  led0 &= 0x03;
260  led1 &= 0x03;
261  led2 &= 0x03;
262  led3 &= 0x03;
263 
264  //printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3);
265 
266  cmd[0] = 0x60 | ((led0 & 1) << 4) | ((led1 & 1) << 3) | ((led2 & 1) << 2) | ((led3 & 1) << 1);
267  cmd[1] = ((led0 & 2) << 3) | ((led1 & 2) << 2) | ((led2 & 2) << 1) | ((led3 & 2) << 0);
268 
270 }
271 
273 {
274  close(actuator_ardrone2_fd);
275 }
bool autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly FIXME is i...
Definition: autopilot.c:245
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:290
Core autopilot interface common to all firmwares.
void actuators_ardrone_close(void)
Definition: actuators.c:272
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)
Definition: actuators.c:255
void actuators_ardrone_commit(void)
Definition: actuators.c:221
#define ARDRONE_GPIO_PIN_IRQ_INPUT
Definition: actuators.c:66
uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB]
Definition: actuators.c:69
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.
Definition: actuators.c:232
int actuator_ardrone2_fd
Power consumption @ 11V all 4 motors running PWM A 0 0.2 80 1.3 100 1.5 150 2.0 190 2....
Definition: actuators.c:56
#define ARDRONE_GPIO_PIN_MOTOR1
Definition: actuators.c:60
void actuators_ardrone_init(void)
Definition: actuators.c:82
uint32_t led_hw_values
Definition: actuators.c:68
static void actuators_ardrone_reset_flipflop(void)
Definition: actuators.c:71
#define ARDRONE_GPIO_PORT
Definition: actuators.c:58
void actuators_ardrone_led_run(void)
Definition: actuators.c:211
int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen)
Definition: actuators.c:159
#define ARDRONE_GPIO_PIN_MOTOR2
Definition: actuators.c:61
#define ARDRONE_GPIO_PIN_MOTOR4
Definition: actuators.c:63
#define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP
Definition: actuators.c:65
#define ARDRONE_GPIO_PIN_MOTOR3
Definition: actuators.c:62
#define BIT_NUMBER(VAL, BIT)
Definition: actuators.c:208
void actuators_ardrone_motor_status(void)
Definition: actuators.c:171
#define MOT_LEDGREEN
Definition: actuators.h:48
#define ACTUATORS_ARDRONE_NB
Definition: actuators.h:37
#define MOT_LEDRED
Definition: actuators.h:47
void gpio_setup_output(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.c:33
void gpio_setup_input(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as inputs.
Definition: gpio_arch.c:40
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_arch.h:98
static void gpio_clear(ioportid_t port, uint16_t pin)
Clear a gpio output to low level.
Definition: gpio_arch.h:108
static uint8_t gpio_get(ioportid_t port, uint16_t pin)
Get level of a gpio.
Definition: gpio_arch.h:88
#define B115200
Definition: uart_arch.h:48
Some architecture independent helper functions for GPIOs.
Hardware independent API for actuators (servos, motor controllers).
ssize_t full_write(int fd, const uint8_t *buf, size_t count)
Write to fd even while being interrupted.
Definition: navdata.c:119
ssize_t full_read(int fd, uint8_t *buf, size_t count)
Read from fd even while being interrupted.
Definition: navdata.c:139
ardrone2 navdata aquisition driver.
Architecture independent timing functions.
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98