2e464a53c2
The switching between different AP_HAL was happening by giving different definitions of AP_HAL_BOARD_DRIVER, and the programs would use it to instantiate. A program or library code would have to explicitly include (and depend) on the concrete implementation of the HAL, even when using it only via interface. The proposed change move this dependency to be link time. There is a AP_HAL::get_HAL() function that is used by the client code. Each implementation of HAL provides its own definition of this function, returning the appropriate concrete instance. Since this replaces the job of AP_HAL_BOARD_DRIVER, the definition was removed. The static variables for PX4 and VRBRAIN were named differently to avoid shadowing the extern symbol 'hal'.
204 lines
6.6 KiB
C++
204 lines
6.6 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
//
|
|
// Unit tests for the AP_Math rotations code
|
|
//
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <stdlib.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Progmem/AP_Progmem.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <AP_HAL_AVR/AP_HAL_AVR.h>
|
|
#include <AP_HAL_SITL/AP_HAL_SITL.h>
|
|
#include <AP_HAL_Empty/AP_HAL_Empty.h>
|
|
#include <AP_HAL_PX4/AP_HAL_PX4.h>
|
|
#include <AP_HAL_Linux/AP_HAL_Linux.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <Filter/Filter.h>
|
|
#include <AP_ADC/AP_ADC.h>
|
|
#include <SITL/SITL.h>
|
|
#include <AP_Compass/AP_Compass.h>
|
|
#include <AP_Baro/AP_Baro.h>
|
|
#include <AP_Notify/AP_Notify.h>
|
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
|
#include <AP_GPS/AP_GPS.h>
|
|
#include <DataFlash/DataFlash.h>
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
#include <AP_Mission/AP_Mission.h>
|
|
#include <StorageManager/StorageManager.h>
|
|
#include <AP_Terrain/AP_Terrain.h>
|
|
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_NavEKF/AP_NavEKF.h>
|
|
#include <AP_Airspeed/AP_Airspeed.h>
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
|
|
#include <AP_Rally/AP_Rally.h>
|
|
#include <AP_BattMonitor/AP_BattMonitor.h>
|
|
#include <AP_RangeFinder/AP_RangeFinder.h>
|
|
#include <AP_OpticalFlow/AP_OpticalFlow.h>
|
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
|
|
|
static void print_vector(Vector3f &v)
|
|
{
|
|
hal.console->printf("[%.4f %.4f %.4f]\n",
|
|
v.x, v.y, v.z);
|
|
}
|
|
|
|
// test rotation method accuracy
|
|
static void test_rotation_accuracy(void)
|
|
{
|
|
Matrix3f attitude;
|
|
Vector3f small_rotation;
|
|
float roll, pitch, yaw;
|
|
int16_t i;
|
|
float rot_angle;
|
|
|
|
hal.console->println_P(PSTR("\nRotation method accuracy:"));
|
|
|
|
for( i=0; i<90; i++ ) {
|
|
|
|
// reset initial attitude
|
|
attitude.from_euler(0,0,0);
|
|
|
|
// calculate small rotation vector
|
|
rot_angle = ToRad(i);
|
|
small_rotation = Vector3f(0,0,rot_angle);
|
|
|
|
// apply small rotation
|
|
attitude.rotate(small_rotation);
|
|
|
|
// get resulting attitude's euler angles
|
|
attitude.to_euler(&roll, &pitch, &yaw);
|
|
|
|
// display results
|
|
hal.console->printf_P(
|
|
PSTR("actual angle: %d\tcalculated angle:%4.2f\n"),
|
|
(int)i,ToDeg(yaw));
|
|
}
|
|
}
|
|
|
|
static void test_euler(enum Rotation rotation, float roll, float pitch, float yaw)
|
|
{
|
|
Vector3f v, v1, v2, diff;
|
|
Matrix3f rotmat;
|
|
const float accuracy = 1.0e-6f;
|
|
|
|
v.x = 1;
|
|
v.y = 2;
|
|
v.z = 3;
|
|
v1 = v;
|
|
|
|
v1.rotate(rotation);
|
|
|
|
rotmat.from_euler(radians(roll), radians(pitch), radians(yaw));
|
|
v2 = v;
|
|
v2 = rotmat * v2;
|
|
|
|
diff = (v2 - v1);
|
|
if (diff.length() > accuracy) {
|
|
hal.console->printf("euler test %u failed : yaw:%d roll:%d pitch:%d\n",
|
|
(unsigned)rotation,
|
|
(int)yaw,
|
|
(int)roll,
|
|
(int)pitch);
|
|
hal.console->printf("fast rotated: ");
|
|
print_vector(v1);
|
|
hal.console->printf("slow rotated: ");
|
|
print_vector(v2);
|
|
hal.console->printf("\n");
|
|
}
|
|
}
|
|
|
|
static void test_eulers(void)
|
|
{
|
|
hal.console->println("euler tests");
|
|
test_euler(ROTATION_NONE, 0, 0, 0);
|
|
test_euler(ROTATION_YAW_45, 0, 0, 45);
|
|
test_euler(ROTATION_YAW_90, 0, 0, 90);
|
|
test_euler(ROTATION_YAW_135, 0, 0, 135);
|
|
test_euler(ROTATION_YAW_180, 0, 0, 180);
|
|
test_euler(ROTATION_YAW_225, 0, 0, 225);
|
|
test_euler(ROTATION_YAW_270, 0, 0, 270);
|
|
test_euler(ROTATION_YAW_315, 0, 0, 315);
|
|
test_euler(ROTATION_ROLL_180, 180, 0, 0);
|
|
test_euler(ROTATION_ROLL_180_YAW_45, 180, 0, 45);
|
|
test_euler(ROTATION_ROLL_180_YAW_90, 180, 0, 90);
|
|
test_euler(ROTATION_ROLL_180_YAW_135, 180, 0, 135);
|
|
test_euler(ROTATION_PITCH_180, 0, 180, 0);
|
|
test_euler(ROTATION_ROLL_180_YAW_225, 180, 0, 225);
|
|
test_euler(ROTATION_ROLL_180_YAW_270, 180, 0, 270);
|
|
test_euler(ROTATION_ROLL_180_YAW_315, 180, 0, 315);
|
|
test_euler(ROTATION_ROLL_90, 90, 0, 0);
|
|
test_euler(ROTATION_ROLL_90_YAW_45, 90, 0, 45);
|
|
test_euler(ROTATION_ROLL_90_YAW_90, 90, 0, 90);
|
|
test_euler(ROTATION_ROLL_90_YAW_135, 90, 0, 135);
|
|
test_euler(ROTATION_ROLL_270, 270, 0, 0);
|
|
test_euler(ROTATION_ROLL_270_YAW_45, 270, 0, 45);
|
|
test_euler(ROTATION_ROLL_270_YAW_90, 270, 0, 90);
|
|
test_euler(ROTATION_ROLL_270_YAW_135, 270, 0, 135);
|
|
test_euler(ROTATION_PITCH_90, 0, 90, 0);
|
|
test_euler(ROTATION_PITCH_270, 0, 270, 0);
|
|
test_euler(ROTATION_PITCH_180_YAW_90, 0, 180, 90);
|
|
test_euler(ROTATION_PITCH_180_YAW_270, 0, 180, 270);
|
|
test_euler(ROTATION_ROLL_90_PITCH_90, 90, 90, 0);
|
|
test_euler(ROTATION_ROLL_180_PITCH_90,180, 90, 0);
|
|
test_euler(ROTATION_ROLL_270_PITCH_90,270, 90, 0);
|
|
test_euler(ROTATION_ROLL_90_PITCH_180, 90, 180, 0);
|
|
test_euler(ROTATION_ROLL_270_PITCH_180,270,180, 0);
|
|
test_euler(ROTATION_ROLL_90_PITCH_270, 90, 270, 0);
|
|
test_euler(ROTATION_ROLL_180_PITCH_270,180,270, 0);
|
|
test_euler(ROTATION_ROLL_270_PITCH_270,270,270, 0);
|
|
test_euler(ROTATION_ROLL_90_PITCH_180_YAW_90, 90, 180, 90);
|
|
test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270);
|
|
test_euler(ROTATION_YAW_293_PITCH_68_ROLL_90,90,68.8,293.3);
|
|
}
|
|
|
|
static bool have_rotation(const Matrix3f &m)
|
|
{
|
|
Matrix3f mt = m.transposed();
|
|
for (enum Rotation r=ROTATION_NONE;
|
|
r<ROTATION_MAX;
|
|
r = (enum Rotation)((uint8_t)r+1)) {
|
|
Vector3f v(1,2,3);
|
|
Vector3f v2 = v;
|
|
v2.rotate(r);
|
|
v2 = mt * v2;
|
|
if ((v2 - v).length() < 0.01f) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static void missing_rotations(void)
|
|
{
|
|
hal.console->println("testing for missing rotations");
|
|
uint16_t roll, pitch, yaw;
|
|
for (yaw=0; yaw<360; yaw += 90)
|
|
for (pitch=0; pitch<360; pitch += 90)
|
|
for (roll=0; roll<360; roll += 90) {
|
|
Matrix3f m;
|
|
m.from_euler(ToRad(roll), ToRad(pitch), ToRad(yaw));
|
|
if (!have_rotation(m)) {
|
|
hal.console->printf("Missing rotation (%u, %u, %u)\n", roll, pitch, yaw);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* rotation tests
|
|
*/
|
|
void setup(void)
|
|
{
|
|
hal.console->println("rotation unit tests\n");
|
|
test_rotation_accuracy();
|
|
test_eulers();
|
|
missing_rotations();
|
|
hal.console->println("rotation unit tests done\n");
|
|
}
|
|
|
|
void loop(void) {}
|
|
|
|
AP_HAL_MAIN();
|