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, 6 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.