source: flair-src/trunk/lib/FlairSensorActuator/src/ParrotBldc_impl.cpp@ 3

Last change on this file since 3 was 3, checked in by Sanahuja Guillaume, 9 years ago

sensoractuator
File size: 6.8 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5// created: 2013/12/19
6// filename: ParrotBldc_impl.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: Classe integrant les moteurs ardrone
14//
15// thanks to Paparazzi (https://wiki.paparazziuav.org) for the protocol
16//
17//
18/*********************************************************************/
19#include "ParrotBldc_impl.h"
20#include "ParrotBldc.h"
21#include <SerialPort.h>
22#include <string.h>
23#include <unistd.h>
24#include <fcntl.h>
25#include <sys/ioctl.h>
26
27#define MAX_VALUE 0x1ff
28
29#define MOT_LEDOFF 0
30#define MOT_LEDRED 1
31#define MOT_LEDGREEN 2
32#define MOT_LEDORANGE 3
33
34#define GPIO_MOTOR1 171
35#define GPIO_MOTOR2 172
36#define GPIO_MOTOR3 173
37#define GPIO_MOTOR4 174
38
39#define GPIO_IRQ_FLIPFLOP 175
40#define GPIO_IRQ_INPUT 176
41#define GPIO_MB_GREEN_LED 180
42#define GPIO_MB_RED_LED 181
43
44#define GPIO_MAGIC 'p'
45#define GPIO_DIRECTION _IOW(GPIO_MAGIC, 0, GpioDirection_t)
46#define GPIO_READ _IOWR(GPIO_MAGIC, 1, GpioData_t)
47#define GPIO_WRITE _IOW(GPIO_MAGIC, 2, GpioData_t)
48
49using namespace flair::core;
50using namespace flair::gui;
51using namespace flair::actuator;
52
53ParrotBldc_impl::ParrotBldc_impl(ParrotBldc* self,SerialPort* serialport) {
54 this->self=self;
55 this->serialport=serialport;
56
57 fd=open("/dev/gpio",O_RDWR);
58 if (fd <= 0) self->Err("/dev/gpio open error\n");
59
60 SetGPIODirection(GPIO_MB_GREEN_LED,GpioMode_t::GpioOutputLow);
61 SetGPIODirection(GPIO_MB_RED_LED,GpioMode_t::GpioOutputLow);
62 SetGPIOValue(GPIO_MB_RED_LED,false);
63 SetGPIOValue(GPIO_MB_GREEN_LED,true);
64
65 SetGPIODirection(GPIO_MOTOR1,GpioMode_t::GpioOutputLow);
66 SetGPIODirection(GPIO_MOTOR2,GpioMode_t::GpioOutputLow);
67 SetGPIODirection(GPIO_MOTOR3,GpioMode_t::GpioOutputLow);
68 SetGPIODirection(GPIO_MOTOR4,GpioMode_t::GpioOutputLow);
69 SetGPIODirection(GPIO_IRQ_FLIPFLOP,GpioMode_t::GpioOutputLow);
70 SetGPIODirection(GPIO_IRQ_INPUT,GpioMode_t::GpioInput);
71
72 ResetBldc();
73}
74
75ParrotBldc_impl::~ParrotBldc_impl() {
76 SetGPIOValue(GPIO_MB_GREEN_LED,false);
77 SetGPIOValue(GPIO_MB_RED_LED,true);
78 SetLeds(MOT_LEDRED,MOT_LEDRED, MOT_LEDRED, MOT_LEDRED);
79
80 close(fd);
81}
82
83void ParrotBldc_impl::ResetBldc() {
84 SetGPIOValue(GPIO_IRQ_FLIPFLOP,false);
85 usleep(20000);
86 SetGPIOValue(GPIO_IRQ_FLIPFLOP,true);
87
88 //all select lines inactive
89 SetGPIOValue(GPIO_MOTOR1,false);
90 SetGPIOValue(GPIO_MOTOR2,false);
91 SetGPIOValue(GPIO_MOTOR3,false);
92 SetGPIOValue(GPIO_MOTOR4,false);
93
94 //configure motors
95 uint8_t reply[121];
96 for(int i=0; i<4; i++) {
97 SetGPIOValue(GPIO_MOTOR1 + i,true);
98
99 WriteCmd(0xe0,reply,2);
100 if(reply[1]==0x50) {
101 self->Warn("Init motor %i\n",i);
102 WriteCmd(0x91,reply,121);
103 WriteCmd(0xa1,reply,2);
104 WriteCmd(i+1,reply,1);
105 usleep(200000);//wait before reconfiguring motor, otherwise it does not reply
106 WriteCmd(0xe0,reply,2);
107 }
108 if(reply[1]!=0x00) {
109 self->Err("Init motor error %i\n",i);
110 }
111
112 WriteCmd(i+1,reply,1);
113 SetGPIOValue(GPIO_MOTOR1+i,false);
114 }
115
116 //all select lines active
117 SetGPIOValue(GPIO_MOTOR1,true);
118 SetGPIOValue(GPIO_MOTOR2,true);
119 SetGPIOValue(GPIO_MOTOR3,true);
120 SetGPIOValue(GPIO_MOTOR4,true);
121
122 //start multicast
123 for(int i=0; i<6; i++) WriteCmd(0xa0,reply,1);
124
125 //reset IRQ flipflop
126 //on error GPIO_IRQ_INPUT reads 1, this code resets GPIO_IRQ_INPUT to 0
127 SetGPIOValue(GPIO_IRQ_FLIPFLOP,false);
128 SetGPIOValue(GPIO_IRQ_FLIPFLOP,true);
129
130 usleep(200000);//wait, otherwise leds stay red when motor is stopped
131 SetLeds(MOT_LEDGREEN,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN);
132
133 is_reseted=true;
134}
135
136void ParrotBldc_impl::SetGPIODirection(int gpio,GpioMode_t mode) {
137 GpioDirection_t dir;
138
139 dir.gpio = gpio;
140 dir.mode = mode;
141 if(ioctl(fd, GPIO_DIRECTION, &dir)<0) self->Err("ioctl error\n");
142}
143
144void ParrotBldc_impl::SetGPIOValue(int gpio,bool value) {
145 GpioData_t data;
146
147 data.gpio = gpio;
148 if(value) {
149 data.value = 1;
150 } else {
151 data.value = 0;
152 }
153 if(ioctl(fd, GPIO_WRITE, &data)<0) self->Err("ioctl error\n");
154}
155
156void ParrotBldc_impl::SetMotors(float* value) {
157 uint8_t tx[5];
158 uint8_t rx[5];
159 uint16_t mot_value[4];
160
161 for(int i=0; i<4; i++) mot_value[i]=(uint16_t)(MAX_VALUE*value[i]);
162
163 if(mot_value[0]!=0 || mot_value[1]!=0 || mot_value[2]!=0 || mot_value[3]!=0) {
164
165 tx[0] = 0x20 | ((mot_value[0]&MAX_VALUE)>>4);
166 tx[1] = ((mot_value[0]&MAX_VALUE)<<4) | ((mot_value[1]&MAX_VALUE)>>5);
167 tx[2] = ((mot_value[1]&MAX_VALUE)<<3) | ((mot_value[3]&MAX_VALUE)>>6);
168 tx[3] = ((mot_value[3]&MAX_VALUE)<<2) | ((mot_value[2]&MAX_VALUE)>>7);
169 tx[4] = ((mot_value[2]&MAX_VALUE)<<1);
170 ssize_t written=serialport->Write(tx, sizeof(tx));
171
172 if(written<0) {
173 self->Err("write error (%s)\n",strerror(-written));
174 } else if (written != sizeof(tx)) {
175 self->Err("write error %i/%i\n",written,sizeof(tx));
176 }
177
178 ssize_t read=serialport->Read(rx, sizeof(rx));
179 if(read<0) {
180 self->Err("read error (%s)\n",strerror(-read));
181 } else if (read != sizeof(rx)) {
182 self->Err("read error %i/%i\n",read,sizeof(rx));
183 }
184 for(int i=0; i<5; i++) {
185 if(rx[i]!=tx[i]) {
186 self->Warn("wrong response\n");
187 for(int i=0; i<5; i++) Printf("%x %x",tx[i],rx[i]);
188 Printf("\n");
189 }
190 }
191
192 is_reseted=false;
193
194 } else {
195 if(is_reseted==false) ResetBldc();
196 }
197}
198
199int ParrotBldc_impl::WriteCmd(uint8_t cmd, uint8_t *reply, int replylen) {
200 ssize_t written=serialport->Write(&cmd, 1);
201 if(written<0) {
202 self->Err("write error (%s)\n",strerror(-written));
203 } else if (written != 1) {
204 self->Err("write error %i/1\n",written);
205 }
206
207 return serialport->Read(reply, replylen);
208}
209
210void ParrotBldc_impl::SetLeds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3) {
211 uint8_t tx[2];
212
213 led0 &= 0x03;
214 led1 &= 0x03;
215 led2 &= 0x03;
216 led3 &= 0x03;
217
218 //printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3);
219
220 tx[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1);
221 tx[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0);
222
223 ssize_t written=serialport->Write(tx, sizeof(tx));
224 if(written<0) {
225 self->Err("write error (%s)\n",strerror(-written));
226 } else if (written != sizeof(tx)) {
227 self->Err("write error %i/%i\n",written,sizeof(tx));
228 }
229
230 serialport->Read(tx, sizeof(tx));//flush
231}
Note: See TracBrowser for help on using the repository browser.