Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 "gpio_ardrone.h"
34 
35 #include <stdio.h> /* Standard input/output definitions */
36 #include <string.h> /* String function definitions */
37 #include <unistd.h> /* UNIX standard function definitions */
38 #include <fcntl.h> /* File control definitions */
39 #include <errno.h> /* Error number definitions */
40 #include <termios.h> /* POSIX terminal control definitions */
41 #include <stdint.h>
42 
53 int mot_fd;
56 {
57  //open mot port
58  mot_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY);
59  if (mot_fd == -1)
60  {
61  perror("open_port: Unable to open /dev/ttyO0 - ");
62  return;
63  }
64  fcntl(mot_fd, F_SETFL, 0); //read calls are non blocking
65  fcntl(mot_fd, F_GETFL, 0);
66 
67  //set port options
68  struct termios options;
69  //Get the current options for the port
70  tcgetattr(mot_fd, &options);
71  //Set the baud rates to 115200
72  cfsetispeed(&options, B115200);
73  cfsetospeed(&options, B115200);
74 
75  options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
76  options.c_iflag = 0; //clear input options
77  options.c_lflag=0; //clear local options
78  options.c_oflag &= ~OPOST; //clear output options (raw output)
79 
80  //Set the new options for the port
81  tcsetattr(mot_fd, TCSANOW, &options);
82 
83  //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
84  gpio_set(106,-1);
85  gpio_set(107,0);
86  gpio_set(107,1);
87 
88  //all select lines inactive
89  gpio_set(68,1);
90  gpio_set(69,1);
91  gpio_set(70,1);
92  gpio_set(71,1);
93 
94  //configure motors
95  uint8_t reply[256];
96  for(int m=0;m<4;m++) {
97  gpio_set(68+m,-1);
98  actuators_ardrone_cmd(0xe0,reply,2);
99  if(reply[0]!=0xe0 || reply[1]!=0x00)
100  {
101  printf("motor%d cmd=0x%02x reply=0x%02x\n",m+1,(int)reply[0],(int)reply[1]);
102  }
103  actuators_ardrone_cmd(m+1,reply,1);
104  gpio_set(68+m,1);
105  }
106 
107  //all select lines active
108  gpio_set(68,-1);
109  gpio_set(69,-1);
110  gpio_set(70,-1);
111  gpio_set(71,-1);
112 
113  //start multicast
114  actuators_ardrone_cmd(0xa0,reply,1);
115  actuators_ardrone_cmd(0xa0,reply,1);
116  actuators_ardrone_cmd(0xa0,reply,1);
117  actuators_ardrone_cmd(0xa0,reply,1);
118  actuators_ardrone_cmd(0xa0,reply,1);
119 
120  //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
121  gpio_set(106,-1);
122  gpio_set(107,0);
123  gpio_set(107,1);
124 
125  //all leds green
126 // actuators_ardrone_set_leds(MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN);
127 }
128 
129 int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) {
130  write(mot_fd, &cmd, 1);
131  return read(mot_fd, reply, replylen);
132 }
133 
135 {
137 }
138 
144 {
145  uint8_t cmd[5];
146  cmd[0] = 0x20 | ((pwm0&0x1ff)>>4);
147  cmd[1] = ((pwm0&0x1ff)<<4) | ((pwm1&0x1ff)>>5);
148  cmd[2] = ((pwm1&0x1ff)<<3) | ((pwm2&0x1ff)>>6);
149  cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7);
150  cmd[4] = ((pwm3&0x1ff)<<1);
151  write(mot_fd, cmd, 5);
152 }
153 
159 {
160  uint8_t cmd[2];
161  cmd[0]=0x60 | ((led0&3)<<3) | ((led1&3)<<1) | ((led2&3)>>1);
162  cmd[1]=((led2&3)<<7) | ((led3&3)<<5);
163  write(mot_fd, cmd, 2);
164 }
165 
167 {
168  close(mot_fd);
169 }
unsigned short uint16_t
Definition: types.h:16
void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3)
Write LED command cmd = 011grgrg rgrxxxxx (this is ardrone1 format, we need ardrone2 format) ...
void actuators_ardrone_init(void)
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.
void actuators_ardrone_commit(void)
int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen)
#define B115200
Definition: uart.h:49
Actuator driver for ardrone2-raw version.
int32_t actuators_pwm_values[ACTUATORS_PWM_NB]
ardrone GPIO driver
Hardware independent API for actuators (servos, motor controllers).
int gpio_set(int nr, int val)
Definition: gpio_ardrone.c:32
unsigned char uint8_t
Definition: types.h:14
int mot_fd
Power consumption @ 11V all 4 motors running PWM A 0 0.2 80 1.3 100 1.5 150 2.0 190 2...
void actuators_ardrone_close(void)