// %flair:license{ // This file is part of the Flair framework distributed under the // CECILL-C License, Version 1.0. // %flair:license} // created: 2013/12/19 // filename: ParrotBldc_impl.cpp // // author: Guillaume Sanahuja // Copyright Heudiasyc UMR UTC/CNRS 7253 // // version: $Id: $ // // purpose: Classe integrant les moteurs ardrone // // thanks to Paparazzi (https://wiki.paparazziuav.org) for the protocol // // /*********************************************************************/ #include "ParrotBldc_impl.h" #include "ParrotBldc.h" #include #include #include #include #include #define MAX_VALUE 0x1ff #define MOT_LEDOFF 0 #define MOT_LEDRED 1 #define MOT_LEDGREEN 2 #define MOT_LEDORANGE 3 #define GPIO_MOTOR1 171 #define GPIO_MOTOR2 172 #define GPIO_MOTOR3 173 #define GPIO_MOTOR4 174 #define GPIO_IRQ_FLIPFLOP 175 #define GPIO_IRQ_INPUT 176 #define GPIO_MB_GREEN_LED 180 #define GPIO_MB_RED_LED 181 #define GPIO_MAGIC 'p' #define GPIO_DIRECTION _IOW(GPIO_MAGIC, 0, GpioDirection_t) #define GPIO_READ _IOWR(GPIO_MAGIC, 1, GpioData_t) #define GPIO_WRITE _IOW(GPIO_MAGIC, 2, GpioData_t) using namespace flair::core; using namespace flair::gui; using namespace flair::actuator; ParrotBldc_impl::ParrotBldc_impl(ParrotBldc* self,SerialPort* serialport) { this->self=self; this->serialport=serialport; fd=open("/dev/gpio",O_RDWR); if (fd <= 0) self->Err("/dev/gpio open error\n"); SetGPIODirection(GPIO_MB_GREEN_LED,GpioMode_t::GpioOutputLow); SetGPIODirection(GPIO_MB_RED_LED,GpioMode_t::GpioOutputLow); SetGPIOValue(GPIO_MB_RED_LED,false); SetGPIOValue(GPIO_MB_GREEN_LED,true); SetGPIODirection(GPIO_MOTOR1,GpioMode_t::GpioOutputLow); SetGPIODirection(GPIO_MOTOR2,GpioMode_t::GpioOutputLow); SetGPIODirection(GPIO_MOTOR3,GpioMode_t::GpioOutputLow); SetGPIODirection(GPIO_MOTOR4,GpioMode_t::GpioOutputLow); SetGPIODirection(GPIO_IRQ_FLIPFLOP,GpioMode_t::GpioOutputLow); SetGPIODirection(GPIO_IRQ_INPUT,GpioMode_t::GpioInput); ResetBldc(); } ParrotBldc_impl::~ParrotBldc_impl() { SetGPIOValue(GPIO_MB_GREEN_LED,false); SetGPIOValue(GPIO_MB_RED_LED,true); SetLeds(MOT_LEDRED,MOT_LEDRED, MOT_LEDRED, MOT_LEDRED); close(fd); } void ParrotBldc_impl::ResetBldc() { SetGPIOValue(GPIO_IRQ_FLIPFLOP,false); usleep(20000); SetGPIOValue(GPIO_IRQ_FLIPFLOP,true); //all select lines inactive SetGPIOValue(GPIO_MOTOR1,false); SetGPIOValue(GPIO_MOTOR2,false); SetGPIOValue(GPIO_MOTOR3,false); SetGPIOValue(GPIO_MOTOR4,false); //configure motors uint8_t reply[121]; for(int i=0; i<4; i++) { SetGPIOValue(GPIO_MOTOR1 + i,true); WriteCmd(0xe0,reply,2); if(reply[1]==0x50) { self->Warn("Init motor %i\n",i); WriteCmd(0x91,reply,121); WriteCmd(0xa1,reply,2); WriteCmd(i+1,reply,1); usleep(200000);//wait before reconfiguring motor, otherwise it does not reply WriteCmd(0xe0,reply,2); } if(reply[1]!=0x00) { self->Err("Init motor error %i\n",i); } WriteCmd(i+1,reply,1); SetGPIOValue(GPIO_MOTOR1+i,false); } //all select lines active SetGPIOValue(GPIO_MOTOR1,true); SetGPIOValue(GPIO_MOTOR2,true); SetGPIOValue(GPIO_MOTOR3,true); SetGPIOValue(GPIO_MOTOR4,true); //start multicast for(int i=0; i<6; i++) WriteCmd(0xa0,reply,1); //reset IRQ flipflop //on error GPIO_IRQ_INPUT reads 1, this code resets GPIO_IRQ_INPUT to 0 SetGPIOValue(GPIO_IRQ_FLIPFLOP,false); SetGPIOValue(GPIO_IRQ_FLIPFLOP,true); usleep(200000);//wait, otherwise leds stay red when motor is stopped SetLeds(MOT_LEDGREEN,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN); is_reseted=true; } void ParrotBldc_impl::SetGPIODirection(int gpio,GpioMode_t mode) { GpioDirection_t dir; dir.gpio = gpio; dir.mode = mode; if(ioctl(fd, GPIO_DIRECTION, &dir)<0) self->Err("ioctl error\n"); } void ParrotBldc_impl::SetGPIOValue(int gpio,bool value) { GpioData_t data; data.gpio = gpio; if(value) { data.value = 1; } else { data.value = 0; } if(ioctl(fd, GPIO_WRITE, &data)<0) self->Err("ioctl error\n"); } void ParrotBldc_impl::SetMotors(float* value) { uint8_t tx[5]; uint8_t rx[5]; uint16_t mot_value[4]; for(int i=0; i<4; i++) mot_value[i]=(uint16_t)(MAX_VALUE*value[i]); if(mot_value[0]!=0 || mot_value[1]!=0 || mot_value[2]!=0 || mot_value[3]!=0) { tx[0] = 0x20 | ((mot_value[0]&MAX_VALUE)>>4); tx[1] = ((mot_value[0]&MAX_VALUE)<<4) | ((mot_value[1]&MAX_VALUE)>>5); tx[2] = ((mot_value[1]&MAX_VALUE)<<3) | ((mot_value[3]&MAX_VALUE)>>6); tx[3] = ((mot_value[3]&MAX_VALUE)<<2) | ((mot_value[2]&MAX_VALUE)>>7); tx[4] = ((mot_value[2]&MAX_VALUE)<<1); ssize_t written=serialport->Write(tx, sizeof(tx)); if(written<0) { self->Err("write error (%s)\n",strerror(-written)); } else if (written != sizeof(tx)) { self->Err("write error %i/%i\n",written,sizeof(tx)); } ssize_t read=serialport->Read(rx, sizeof(rx)); if(read<0) { self->Err("read error (%s)\n",strerror(-read)); } else if (read != sizeof(rx)) { self->Err("read error %i/%i\n",read,sizeof(rx)); } for(int i=0; i<5; i++) { if(rx[i]!=tx[i]) { self->Warn("wrong response\n"); for(int i=0; i<5; i++) Printf("%x %x",tx[i],rx[i]); Printf("\n"); } } is_reseted=false; } else { if(is_reseted==false) ResetBldc(); } } int ParrotBldc_impl::WriteCmd(uint8_t cmd, uint8_t *reply, int replylen) { ssize_t written=serialport->Write(&cmd, 1); if(written<0) { self->Err("write error (%s)\n",strerror(-written)); } else if (written != 1) { self->Err("write error %i/1\n",written); } return serialport->Read(reply, replylen); } void ParrotBldc_impl::SetLeds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3) { uint8_t tx[2]; led0 &= 0x03; led1 &= 0x03; led2 &= 0x03; led3 &= 0x03; //printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3); tx[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1); tx[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0); ssize_t written=serialport->Write(tx, sizeof(tx)); if(written<0) { self->Err("write error (%s)\n",strerror(-written)); } else if (written != sizeof(tx)) { self->Err("write error %i/%i\n",written,sizeof(tx)); } serialport->Read(tx, sizeof(tx));//flush }