Changeset 15 in flair-src for trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp
- Timestamp:
- Apr 8, 2016, 3:40:57 PM (9 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp
r3 r15 38 38 using namespace flair::sensor; 39 39 40 Gx3_25_imu_impl::Gx3_25_imu_impl(Gx3_25_imu* self,string name,SerialPort *serialport,Gx3_25_imu::Command_t command,GroupBox* setupgroupbox) { 41 int err = 0; 42 43 this->self=self; 44 this->command=(uint8_t)command; 45 this->setupgroupbox=setupgroupbox; 46 this->serialport=serialport; 47 48 ahrsData=new AhrsData((Imu*)self); 49 50 //station sol 51 button_bias=new PushButton(setupgroupbox->NewRow(),"gyros bias"); 52 data_rate=new SpinBox(setupgroupbox->NewRow(),"data rate (Hz):",1,500,1,200); 53 data_rate_label=new Label(setupgroupbox->LastRowLastCol(),"data_rate"); 54 gyro_acc_size=new SpinBox(setupgroupbox->NewRow(),"gyro and acc filter win size:",1,32,1,15); 55 mag_size=new SpinBox(setupgroupbox->LastRowLastCol(),"mag filter win size:",1,32,1,17); 56 up_comp=new SpinBox(setupgroupbox->NewRow(),"up compensation (s):",1,1000,1,10); 57 north_comp=new SpinBox(setupgroupbox->LastRowLastCol(),"north compensation (s):",1,1000,1,10); 58 coning=new CheckBox(setupgroupbox->NewRow(),"enable Coning&Sculling:",true); 59 disable_magn=new CheckBox(setupgroupbox->LastRowLastCol(),"disable magnetometer:",true); 60 disable_north_comp=new CheckBox(setupgroupbox->NewRow(),"disable magnetic north compensation:",false); 61 disable_grav_comp=new CheckBox(setupgroupbox->NewRow(),"disable gravity compensation:",false); 62 } 63 64 Gx3_25_imu_impl::~Gx3_25_imu_impl() { 65 } 40 Gx3_25_imu_impl::Gx3_25_imu_impl(Gx3_25_imu *self, string name, 41 SerialPort *serialport, 42 Gx3_25_imu::Command_t command, 43 GroupBox *setupgroupbox) { 44 int err = 0; 45 46 this->self = self; 47 this->command = (uint8_t)command; 48 this->setupgroupbox = setupgroupbox; 49 this->serialport = serialport; 50 51 ahrsData = new AhrsData((Imu *)self); 52 53 // station sol 54 button_bias = new PushButton(setupgroupbox->NewRow(), "gyros bias"); 55 data_rate = 56 new SpinBox(setupgroupbox->NewRow(), "data rate (Hz):", 1, 500, 1, 200); 57 data_rate_label = new Label(setupgroupbox->LastRowLastCol(), "data_rate"); 58 gyro_acc_size = new SpinBox(setupgroupbox->NewRow(), 59 "gyro and acc filter win size:", 1, 32, 1, 15); 60 mag_size = new SpinBox(setupgroupbox->LastRowLastCol(), 61 "mag filter win size:", 1, 32, 1, 17); 62 up_comp = new SpinBox(setupgroupbox->NewRow(), "up compensation (s):", 1, 63 1000, 1, 10); 64 north_comp = new SpinBox(setupgroupbox->LastRowLastCol(), 65 "north compensation (s):", 1, 1000, 1, 10); 66 coning = 67 new CheckBox(setupgroupbox->NewRow(), "enable Coning&Sculling:", true); 68 disable_magn = new CheckBox(setupgroupbox->LastRowLastCol(), 69 "disable magnetometer:", true); 70 disable_north_comp = new CheckBox( 71 setupgroupbox->NewRow(), "disable magnetic north compensation:", false); 72 disable_grav_comp = new CheckBox(setupgroupbox->NewRow(), 73 "disable gravity compensation:", false); 74 } 75 76 Gx3_25_imu_impl::~Gx3_25_imu_impl() {} 66 77 67 78 void Gx3_25_imu_impl::Run(void) { 68 Time imuTime; 69 ImuData* imuData; 70 self->GetDatas(&imuData); 71 72 self->WarnUponSwitches(true); 73 74 //reset IMU to be sure it is at 115200 75 Printf("Gx3_25_imu::Reset IMU at 921600\n"); 76 serialport->SetBaudrate(921600); 77 DeviceReset(); 78 self->Thread::SleepMS(100); 79 serialport->FlushInput(); 80 serialport->SetBaudrate(115200); 81 82 SetBaudrate(921600); 83 Printf("Gx3_25_imu firmware number: %i\n",FirmwareNumber()); 84 PrintModelInfo(); 85 SamplingSettings(); 86 GyrosBias(); 87 //RealignUpNorth(true,true); 88 89 //on provoque les 9 ValueChanged 90 for(int i=0; i<9; i++) { 91 if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true 92 || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) { 93 if(setupgroupbox->isEnabled()==true) SamplingSettings(); 94 } 95 } 96 97 //periode a passer an argument (reglable) 98 //ou plutot laisser la periode geree par le centrale (polling) 99 //self->SetPeriodMS(2); 100 SetContinuousMode(command); 101 102 //_printf("firmware version: %i\n",get_firmware_number()); 103 104 while(!self->ToBeStopped()) { 105 if(command==Gx3_25_imu::EulerAnglesAndAngularRates) { 106 uint8_t response[31] = {0}; 107 uint8_t *buf=&response[1]; 108 GetData(response,sizeof(response),&imuTime); 109 110 Euler eulerAngles; 111 eulerAngles.roll=Dequeue(&buf); 112 eulerAngles.pitch=Dequeue(&buf); 113 eulerAngles.yaw=Dequeue(&buf); 114 115 Vector3D filteredAngRates; 116 filteredAngRates.x=Dequeue(&buf); 117 filteredAngRates.y=Dequeue(&buf); 118 filteredAngRates.z=Dequeue(&buf); 119 120 ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(),filteredAngRates); 121 } else if(command==Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix) { 122 uint8_t response[67] = {0}; 123 uint8_t *buf=&response[1]; 124 GetData(response,sizeof(response),&imuTime); 125 126 Vector3D rawAcc; 127 rawAcc.x=9.80665*Dequeue(&buf); 128 rawAcc.y=9.80665*Dequeue(&buf); 129 rawAcc.z=9.80665*Dequeue(&buf); 130 131 Vector3D filteredAngRates; 132 filteredAngRates.x=Dequeue(&buf); 133 filteredAngRates.y=Dequeue(&buf); 134 filteredAngRates.z=Dequeue(&buf); 135 136 RotationMatrix matrix; 137 matrix(0,0)=Dequeue(&buf); 138 matrix(0,1)=Dequeue(&buf); 139 matrix(0,2)=Dequeue(&buf); 140 matrix(1,0)=Dequeue(&buf); 141 matrix(1,1)=Dequeue(&buf); 142 matrix(1,2)=Dequeue(&buf); 143 matrix(2,0)=Dequeue(&buf); 144 matrix(2,1)=Dequeue(&buf); 145 matrix(2,2)=Dequeue(&buf); 146 147 ahrsData->SetQuaternionAndAngularRates(matrix.ToEuler().ToQuaternion(),filteredAngRates); 148 imuData->SetRawAcc(rawAcc); 149 } 150 151 //change settings as soon as possible 152 //we assume that new imu datas will not come so fast 153 //so newt message from imu is an ack from the change settings 154 if(button_bias->Clicked()==true) GyrosBias(); 155 156 if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true 157 || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) { 158 if(setupgroupbox->isEnabled()==true) SamplingSettings(); 159 } 160 161 imuData->SetDataTime(imuTime); 162 ahrsData->SetDataTime(imuTime); 163 self->UpdateImu(); 164 self->ProcessUpdate(ahrsData); 165 } 166 SetContinuousMode(0); 167 SetBaudrate(115200); 168 169 self->WarnUponSwitches(false); 170 } 171 172 void Gx3_25_imu_impl::GetData(uint8_t* buf,ssize_t buf_size,Time *time) { 79 Time imuTime; 80 ImuData *imuData; 81 self->GetDatas(&imuData); 82 83 self->WarnUponSwitches(true); 84 85 // reset IMU to be sure it is at 115200 86 Printf("Gx3_25_imu::Reset IMU at 921600\n"); 87 serialport->SetBaudrate(921600); 88 DeviceReset(); 89 self->Thread::SleepMS(100); 90 serialport->FlushInput(); 91 serialport->SetBaudrate(115200); 92 93 SetBaudrate(921600); 94 Printf("Gx3_25_imu firmware number: %i\n", FirmwareNumber()); 95 PrintModelInfo(); 96 SamplingSettings(); 97 GyrosBias(); 98 // RealignUpNorth(true,true); 99 100 // on provoque les 9 ValueChanged 101 for (int i = 0; i < 9; i++) { 102 if (data_rate->ValueChanged() == true || 103 disable_magn->ValueChanged() == true || 104 disable_north_comp->ValueChanged() == true || 105 disable_grav_comp->ValueChanged() == true || 106 coning->ValueChanged() == true || 107 gyro_acc_size->ValueChanged() == true || 108 mag_size->ValueChanged() == true || up_comp->ValueChanged() == true || 109 north_comp->ValueChanged() == true) { 110 if (setupgroupbox->isEnabled() == true) 111 SamplingSettings(); 112 } 113 } 114 115 // periode a passer an argument (reglable) 116 // ou plutot laisser la periode geree par le centrale (polling) 117 // self->SetPeriodMS(2); 118 SetContinuousMode(command); 119 120 //_printf("firmware version: %i\n",get_firmware_number()); 121 122 while (!self->ToBeStopped()) { 123 if (command == Gx3_25_imu::EulerAnglesAndAngularRates) { 124 uint8_t response[31] = {0}; 125 uint8_t *buf = &response[1]; 126 GetData(response, sizeof(response), &imuTime); 127 128 Euler eulerAngles; 129 eulerAngles.roll = Dequeue(&buf); 130 eulerAngles.pitch = Dequeue(&buf); 131 eulerAngles.yaw = Dequeue(&buf); 132 133 Vector3D filteredAngRates; 134 filteredAngRates.x = Dequeue(&buf); 135 filteredAngRates.y = Dequeue(&buf); 136 filteredAngRates.z = Dequeue(&buf); 137 138 ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(), 139 filteredAngRates); 140 } else if (command == 141 Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix) { 142 uint8_t response[67] = {0}; 143 uint8_t *buf = &response[1]; 144 GetData(response, sizeof(response), &imuTime); 145 146 Vector3D rawAcc; 147 rawAcc.x = 9.80665 * Dequeue(&buf); 148 rawAcc.y = 9.80665 * Dequeue(&buf); 149 rawAcc.z = 9.80665 * Dequeue(&buf); 150 151 Vector3D filteredAngRates; 152 filteredAngRates.x = Dequeue(&buf); 153 filteredAngRates.y = Dequeue(&buf); 154 filteredAngRates.z = Dequeue(&buf); 155 156 RotationMatrix matrix; 157 matrix(0, 0) = Dequeue(&buf); 158 matrix(0, 1) = Dequeue(&buf); 159 matrix(0, 2) = Dequeue(&buf); 160 matrix(1, 0) = Dequeue(&buf); 161 matrix(1, 1) = Dequeue(&buf); 162 matrix(1, 2) = Dequeue(&buf); 163 matrix(2, 0) = Dequeue(&buf); 164 matrix(2, 1) = Dequeue(&buf); 165 matrix(2, 2) = Dequeue(&buf); 166 167 ahrsData->SetQuaternionAndAngularRates(matrix.ToEuler().ToQuaternion(), 168 filteredAngRates); 169 imuData->SetRawAcc(rawAcc); 170 } 171 172 // change settings as soon as possible 173 // we assume that new imu datas will not come so fast 174 // so newt message from imu is an ack from the change settings 175 if (button_bias->Clicked() == true) 176 GyrosBias(); 177 178 if (data_rate->ValueChanged() == true || 179 disable_magn->ValueChanged() == true || 180 disable_north_comp->ValueChanged() == true || 181 disable_grav_comp->ValueChanged() == true || 182 coning->ValueChanged() == true || 183 gyro_acc_size->ValueChanged() == true || 184 mag_size->ValueChanged() == true || up_comp->ValueChanged() == true || 185 north_comp->ValueChanged() == true) { 186 if (setupgroupbox->isEnabled() == true) 187 SamplingSettings(); 188 } 189 190 imuData->SetDataTime(imuTime); 191 ahrsData->SetDataTime(imuTime); 192 self->UpdateImu(); 193 self->ProcessUpdate(ahrsData); 194 } 195 SetContinuousMode(0); 196 SetBaudrate(115200); 197 198 self->WarnUponSwitches(false); 199 } 200 201 void Gx3_25_imu_impl::GetData(uint8_t *buf, ssize_t buf_size, Time *time) { 202 ssize_t read = 0; 203 ssize_t written = 0; 204 /* 205 written = serialport->Write(&command, sizeof(command)); 206 if(written<0) 207 { 208 self->Thread::Err("Write error (%s)\n",strerror(-written)); 209 } 210 else if (written != sizeof(command)) 211 { 212 self->Thread::Err("Write error %i/%i\n",written,sizeof(command)); 213 } 214 */ 215 read = serialport->Read(buf, buf_size); 216 *time = GetTime(); 217 if (read < 0) { 218 self->Thread::Err("Read error (%s)\n", strerror(-read)); 219 } else if (read != buf_size) { 220 self->Thread::Err("Read error %i/%i\n", read, buf_size); 221 } 222 223 if (CalcChecksum(buf, buf_size) == false) { 224 self->Thread::Err("wrong checksum\n"); 225 return; 226 } 227 } 228 229 float Gx3_25_imu_impl::Dequeue(uint8_t **buf) { 230 union float_4uint8 { 231 float f; 232 uint8_t b[4]; 233 } float_value; 234 235 float_value.b[0] = (*buf)[3]; 236 float_value.b[1] = (*buf)[2]; 237 float_value.b[2] = (*buf)[1]; 238 float_value.b[3] = (*buf)[0]; 239 (*buf) += sizeof(float); 240 241 return float_value.f; 242 } 243 244 void Gx3_25_imu_impl::GyrosBias(void) { 245 if (setupgroupbox->isEnabled() == true) { // devrait toujours etre bon 246 uint8_t response[19] = {0}; 247 uint8_t command[5]; 173 248 ssize_t read = 0; 174 249 ssize_t written = 0; 175 /* 176 written = serialport->Write(&command, sizeof(command)); 177 if(written<0) 178 { 179 self->Thread::Err("Write error (%s)\n",strerror(-written)); 180 } 181 else if (written != sizeof(command)) 182 { 183 self->Thread::Err("Write error %i/%i\n",written,sizeof(command)); 184 } 185 */ 186 read = serialport->Read(buf,buf_size); 187 *time=GetTime(); 188 if(read<0) { 189 self->Thread::Err("Read error (%s)\n",strerror(-read)); 190 } else if (read != buf_size) { 191 self->Thread::Err("Read error %i/%i\n",read,buf_size); 192 } 193 194 if(CalcChecksum(buf,buf_size)==false) { 195 self->Thread::Err("wrong checksum\n"); 196 return; 197 } 198 } 199 200 float Gx3_25_imu_impl::Dequeue(uint8_t** buf) { 201 union float_4uint8 { 202 float f; 203 uint8_t b[4]; 204 } 205 float_value; 206 207 float_value.b[0]=(*buf)[3]; 208 float_value.b[1]=(*buf)[2]; 209 float_value.b[2]=(*buf)[1]; 210 float_value.b[3]=(*buf)[0]; 211 (*buf)+=sizeof(float); 212 213 return float_value.f; 214 } 215 216 void Gx3_25_imu_impl::GyrosBias(void) { 217 if(setupgroupbox->isEnabled()==true) { //devrait toujours etre bon 218 uint8_t response[19] = {0}; 219 uint8_t command[5]; 220 ssize_t read = 0; 221 ssize_t written = 0; 222 223 Printf("Gx3_25_imu::gyros_bias: %s\n",self->IODevice::ObjectName().c_str()); 224 225 command[0]=0xcd;//entete 226 command[1]=0xc1;//confirm 227 command[2]=0x29;//confirm 228 command[3]=0x27;//time MSB en us 229 command[4]=0x10;//time LSB en us 230 231 written = serialport->Write( &command, sizeof(command)); 232 if(written<0) { 233 self->Thread::Err("Write error (%s)\n",strerror(-written)); 234 } else if (written != sizeof(command)) { 235 self->Thread::Err("Write error %i/%i\n",written,sizeof(command)); 236 } 237 238 self->SleepUS(1.1*(command[3]*256+command[4]));//on fait un sleep un peu plus grand 239 240 read = serialport->Read(&response[0],sizeof(response)); 241 if(read<0) { 242 self->Thread::Err("Read error (%s)\n",strerror(-read)); 243 } else if (read != sizeof(response)) { 244 self->Thread::Err("Read error %i/%i\n",read,sizeof(response)); 245 } 246 247 if(CalcChecksum(response,sizeof(response))==false) { 248 self->Thread::Err("wrong checksum\n"); 249 //return -1; 250 } 251 252 Printf("Gx3_25_imu::gyros_bias: %s ok\n",self->IODevice::ObjectName().c_str()); 253 254 } else { 255 self->Thread::Err("error locked\n"); 256 } 250 251 Printf("Gx3_25_imu::gyros_bias: %s\n", 252 self->IODevice::ObjectName().c_str()); 253 254 command[0] = 0xcd; // entete 255 command[1] = 0xc1; // confirm 256 command[2] = 0x29; // confirm 257 command[3] = 0x27; // time MSB en us 258 command[4] = 0x10; // time LSB en us 259 260 written = serialport->Write(&command, sizeof(command)); 261 if (written < 0) { 262 self->Thread::Err("Write error (%s)\n", strerror(-written)); 263 } else if (written != sizeof(command)) { 264 self->Thread::Err("Write error %i/%i\n", written, sizeof(command)); 265 } 266 267 self->SleepUS(1.1 * (command[3] * 256 + 268 command[4])); // on fait un sleep un peu plus grand 269 270 read = serialport->Read(&response[0], sizeof(response)); 271 if (read < 0) { 272 self->Thread::Err("Read error (%s)\n", strerror(-read)); 273 } else if (read != sizeof(response)) { 274 self->Thread::Err("Read error %i/%i\n", read, sizeof(response)); 275 } 276 277 if (CalcChecksum(response, sizeof(response)) == false) { 278 self->Thread::Err("wrong checksum\n"); 279 // return -1; 280 } 281 282 Printf("Gx3_25_imu::gyros_bias: %s ok\n", 283 self->IODevice::ObjectName().c_str()); 284 285 } else { 286 self->Thread::Err("error locked\n"); 287 } 257 288 } 258 289 259 290 void Gx3_25_imu_impl::SetContinuousMode(uint8_t continuous_command) { 260 uint8_t response[8] = {0}; 261 uint8_t command[4]; 262 ssize_t read = 0; 263 ssize_t written = 0; 264 265 command[0]=0xc4;//entete 266 command[1]=0xc1;//confirm 267 command[2]=0x29;//confirm 268 command[3]=continuous_command; 269 270 written = serialport->Write(command,sizeof(command)); 271 if(written<0) { 272 self->Thread::Err("Write error (%s)\n",strerror(-written)); 291 uint8_t response[8] = {0}; 292 uint8_t command[4]; 293 ssize_t read = 0; 294 ssize_t written = 0; 295 296 command[0] = 0xc4; // entete 297 command[1] = 0xc1; // confirm 298 command[2] = 0x29; // confirm 299 command[3] = continuous_command; 300 301 written = serialport->Write(command, sizeof(command)); 302 if (written < 0) { 303 self->Thread::Err("Write error (%s)\n", strerror(-written)); 304 } else if (written != sizeof(command)) { 305 self->Thread::Err("Write error %i/%i\n", written, sizeof(command)); 306 } 307 308 read = serialport->Read(response, sizeof(response)); 309 if (read < 0) { 310 self->Thread::Err("Read error (%s)\n", strerror(-read)); 311 } else if (read != sizeof(response)) { 312 self->Thread::Err("Read error %i/%i\n", read, sizeof(response)); 313 } 314 315 if (CalcChecksum(response, sizeof(response)) == false) { 316 self->Thread::Err("wrong checksum\n", self->IODevice::ObjectName().c_str()); 317 } 318 } 319 320 void Gx3_25_imu_impl::SamplingSettings(void) { 321 uint8_t response[19] = {0}; 322 uint8_t command[20]; 323 uint8_t result; 324 ssize_t read = 0; 325 ssize_t written = 0; 326 327 uint16_t rate = 1000 / data_rate->Value(); 328 329 command[0] = 0xdb; // entete 330 command[1] = 0xa8; // confirm 331 command[2] = 0xb9; // confirm 332 command[3] = 1; // change values 333 command[4] = (rate >> 8) & 0xff; // data rate MSB 334 command[5] = rate & 0xff; // data rate LSB 335 result = 0; 336 if (disable_magn->IsChecked() == true) 337 result |= 0x01; 338 if (disable_north_comp->IsChecked() == true) 339 result |= 0x04; 340 if (disable_grav_comp->IsChecked() == true) 341 result |= 0x08; 342 343 command[6] = result; 344 result = 0x01; // Calculate orientation 345 if (coning->IsChecked() == true) 346 result |= 0x02; 347 348 command[7] = result; 349 command[8] = gyro_acc_size->Value(); // gyro acc filter window 350 command[9] = mag_size->Value(); // mag filter window 351 command[10] = (up_comp->Value() >> 8) & 0xff; // up comp MSB 352 command[11] = up_comp->Value() & 0xff; // up comp LSB 353 command[12] = (north_comp->Value() >> 8) & 0xff; // north comp MSB 354 command[13] = north_comp->Value() & 0xff; // north comp LSB 355 command[14] = 0; // reserved 356 command[15] = 0; // reserved 357 command[16] = 0; // reserved 358 command[17] = 0; // reserved 359 command[18] = 0; // reserved 360 command[19] = 0; // reserved 361 362 written = serialport->Write(&command, sizeof(command)); 363 if (written < 0) { 364 self->Thread::Err("Write error (%s)\n", strerror(-written)); 365 } else if (written != sizeof(command)) { 366 self->Thread::Err("Write error %i/%i\n", written, sizeof(command)); 367 } 368 369 read = serialport->Read(&response[0], sizeof(response)); 370 if (read < 0) { 371 self->Thread::Err("Read error (%s)\n", strerror(-read)); 372 } else if (read != sizeof(response)) { 373 self->Thread::Err("Read error %i/%i\n", read, sizeof(response)); 374 } 375 376 if (CalcChecksum(response, sizeof(response)) == false) { 377 self->Thread::Err("wrong checksum\n", self->IODevice::ObjectName().c_str()); 378 } else { 379 data_rate_label->SetText("real: %.2fHz", 1000. / rate); 380 } 381 } 382 383 void Gx3_25_imu_impl::SetBaudrate(int value) { 384 uint8_t response[10] = {0}; 385 uint8_t command[11]; 386 ssize_t read = 0; 387 ssize_t written = 0; 388 389 union int32_4uint8 { 390 int32_t i; 391 uint8_t b[4]; 392 } baudrate_value; 393 394 baudrate_value.i = value; 395 Printf("Gx3_25_imu::SetBaudrate: %s ->%i\n", 396 self->IODevice::ObjectName().c_str(), baudrate_value.i); 397 398 command[0] = 0xd9; // entete 399 command[1] = 0xc3; // confirm 400 command[2] = 0x55; // confirm 401 command[3] = 1; // primary uart 402 command[4] = 1; // chgt temporaire 403 command[5] = baudrate_value.b[3]; 404 command[6] = baudrate_value.b[2]; 405 command[7] = baudrate_value.b[1]; 406 command[8] = baudrate_value.b[0]; 407 command[9] = 2; // uart enabled 408 command[10] = 0; // reserved 409 410 written = serialport->Write(&command, sizeof(command)); 411 if (written < 0) { 412 self->Thread::Err("Write error (%s)\n", strerror(-written)); 413 } else if (written != sizeof(command)) { 414 self->Thread::Err("Write error %i/%i\n", written, sizeof(command)); 415 } 416 417 read = serialport->Read(&response[0], sizeof(response)); 418 if (read < 0) { 419 self->Thread::Err("Read error (%s)\n", strerror(-read)); 420 } else if (read != sizeof(response)) { 421 self->Thread::Err("Read error %i/%i\n", read, sizeof(response)); 422 } 423 424 if (CalcChecksum(response, sizeof(response)) == false) { 425 self->Thread::Err("wrong checksum\n"); 426 return; 427 } 428 429 serialport->SetBaudrate(value); 430 } 431 432 int Gx3_25_imu_impl::FirmwareNumber(void) { 433 uint8_t response[7] = {0}; 434 uint8_t command; 435 ssize_t read = 0; 436 ssize_t written = 0; 437 union int32_4uint8 { 438 int32_t i; 439 uint8_t b[4]; 440 } value; 441 442 command = 0xe9; // entete 443 444 written = serialport->Write(&command, sizeof(command)); 445 if (written < 0) { 446 self->Thread::Err("Write error (%s)\n", strerror(-written)); 447 } else if (written != sizeof(command)) { 448 self->Thread::Err("Write error %i/%i\n", written, sizeof(command)); 449 } 450 451 read = serialport->Read(&response[0], sizeof(response)); 452 if (read < 0) { 453 self->Thread::Err("Read error (%s)\n", strerror(-read)); 454 } else if (read != sizeof(response)) { 455 self->Thread::Err("Read error %i/%i\n", read, sizeof(response)); 456 } 457 458 if (CalcChecksum(response, sizeof(response)) == false) { 459 self->Thread::Err("wrong checksum\n"); 460 return -1; 461 } 462 463 value.b[3] = response[1]; 464 value.b[2] = response[2]; 465 value.b[1] = response[3]; 466 value.b[0] = response[4]; 467 468 return value.i; 469 } 470 471 void Gx3_25_imu_impl::PrintModelInfo(void) { 472 uint8_t response[20] = {0}; 473 uint8_t command[2]; 474 ssize_t read = 0; 475 ssize_t written = 0; 476 477 for (int i = 0; i < 3; i++) { 478 command[0] = 0xea; // entete 479 command[1] = i; // entete 480 481 written = serialport->Write(&command, sizeof(command)); 482 if (written < 0) { 483 self->Thread::Err("Write error (%s)\n", strerror(-written)); 273 484 } else if (written != sizeof(command)) { 274 self->Thread::Err("Write error %i/%i\n",written,sizeof(command));275 } 276 277 read = serialport->Read( response,sizeof(response));278 if (read<0) {279 self->Thread::Err("Read error (%s)\n",strerror(-read));485 self->Thread::Err("Write error %i/%i\n", written, sizeof(command)); 486 } 487 488 read = serialport->Read(&response[0], sizeof(response)); 489 if (read < 0) { 490 self->Thread::Err("Read error (%s)\n", strerror(-read)); 280 491 } else if (read != sizeof(response)) { 281 self->Thread::Err("Read error %i/%i\n",read,sizeof(response)); 282 } 283 284 if(CalcChecksum(response,sizeof(response))==false) { 285 self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str()); 286 } 287 } 288 289 void Gx3_25_imu_impl::SamplingSettings(void) { 290 uint8_t response[19] = {0}; 291 uint8_t command[20]; 292 uint8_t result; 293 ssize_t read = 0; 294 ssize_t written = 0; 295 296 uint16_t rate=1000/data_rate->Value(); 297 298 command[0]=0xdb;//entete 299 command[1]=0xa8;//confirm 300 command[2]=0xb9;//confirm 301 command[3]=1;//change values 302 command[4]=(rate>>8)&0xff;//data rate MSB 303 command[5]=rate&0xff;//data rate LSB 304 result=0; 305 if(disable_magn->IsChecked()==true) result|=0x01; 306 if(disable_north_comp->IsChecked()==true) result|=0x04; 307 if(disable_grav_comp->IsChecked()==true) result|=0x08; 308 309 command[6]=result; 310 result=0x01;//Calculate orientation 311 if(coning->IsChecked()==true) result|=0x02; 312 313 command[7]=result; 314 command[8]=gyro_acc_size->Value();//gyro acc filter window 315 command[9]=mag_size->Value();//mag filter window 316 command[10]=(up_comp->Value()>>8)&0xff;//up comp MSB 317 command[11]=up_comp->Value()&0xff;//up comp LSB 318 command[12]=(north_comp->Value()>>8)&0xff;//north comp MSB 319 command[13]=north_comp->Value()&0xff;//north comp LSB 320 command[14]=0;//reserved 321 command[15]=0;//reserved 322 command[16]=0;//reserved 323 command[17]=0;//reserved 324 command[18]=0;//reserved 325 command[19]=0;//reserved 326 327 written = serialport->Write(&command, sizeof(command)); 328 if(written<0) { 329 self->Thread::Err("Write error (%s)\n",strerror(-written)); 330 } else if (written != sizeof(command)) { 331 self->Thread::Err("Write error %i/%i\n",written,sizeof(command)); 332 } 333 334 read = serialport->Read(&response[0],sizeof(response)); 335 if(read<0) { 336 self->Thread::Err("Read error (%s)\n",strerror(-read)); 337 } else if (read != sizeof(response)) { 338 self->Thread::Err("Read error %i/%i\n",read,sizeof(response)); 339 } 340 341 if(CalcChecksum(response,sizeof(response))==false) { 342 self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str()); 343 } else { 344 data_rate_label->SetText("real: %.2fHz",1000./rate); 345 } 346 } 347 348 void Gx3_25_imu_impl::SetBaudrate(int value) { 349 uint8_t response[10] = {0}; 350 uint8_t command[11]; 351 ssize_t read = 0; 352 ssize_t written = 0; 353 354 union int32_4uint8 { 355 int32_t i; 356 uint8_t b[4]; 357 } 358 baudrate_value; 359 360 baudrate_value.i=value; 361 Printf("Gx3_25_imu::SetBaudrate: %s ->%i\n",self->IODevice::ObjectName().c_str(),baudrate_value.i); 362 363 command[0]=0xd9;//entete 364 command[1]=0xc3;//confirm 365 command[2]=0x55;//confirm 366 command[3]=1;//primary uart 367 command[4]=1;//chgt temporaire 368 command[5]=baudrate_value.b[3]; 369 command[6]=baudrate_value.b[2]; 370 command[7]=baudrate_value.b[1]; 371 command[8]=baudrate_value.b[0]; 372 command[9]=2;//uart enabled 373 command[10]=0;//reserved 374 375 written = serialport->Write(&command, sizeof(command)); 376 if(written<0) { 377 self->Thread::Err("Write error (%s)\n",strerror(-written)); 378 } else if (written != sizeof(command)) { 379 self->Thread::Err("Write error %i/%i\n",written,sizeof(command)); 380 } 381 382 read = serialport->Read(&response[0],sizeof(response)); 383 if(read<0) { 384 self->Thread::Err("Read error (%s)\n",strerror(-read)); 385 } else if (read != sizeof(response)) { 386 self->Thread::Err("Read error %i/%i\n",read,sizeof(response)); 387 } 388 389 if(CalcChecksum(response,sizeof(response))==false) { 390 self->Thread::Err("wrong checksum\n"); 391 return ; 392 } 393 394 serialport->SetBaudrate(value); 395 } 396 397 int Gx3_25_imu_impl::FirmwareNumber(void) { 398 uint8_t response[7] = {0}; 399 uint8_t command; 400 ssize_t read = 0; 401 ssize_t written = 0; 402 union int32_4uint8 { 403 int32_t i; 404 uint8_t b[4]; 405 } 406 value; 407 408 command=0xe9;//entete 409 410 written = serialport->Write(&command, sizeof(command)); 411 if(written<0) { 412 self->Thread::Err("Write error (%s)\n",strerror(-written)); 413 } else if (written != sizeof(command)) { 414 self->Thread::Err("Write error %i/%i\n",written,sizeof(command)); 415 } 416 417 read = serialport->Read(&response[0],sizeof(response)); 418 if(read<0) { 419 self->Thread::Err("Read error (%s)\n",strerror(-read)); 420 } else if (read != sizeof(response)) { 421 self->Thread::Err("Read error %i/%i\n",read,sizeof(response)); 422 } 423 424 if(CalcChecksum(response,sizeof(response))==false) { 425 self->Thread::Err("wrong checksum\n"); 426 return -1; 427 } 428 429 value.b[3]=response[1]; 430 value.b[2]=response[2]; 431 value.b[1]=response[3]; 432 value.b[0]=response[4]; 433 434 return value.i; 435 436 } 437 438 void Gx3_25_imu_impl::PrintModelInfo(void) { 439 uint8_t response[20] = {0}; 440 uint8_t command[2]; 441 ssize_t read = 0; 442 ssize_t written = 0; 443 444 for(int i=0; i<3; i++) { 445 command[0]=0xea;//entete 446 command[1]=i;//entete 447 448 written = serialport->Write(&command, sizeof(command)); 449 if(written<0) { 450 self->Thread::Err("Write error (%s)\n",strerror(-written)); 451 } else if (written != sizeof(command)) { 452 self->Thread::Err("Write error %i/%i\n",written,sizeof(command)); 453 } 454 455 read = serialport->Read(&response[0],sizeof(response)); 456 if(read<0) { 457 self->Thread::Err("Read error (%s)\n",strerror(-read)); 458 } else if (read != sizeof(response)) { 459 self->Thread::Err("Read error %i/%i\n",read,sizeof(response)); 460 } 461 462 if(CalcChecksum(response,sizeof(response))==false) { 463 self->Thread::Err("wrong checksum\n"); 464 //return -1; 465 } 466 467 char* msg=(char*)(&response[2]); 468 msg[16]=0; 469 switch(i) { 470 case 0: 471 Printf("Gx3_25_imu model number: %s\n",msg); 472 break; 473 case 1: 474 Printf("Gx3_25_imu serial number: %s\n",msg); 475 break; 476 case 2: 477 Printf("Gx3_25_imu model name: %s\n",msg); 478 break; 479 } 480 } 481 482 } 483 484 void Gx3_25_imu_impl::RealignUpNorth(bool realign_up,bool realign_north) { 485 uint8_t response[7] = {0}; 486 uint8_t command[10]; 487 ssize_t read = 0; 488 ssize_t written = 0; 489 490 command[0]=0xdd;//entete 491 command[1]=0x54;//confirm 492 command[2]=0x4c;//confirm 493 command[3]=0;//send reply 494 command[4]=255;//up realign 495 command[5]=1;//north realign 496 command[6]=0;//reserved 497 command[7]=0;//reserved 498 command[8]=0;//reserved 499 command[9]=0;//reserved 500 501 written = serialport->Write(&command, sizeof(command)); 502 if(written<0) { 503 self->Thread::Err("Write error (%s)\n",strerror(-written)); 504 } else if (written != sizeof(command)) { 505 self->Thread::Err("Write error %i/%i\n",written,sizeof(command)); 506 } 507 508 read = serialport->Read(&response[0],sizeof(response)); 509 if(read<0) { 510 self->Thread::Err("Read error(%s)\n",strerror(-read)); 511 } else if (read != sizeof(response)) { 512 self->Thread::Err("Read error %i/%i\n",read,sizeof(response)); 513 } 514 515 if(CalcChecksum(response,sizeof(response))==false) { 516 self->Thread::Err("wrong checksum\n"); 517 } 492 self->Thread::Err("Read error %i/%i\n", read, sizeof(response)); 493 } 494 495 if (CalcChecksum(response, sizeof(response)) == false) { 496 self->Thread::Err("wrong checksum\n"); 497 // return -1; 498 } 499 500 char *msg = (char *)(&response[2]); 501 msg[16] = 0; 502 switch (i) { 503 case 0: 504 Printf("Gx3_25_imu model number: %s\n", msg); 505 break; 506 case 1: 507 Printf("Gx3_25_imu serial number: %s\n", msg); 508 break; 509 case 2: 510 Printf("Gx3_25_imu model name: %s\n", msg); 511 break; 512 } 513 } 514 } 515 516 void Gx3_25_imu_impl::RealignUpNorth(bool realign_up, bool realign_north) { 517 uint8_t response[7] = {0}; 518 uint8_t command[10]; 519 ssize_t read = 0; 520 ssize_t written = 0; 521 522 command[0] = 0xdd; // entete 523 command[1] = 0x54; // confirm 524 command[2] = 0x4c; // confirm 525 command[3] = 0; // send reply 526 command[4] = 255; // up realign 527 command[5] = 1; // north realign 528 command[6] = 0; // reserved 529 command[7] = 0; // reserved 530 command[8] = 0; // reserved 531 command[9] = 0; // reserved 532 533 written = serialport->Write(&command, sizeof(command)); 534 if (written < 0) { 535 self->Thread::Err("Write error (%s)\n", strerror(-written)); 536 } else if (written != sizeof(command)) { 537 self->Thread::Err("Write error %i/%i\n", written, sizeof(command)); 538 } 539 540 read = serialport->Read(&response[0], sizeof(response)); 541 if (read < 0) { 542 self->Thread::Err("Read error(%s)\n", strerror(-read)); 543 } else if (read != sizeof(response)) { 544 self->Thread::Err("Read error %i/%i\n", read, sizeof(response)); 545 } 546 547 if (CalcChecksum(response, sizeof(response)) == false) { 548 self->Thread::Err("wrong checksum\n"); 549 } 518 550 } 519 551 520 552 void Gx3_25_imu_impl::DeviceReset(void) { 521 uint8_t command[3]; 522 ssize_t written = 0; 523 524 command[0]=0xfe;//entete 525 command[1]=0x9e;//confirm 526 command[2]=0x3a;//confirm 527 528 written = serialport->Write(&command, sizeof(command)); 529 if(written<0) { 530 self->Thread::Err("Write error (%s)\n",strerror(-written)); 531 } else if (written != sizeof(command)) { 532 self->Thread::Err("Write error %i/%i\n",written,sizeof(command)); 533 } 534 } 535 536 bool Gx3_25_imu_impl::CalcChecksum(uint8_t *buf,int size) { 537 uint16_t tChksum,tResponseChksum; 538 539 tChksum = 0; 540 for (int i = 0; i < size - 2; i++) tChksum += buf[i]; 541 // Extract the big-endian checksum from reply 542 tResponseChksum = 0; 543 tResponseChksum = buf[size - 2] << 8; 544 tResponseChksum += buf[size - 1]; 545 546 if(tChksum!=tResponseChksum) 547 return false; 548 else 549 return true; 550 } 553 uint8_t command[3]; 554 ssize_t written = 0; 555 556 command[0] = 0xfe; // entete 557 command[1] = 0x9e; // confirm 558 command[2] = 0x3a; // confirm 559 560 written = serialport->Write(&command, sizeof(command)); 561 if (written < 0) { 562 self->Thread::Err("Write error (%s)\n", strerror(-written)); 563 } else if (written != sizeof(command)) { 564 self->Thread::Err("Write error %i/%i\n", written, sizeof(command)); 565 } 566 } 567 568 bool Gx3_25_imu_impl::CalcChecksum(uint8_t *buf, int size) { 569 uint16_t tChksum, tResponseChksum; 570 571 tChksum = 0; 572 for (int i = 0; i < size - 2; i++) 573 tChksum += buf[i]; 574 // Extract the big-endian checksum from reply 575 tResponseChksum = 0; 576 tResponseChksum = buf[size - 2] << 8; 577 tResponseChksum += buf[size - 1]; 578 579 if (tChksum != tResponseChksum) 580 return false; 581 else 582 return true; 583 }
Note:
See TracChangeset
for help on using the changeset viewer.