Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
31 #include "subsystems/actuators.h"
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 
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_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY);
87  if (actuator_ardrone2_fd == -1) {
88  perror("open_port: Unable to open /dev/ttyO0 - ");
89  return;
90  }
91  fcntl(actuator_ardrone2_fd, F_SETFL, 0); //read calls are non blocking
92  fcntl(actuator_ardrone2_fd, F_GETFL, 0);
93 
94  //set port options
95  struct termios options;
96  //Get the current options for the port
97  tcgetattr(actuator_ardrone2_fd, &options);
98  //Set the baud rates to 115200
99  cfsetispeed(&options, B115200);
100  cfsetospeed(&options, B115200);
101 
102  options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
103  options.c_iflag = 0; //clear input options
104  options.c_lflag = 0; //clear local options
105  options.c_oflag &= ~OPOST; //clear output options (raw output)
106 
107  //Set the new options for the port
108  tcsetattr(actuator_ardrone2_fd, TCSANOW, &options);
109 
110  //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
113 
114 
115  //all select lines active
124 
125  //configure motors
126  uint8_t reply[256];
127  for (int m = 0; m < 4; m++) {
129  actuators_ardrone_cmd(0xe0, reply, 2);
130  if (reply[0] != 0xe0 || reply[1] != 0x00) {
131  printf("motor%d cmd=0x%02x reply=0x%02x\n", m + 1, (int)reply[0], (int)reply[1]);
132  }
133  actuators_ardrone_cmd(m + 1, reply, 1);
135  }
136 
137  //all select lines active
142 
143  //start multicast
144  actuators_ardrone_cmd(0xa0, reply, 1);
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 
150  //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0
153 
154  // Left Red, Right Green
156 }
157 
158 int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen)
159 {
160  if (full_write(actuator_ardrone2_fd, &cmd, 1) < 0) {
161  perror("actuators_ardrone_cmd: write failed");
162  return -1;
163  }
164  return full_read(actuator_ardrone2_fd, reply, replylen);
165 }
166 
167 #include "autopilot.h"
168 
171 {
172  static bool last_motor_on = false;
173 
174  // Reset Flipflop sequence
175  static uint8_t reset_flipflop_counter = 0;
176  if (reset_flipflop_counter > 0) {
177  reset_flipflop_counter--;
178 
179  if (reset_flipflop_counter == 10) {
180  // Reset flipflop
183  } else if (reset_flipflop_counter == 1) {
184  // Listen to IRQ again
186  }
187  return;
188  }
189 
190  // If a motor IRQ line is set
192  if (autopilot_get_motors_on()) {
193  if (last_motor_on) {
194  // Tell paparazzi that one motor has stalled
196  } else {
197  // Toggle Flipflop reset so motors can be re-enabled
198  reset_flipflop_counter = 20;
199  }
200 
201  }
202  }
203  last_motor_on = autopilot_get_motors_on();
204 
205 }
206 
207 #define BIT_NUMBER(VAL,BIT) (((VAL)>>BIT)&0x03)
208 
209 void actuators_ardrone_led_run(void);
211 {
212  static uint32_t previous_led_hw_values = 0x00;
213  if (previous_led_hw_values != led_hw_values) {
214  previous_led_hw_values = led_hw_values;
217  }
218 }
219 
221 {
224  RunOnceEvery(100, actuators_ardrone_motor_status());
225 }
226 
232 {
233  uint8_t cmd[5];
234  cmd[0] = 0x20 | ((pwm0 & 0x1ff) >> 4);
235  cmd[1] = ((pwm0 & 0x1ff) << 4) | ((pwm1 & 0x1ff) >> 5);
236  cmd[2] = ((pwm1 & 0x1ff) << 3) | ((pwm2 & 0x1ff) >> 6);
237  cmd[3] = ((pwm2 & 0x1ff) << 2) | ((pwm3 & 0x1ff) >> 7);
238  cmd[4] = ((pwm3 & 0x1ff) << 1);
240  RunOnceEvery(20, actuators_ardrone_led_run());
241 }
242 
255 {
256  uint8_t cmd[2];
257 
258  led0 &= 0x03;
259  led1 &= 0x03;
260  led2 &= 0x03;
261  led3 &= 0x03;
262 
263  //printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3);
264 
265  cmd[0] = 0x60 | ((led0 & 1) << 4) | ((led1 & 1) << 3) | ((led2 & 1) << 2) | ((led3 & 1) << 1);
266  cmd[1] = ((led0 & 2) << 3) | ((led1 & 2) << 2) | ((led2 & 2) << 1) | ((led3 & 2) << 0);
267 
269 }
270 
272 {
273  close(actuator_ardrone2_fd);
274 }
unsigned short uint16_t
Definition: types.h:16
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
static void gpio_clear(ioportid_t port, uint16_t pin)
Clear a gpio output to low level.
Definition: gpio_arch.h:108
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:212
#define B115200
Definition: uart_arch.h:48
Some architecture independent helper functions for GPIOs.
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
uint32_t led_hw_values
Definition: actuators.c:68
#define FALSE
Definition: std.h:5
#define ARDRONE_GPIO_PIN_MOTOR2
Definition: actuators.c:61
void actuators_ardrone_commit(void)
Definition: actuators.c:220
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:254
Hardware independent API for actuators (servos, motor controllers).
Architecture independent timing functions.
#define ARDRONE_GPIO_PIN_IRQ_INPUT
Definition: actuators.c:66
#define MOT_LEDGREEN
Definition: actuators.h:48
#define ARDRONE_GPIO_PIN_MOTOR4
Definition: actuators.c:63
static void actuators_ardrone_reset_flipflop(void)
Definition: actuators.c:70
unsigned long uint32_t
Definition: types.h:18
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:231
#define MOT_LEDRED
Definition: actuators.h:47
void actuators_ardrone_motor_status(void)
Definition: actuators.c:170
signed long int32_t
Definition: types.h:19
#define ARDRONE_GPIO_PIN_MOTOR3
Definition: actuators.c:62
Core autopilot interface common to all firmwares.
static uint8_t gpio_get(ioportid_t port, uint16_t pin)
Get level of a gpio.
Definition: gpio_arch.h:88
unsigned char uint8_t
Definition: types.h:14
#define BIT_NUMBER(VAL, BIT)
Definition: actuators.c:207
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
#define ARDRONE_GPIO_PORT
Definition: actuators.c:58
void 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:200
Actuator driver for ardrone2-raw version.
void actuators_ardrone_close(void)
Definition: actuators.c:271
void actuators_ardrone_led_run(void)
Definition: actuators.c:210
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
int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen)
Definition: actuators.c:158
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_arch.h:98
void actuators_ardrone_init(void)
Definition: actuators.c:81
#define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP
Definition: actuators.c:65
int32_t actuators_pwm_values[ACTUATORS_PWM_NB]
#define ARDRONE_GPIO_PIN_MOTOR1
Definition: actuators.c:60