Uploaded image for project: 'LibrePilot'
  1. LP-376

GCS: Implement automatic yaw detection for virtual board rotation

    Details

    • Type: Improvement
    • Status: Open
    • Priority: Medium
    • Resolution: Unresolved
    • Affects Version/s: None
    • Fix Version/s: future_release
    • Component/s: GCS
    • Labels:
      None
    • Sprint:

      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":

      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.

        Attachments

          Issue links

            Activity

              People

              • Assignee:
                Unassigned
                Reporter:
                mindnever Vladimir Zidar
              • Votes:
                0 Vote for this issue
                Watchers:
                2 Start watching this issue

                Dates

                • Created:
                  Updated: