We're updating the issue view to help you get more done. 

GCS: Implement automatic yaw detection for virtual board rotation

Description

Correct board rotation can be detected by GCS using accel data and known vehicle attitude ( 30 degree pitch forward ).

For the reference only, here is the part of the code which does it in the "other firmware":

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 bool Calibration::storeYawOrientationMeasurement(UAVObject *obj) { Accels * accels = Accels::GetInstance(getObjectManager()); // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES_YAW_ORIENTATION samples if(obj->getObjID() == Accels::OBJID) { Accels::DataFields accelsData = accels->getData(); accel_accum_x.append(accelsData.x); accel_accum_y.append(accelsData.y); accel_accum_z.append(accelsData.z); } // update the progress indicator emit yawOrientationProgressChanged((double)accel_accum_x.size() / NUM_SENSOR_UPDATES_YAW_ORIENTATION * 100); // If we have enough samples, then stop sampling and compute the biases if (accel_accum_x.size() >= NUM_SENSOR_UPDATES_YAW_ORIENTATION) { timer.stop(); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); // Get the existing attitude settings AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData(); // Use sensor data without rotation, as it has already been rotated on-board. double a_body[3] = { listMean(accel_accum_x), listMean(accel_accum_y), listMean(accel_accum_z) }; // Temporary variable double psi; // Solve "a_sensor = Rot(phi, theta, psi) *[0;0;-g]" for the roll (phi) and pitch (theta) values. // Recall that phi is in [-pi, pi] and theta is in [-pi/2, pi/2] psi = atan2(a_body[1], -a_body[0]); attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] += psi * RAD2DEG * 100.0; // Scale by 100 because units are [100*deg] // Wrap to [-pi, pi] while (attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] > 180*100) // Scale by 100 because units are [100*deg] attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] -= 360*100; while (attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] < -180*100) attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] += 360*100; attitudeSettings->setData(attitudeSettingsData); attitudeSettings->updated(); // Inform the system that the calibration process has completed emit calibrationCompleted(); return true; } return false; }

The implementation on Configuration/Attitude/Calibration page could look something like this

And when clicked on "Start" could display following text:

"Place in approximately 30 degree pitch forward (nose down) position. Please take care not to roll the vehicle."

It should start collecting accel samples on "Save Position" and then after collecting NUM_SENSOR_UPDATES_YAW_ORIENTATION of them, do the magic math and save calculated AttitudeSettings.BoardRotation.Yaw value.

Status

Assignee

Unassigned

Reporter

Vladimir Zidar

Labels

None

Components

Fix versions

Priority

Medium