| 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 |
|
---|
| 49 | using namespace flair::core;
|
---|
| 50 | using namespace flair::gui;
|
---|
| 51 | using namespace flair::actuator;
|
---|
| 52 |
|
---|
| 53 | ParrotBldc_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 |
|
---|
| 75 | ParrotBldc_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 |
|
---|
| 83 | void 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 |
|
---|
| 136 | void 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 |
|
---|
| 144 | void 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 |
|
---|
| 156 | void 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 |
|
---|
| 199 | int 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 |
|
---|
| 210 | void 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 | }
|
---|