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