3. IMU, GPS, and Compass - roverwing/RoverWingLibrary GitHub Wiki

This section describes use of navigation sensors.

• Inertial Motion Unit (IMU)
• Compass
• GPS

RoverWing contains a built-in Inertial Motion Unit (IMU), which is based on MPU6500 chip from Invensense. This chip combines a 3-axis accelerometer and a 3-axis gyro sensor, which provide information about acceleration and rotational speed. RoverWing firmware combines the sensor data to provide information about rover orientation in space, in the form of Yaw, Pitch, and Roll angles. (RoverWing's firmware is based on the work of Kris Winer and uses data fusion algorithm invented by Sebastian Madgwick.)

Note:

Current version of RoverWing firmware requires that the RoverWing be mounted horizontally.

By default, the IMU is inactive. To activate it, use the following function:

void  IMUbegin();

You can check whether activation was successful by calling:

bool IMUisActive();

which returns true if the IMU is active.

To stop IMU (to save computing resources), use:

void IMUend();

Before use, the IMU needs to be calibrated. The calibration process determines and then applies corrections to the raw data; without these corrections, the data returned by the sensor is very inaccurate. This is normally done immediately after activation. There are two ways to do this:

1. If you haven't yet calibrated the sensor before (or want to recalibrate it), use the following function:

   void IMUcalibrate(int16_t * aOffsets, int16_t * gOffsets);
   

   This function will take about a second to execute; during this time, the robot must be completely stationary. This function will determine and apply the corrections; it will also save these corrections in arrays aOffsets (for accelerometer offsets) and gOffsets (for gyro offsets). You can print these values to serial monitor and write them down for future use. Unfortunately, current version of RoverWing doesn't have any persistent storage, so there is no way to save these values to the board memory between resets.

2. If you had previously calibrated the sensor and have recorded the calibration offsets, you do not need to repeat the calibration process - you can just apply the previously found values. To do that, enter the offset values in your sketch:

   int16_t aOffsets[]={0,0,0};
   int16_t gOffsets[]={0,0,0};
   

   (replacing zeros with actual values) and then use this function:

   void IMUsetOffsets(int16_t * aOffsets, int16_t * gOffsets);
   

   This function executes almost instantaneously; to be on the safe side, you might wait 100 ms before reading the IMU values.

RoverWing allows you to read both the raw data (accelerometer and gyro readings) and computed orientation, using the following functions:

void getAccel():

Fetches from the RoverWing raw acceleration data. This data can be accessed using member variables ax, ay, az, which give the acceleration in x-, y-, and z- directions respectively in g as floats.

void getGyro():

Fetches from the RoverWing raw gyro data. This data can be accessed using member variables gx, gy, gz, which give the angular rotation velocity around x-, y-, and z- axes respectively, in degree/s (as floats).

float getYaw():

float getPitch():

float getRoll():

These functions return yaw, pitch, and roll angles for the robot. These three angles describe the robot orientation:

• yaw is the rotation around the vertical axis (positive angle corresponds to clockwise rotation, i.e. right turns), relative to the starting position of the robot
• pitch is the rotation around the horizontal line, running from left to right. Positive pitch angle corresponds to raising the front of the robot and lowering the back
• roll is the rotation around the horizontal line running from front to back. Positive roll angle corresponds to raising the left side of the robot and lowering the right.

For more information about yaw, pitch, and roll angles, please visit https://en.wikipedia.org/wiki/Aircraft_principal_axes

void getOrientationQuat():

Gets robot orientation as a unit quaternion (see https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation). T he result can be accessed using member variable float quat[4], which contains the four components of the quaternion:

q=quat[0]+i*quat[1]+j*quat[2]+k*quat[3]

RoverWing provides a connector for attaching a combined GPS and a compass (magnetometer) sensor. If you have such a sensor connected, you can activate it and use to determine absolute orientation using the functions below.

Note:

Most GPS/compass combination sensors used for drones provide power to the sensor via the GPS connector. Thus, you need to connect both GPS and compass connector, even if you only intend to use the compass.

To activate/deactivate the compass sensor, use the following functions.

void magBegin():

Activate the compass (magnetometer) sensor.

void magEnd():

Stop the compass sensor.

uint8_t magStatus():

Returns the status of the compass sensor. You can compare it with one of predefined values:

• MAG_OFF: magnetometer is inactive or absent
• MAG_OK: magnetometer is active
• MAG_CALIBRATING: magnetometer calibration in progress

As with the IMU, the compass sensor needs to be calibrated before use. You have two options:

1. If you have not calibrated the sensor, or want to recalibrate it, you can run the calibration by using the function void magStartCalibration(). Calling this function starts the calibration process. It takes about 20 seconds, during which time you need to move the robot in a figure 8 pattern in 3d (not just rotating around vertical axis!). Make sure that the USB cable used to connect the feather board to the computer is long enough.

   To check when the calibration process is complete, use function magStatus() described above. After the calibration has completed, you can get the calibration data and save it or print to serial monitor for future use. Currently, the calibration process only determines one kind of calibration data, the magnetometer offsets, an array of three integer values. To get these values, use:

   void magGetOffsets(int16_t offsets[3]):
   

2. If you had run the calibration before and have recorded the computed calibration data, you can skip the calibration, instead using the recorded values. To do that, use:

void magSetCalData(int16_t offsets[3], float matrix[3][3])

By itself, the compass sensor can only determine robot heading relative to magnetic north, which does not coincide with the true geographic north. If you need heading relative to true north, you need apply correction known as magnetic declination. This correction depends on your geographic location. To learn more about it or find the magnetic declination for your location, you can visit, for example, http://www.magnetic-declination.com/what-is-magnetic-declination.php.

To set magnetic declination, use the function below:

void setDeclination(float d):

which applies the declination: after that, the heading value returned by getHeading will be relative to true north.

Once the compass has been calibrated, you can get the the readings by using the functions below.

float getHeading():

Returns current robot heading in degrees. Heading is zero when the robot is pointing north (true north if you have set the declination, magnetic north otherwise); positive values correspond to robot pointing east, negoative, robot pointing west. The returned value is between -180 and 180.

void getMagData(int16_t m[3]):

Fetches and saves to array m the raw magnetometer readings, i.e., the x-, y-, and z- components of the magnetic field, in units of 0.93 milligauss. Note that these values are not affected by the magnetic declination.

void GPSbegin():

Start the GPS. Note that after starting, it can take the sensor a while to get GPS location fix: the time ranges from several seconds if the sensor had recently been used in a nearby location to several minutes if the sensor has been moved to a completely new location.

void GPSend():

Stops the GPS sensor.

uint8_t GPSstatus()

Gets current GPS status. Possible values are

• GPS_OFF: GPS is inactive
• GPS_OK: GPS is active and has a valid location fix
• GPS_WAITING: GPS is active, but is waiting to receive a location fix. The sensor switches to this status if it hasn't received a valid GPS signal for more than 3 seconds.

AFter the gPS has been initialized and received location fix, you can use the following functions to access the GPS coordinates.

void getGPSlocation():

Gets from RoverWing and saves the latest GPS location data, which can later be accessed using functions below.

double latitude():

double longitude():

Return the robot latitude and longitude in degrees. Note that these coordinates refer to the location fetched at last call of getGPSlocation().

int32_t latitudeL():

int32_t longitudeL():

Return longitude and latitude of the robot, in units of 10^{-7} degree (about 10 cm).

uint32_t GPStimestamp():

Returns time when the last GPS location fix was received, in milliseconds since reboot.

RoverWing library provides a type for storing GPS location and timestamp. It is defined in RoverWing.h as follows:

struct location_t {
  int32_t latitude; //latitude, in units of 10^{-7} degree
  int32_t longitude;
  uint32_t timestamp; //in ms, as reported by millis()
};

The functions below provide some tools for working with location data:

void saveGPSlocation(location_t & l):

Saves current robot location to variable l.

float distanceTo(const location_t & l ):

Returns distance from current robot location to location l, in meters. Note: this function uses flat map model. The results are accurate enough for distances up to 10 km, but if you want to find the distance between your robot and Mount Everest, you need to write your own code.

float bearingTo(const location_t & l ):

Returns bearing from current robot location to l. The bearing is measured in degrees and ranges from -180 to 180, with North being 0. As with distanceTo() function this function shoudl only be used when the distance between locations is under 10 km.