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