Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: port to work on coreless arduino (AP_HAL)

This commit is contained in:
Pat Hickey 2012-09-18 11:08:18 -07:00 committed by Andrew Tridgell
parent f63fb29d52
commit 7dbe198e5c
14 changed files with 104 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Math/AP_Math.h
View File

@ -16,6 +16,12 @@
#include "quaternion.h"
#include "polygon.h"
#ifndef PI
#define PI 3.141592653589793
#endif
#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105
// define AP_Param types AP_Vector3f and Ap_Matrix3f
AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
@ -55,5 +61,17 @@ void location_update(struct Location *loc, float bearing, float distance)
// extrapolate latitude/longitude given distances north and east
void location_offset(struct Location *loc, float ofs_north, float ofs_east);
#ifdef radians
#error "You need to add empty nocore.inoflag and Arduino.h files to your sketch"
#endif
/* The following three functions used to be arduino core macros */
#define radians(deg) ((deg) * DEG_TO_RAD)
#define degrees(rad) ((rad) * RAD_TO_DEG)
#define sq(x) ((x)*(x))
#define max(a,b) ((a)>(b)?(a):(b))
#define min(a,b) ((a)<(b)?(a):(b))
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#endif // AP_MATH_H

0
libraries/AP_Math/examples/eulers/Arduino.h Normal file
View File

78
libraries/AP_Math/examples/eulers/eulers.pde
View File

@ -3,38 +3,14 @@
// Unit tests for the AP_Math euler code
//
#include <FastSerial.h>
#include <stdlib.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_HAL.h>
FastSerialPort(Serial, 0);
#ifdef DESKTOP_BUILD
// all of this is needed to build with SITL
#include <SPI.h>
#include <I2C.h>
#include <DataFlash.h>
#include <APM_RC.h>
#include <GCS_MAVLink.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_ADC.h>
#include <AP_Baro.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_Declination.h>
#include <AP_Semaphore.h>
#include <Filter.h>
#include <AP_Buffer.h>
#include <SITL.h>
Arduino_Mega_ISR_Registry isr_registry;
AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
#endif
#include <AP_HAL_AVR.h>
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
static float rad_diff(float rad1, float rad2)
{
@ -54,7 +30,7 @@ static void check_result(float roll, float pitch, float yaw,
if (isnan(roll2) ||
isnan(pitch2) ||
isnan(yaw2)) {
Serial.printf("NAN eulers roll=%f pitch=%f yaw=%f\n",
hal.console->printf("NAN eulers roll=%f pitch=%f yaw=%f\n",
roll, pitch, yaw);
}
@ -73,11 +49,17 @@ static void check_result(float roll, float pitch, float yaw,
ToDeg(rad_diff(pitch, PI/2)) < 1 ||
ToDeg(rad_diff(pitch, -PI/2)) < 1) {
// we expect breakdown at these poles
Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));
hal.console->printf_P(
PSTR("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n"),
ToDeg(roll), ToDeg(roll2),
ToDeg(pitch), ToDeg(pitch2),
ToDeg(yaw), ToDeg(yaw2));
} else {
Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));
hal.console->printf_P(
PSTR("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n"),
ToDeg(roll), ToDeg(roll2),
ToDeg(pitch), ToDeg(pitch2),
ToDeg(yaw), ToDeg(yaw2));
}
}
}
@ -102,14 +84,14 @@ void test_matrix_eulers(void)
uint8_t i, j, k;
uint8_t N = ARRAY_LENGTH(angles);
Serial.println("rotation matrix unit tests\n");
hal.console->println("rotation matrix unit tests\n");
for (i=0; i<N; i++)
for (j=0; j<N; j++)
for (k=0; k<N; k++)
test_euler(angles[i], angles[j], angles[k]);
Serial.println("tests done\n");
hal.console->println("tests done\n");
}
static void test_quaternion(float roll, float pitch, float yaw)
@ -127,7 +109,7 @@ void test_quaternion_eulers(void)
uint8_t i, j, k;
uint8_t N = ARRAY_LENGTH(angles);
Serial.println("quaternion unit tests\n");
hal.console->println("quaternion unit tests\n");
test_quaternion(PI/4, 0, 0);
test_quaternion(0, PI/4, 0);
@ -152,7 +134,7 @@ void test_quaternion_eulers(void)
for (k=0; k<N; k++)
test_quaternion(angles[i], angles[j], angles[k]);
Serial.println("tests done\n");
hal.console->println("tests done\n");
}
@ -174,7 +156,7 @@ static void test_conversion(float roll, float pitch, float yaw)
m2.from_euler(roll, pitch, yaw);
m2.to_euler(&roll3, &pitch3, &yaw3);
if (m.is_nan()) {
Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n",
hal.console->printf("NAN matrix roll=%f pitch=%f yaw=%f\n",
roll, pitch, yaw);
}
@ -187,7 +169,7 @@ void test_conversions(void)
uint8_t i, j, k;
uint8_t N = ARRAY_LENGTH(angles);
Serial.println("matrix/quaternion tests\n");
hal.console->println("matrix/quaternion tests\n");
test_conversion(1, 1.1, 1.2);
test_conversion(1, -1.1, 1.2);
@ -200,7 +182,7 @@ void test_conversions(void)
for (k=0; k<N; k++)
test_conversion(angles[i], angles[j], angles[k]);
Serial.println("tests done\n");
hal.console->println("tests done\n");
}
void test_frame_transforms(void)
@ -209,12 +191,12 @@ void test_frame_transforms(void)
Quaternion q;
Matrix3f m;
Serial.println("frame transform tests\n");
hal.console->println("frame transform tests\n");
q.from_euler(ToRad(90), 0, 0);
v2 = v = Vector3f(0, 0, 1);
q.earth_to_body(v2);
printf("%f %f %f\n", v2.x, v2.y, v2.z);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
}
// generate a random float between -1 and 1
@ -258,7 +240,7 @@ void test_matrix_rotate(void)
float err = diff.a.length() + diff.b.length() + diff.c.length();
if (err > 0) {
Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err);
hal.console->printf("ERROR: i=%u err=%f\n", (unsigned)i, err);
}
}
}
@ -268,8 +250,7 @@ void test_matrix_rotate(void)
*/
void setup(void)
{
Serial.begin(115200);
Serial.println("euler unit tests\n");
hal.console->println("euler unit tests\n");
test_conversion(0, PI, 0);
@ -280,7 +261,6 @@ void setup(void)
test_matrix_rotate();
}
void
loop(void)
{
}
void loop(void){}
AP_HAL_MAIN();

0
libraries/AP_Math/examples/eulers/nocore.inoflag Normal file
View File

0
libraries/AP_Math/examples/location/Arduino.h Normal file
View File

51
libraries/AP_Math/examples/location/location.pde
View File

@ -3,36 +3,13 @@
// Unit tests for the AP_Math polygon code
//
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_HAL.h>
#include <AP_Math.h>
#ifdef DESKTOP_BUILD
// all of this is needed to build with SITL
#include <SPI.h>
#include <I2C.h>
#include <DataFlash.h>
#include <APM_RC.h>
#include <GCS_MAVLink.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_ADC.h>
#include <AP_Baro.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_Declination.h>
#include <AP_Semaphore.h>
#include <Filter.h>
#include <AP_Buffer.h>
#include <SITL.h>
Arduino_Mega_ISR_Registry isr_registry;
AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
SITL sitl;
#endif
FastSerialPort(Serial, 0);
#include <AP_HAL_AVR.h>
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
static const struct {
Vector2f wp1, wp2, location;
@ -73,17 +50,17 @@ static struct Location location_from_point(Vector2f pt)
static void test_passed_waypoint(void)
{
Serial.println("waypoint tests starting");
hal.console->println("waypoint tests starting");
for (uint8_t i=0; i<ARRAY_LENGTH(test_points); i++) {
struct Location loc = location_from_point(test_points[i].location);
struct Location wp1 = location_from_point(test_points[i].wp1);
struct Location wp2 = location_from_point(test_points[i].wp2);
if (location_passed_point(loc, wp1, wp2) != test_points[i].passed) {
Serial.printf("Failed waypoint test %u\n", (unsigned)i);
hal.console->printf("Failed waypoint test %u\n", (unsigned)i);
return;
}
}
Serial.println("waypoint tests OK");
hal.console->println("waypoint tests OK");
}
static void test_one_offset(struct Location &loc,
@ -94,10 +71,10 @@ static void test_one_offset(struct Location &loc,
float dist2, bearing2;
loc2 = loc;
uint32_t t1 = micros();
uint32_t t1 = hal.scheduler->micros();
location_offset(&loc2, ofs_north, ofs_east);
Serial.printf("location_offset took %u usec\n",
micros() - t1);
hal.console->printf("location_offset took %u usec\n",
hal.scheduler->micros() - t1);
dist2 = get_distance(&loc, &loc2);
bearing2 = get_bearing_cd(&loc, &loc2) * 0.01;
float brg_error = bearing2-bearing;
@ -109,7 +86,7 @@ static void test_one_offset(struct Location &loc,
if (fabs(dist - dist2) > 1.0 ||
brg_error > 1.0) {
Serial.printf("Failed offset test brg_error=%f dist_error=%f\n",
hal.console->printf("Failed offset test brg_error=%f dist_error=%f\n",
brg_error, dist-dist2);
}
}
@ -144,12 +121,10 @@ static void test_offset(void)
*/
void setup(void)
{
Serial.begin(115200);
test_passed_waypoint();
test_offset();
}
void
loop(void)
{
}
void loop(void){}
AP_HAL_MAIN();

0
libraries/AP_Math/examples/location/nocore.inoflag Normal file
View File

0
libraries/AP_Math/examples/polygon/Arduino.h Normal file
View File

0
libraries/AP_Math/examples/polygon/nocore.inoflag Normal file
View File

38
libraries/AP_Math/examples/polygon/polygon.pde
View File

@ -3,12 +3,13 @@
// Unit tests for the AP_Math polygon code
//
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
FastSerialPort(Serial, 0);
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
/*
* this is the boundary of the 2010 outback challenge
@ -68,23 +69,23 @@ void setup(void)
bool all_passed = true;
uint32_t start_time;
Serial.begin(115200);
Serial.println("polygon unit tests\n");
hal.console->println("polygon unit tests\n");
if (!Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary))) {
Serial.println("OBC boundary is not complete!");
hal.console->println("OBC boundary is not complete!");
all_passed = false;
}
if (Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary)-1)) {
Serial.println("Polygon_complete test failed");
hal.console->println("Polygon_complete test failed");
all_passed = false;
}
for (i=0; i<ARRAY_LENGTH(test_points); i++) {
bool result;
result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));
Serial.printf_P(PSTR("%10f,%10f %s %s\n"),
result = Polygon_outside(test_points[i].point,
OBC_boundary, ARRAY_LENGTH(OBC_boundary));
hal.console->printf_P(PSTR("%10f,%10f %s %s\n"),
1.0e-7*test_points[i].point.x,
1.0e-7*test_points[i].point.y,
result ? "OUTSIDE" : "INSIDE ",
@ -93,24 +94,25 @@ void setup(void)
all_passed = false;
}
}
Serial.println(all_passed ? "TEST PASSED" : "TEST FAILED");
hal.console->println(all_passed ? "TEST PASSED" : "TEST FAILED");
Serial.println("Speed test:");
start_time = micros();
hal.console->println("Speed test:");
start_time = hal.scheduler->micros();
for (count=0; count<1000; count++) {
for (i=0; i<ARRAY_LENGTH(test_points); i++) {
bool result;
result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));
result = Polygon_outside(test_points[i].point,
OBC_boundary, ARRAY_LENGTH(OBC_boundary));
if (result != test_points[i].outside) {
all_passed = false;
}
}
}
Serial.printf("%u usec/call\n", (unsigned)((micros() - start_time)/(count*ARRAY_LENGTH(test_points))));
Serial.println(all_passed ? "ALL TESTS PASSED" : "TEST FAILED");
hal.console->printf("%u usec/call\n", (unsigned)((hal.scheduler->micros()
- start_time)/(count*ARRAY_LENGTH(test_points))));
hal.console->println(all_passed ? "ALL TESTS PASSED" : "TEST FAILED");
}
void
loop(void)
{
}
void loop(void){}
AP_HAL_MAIN();

0
libraries/AP_Math/examples/rotations/Arduino.h Normal file
View File

0
libraries/AP_Math/examples/rotations/nocore.inoflag Normal file
View File

68
libraries/AP_Math/examples/rotations/rotations.pde
View File

@ -2,37 +2,15 @@
//
// Unit tests for the AP_Math rotations code
//
#include <FastSerial.h>
#include <stdlib.h>
#include <AP_Common.h>
#include <AP_HAL.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
FastSerialPort(Serial, 0);
#ifdef DESKTOP_BUILD
// all of this is needed to build with SITL
#include <SPI.h>
#include <I2C.h>
#include <DataFlash.h>
#include <APM_RC.h>
#include <GCS_MAVLink.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_ADC.h>
#include <AP_Baro.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_Semaphore.h>
#include <Filter.h>
#include <AP_Buffer.h>
#include <SITL.h>
Arduino_Mega_ISR_Registry isr_registry;
AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
#endif
#include <AP_HAL_AVR.h>
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
// standard rotation matrices (these are the originals from the old code)
#define MATRIX_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0,0, 1)
@ -54,7 +32,7 @@ AP_Compass_HIL compass;
static void print_matrix(Matrix3f &m)
{
Serial.printf("[%.2f %.2f %.2f] [%.2f %.2f %.2f] [%.2f %.2f %.2f]\n",
hal.console->printf("[%.2f %.2f %.2f] [%.2f %.2f %.2f] [%.2f %.2f %.2f]\n",
m.a.x, m.a.y, m.a.z,
m.b.x, m.b.y, m.b.z,
m.c.x, m.c.y, m.c.z);
@ -70,7 +48,7 @@ static void test_matrix(enum Rotation rotation, Matrix3f m)
if (diff.a.length() > accuracy ||
diff.b.length() > accuracy ||
diff.c.length() > accuracy) {
Serial.printf("rotation matrix %u incorrect\n", (unsigned)rotation);
hal.console->printf("rotation matrix %u incorrect\n", (unsigned)rotation);
print_matrix(m);
print_matrix(m2);
}
@ -79,7 +57,7 @@ static void test_matrix(enum Rotation rotation, Matrix3f m)
// test generation of rotation matrices
static void test_matrices(void)
{
Serial.println("testing rotation matrices\n");
hal.console->println("testing rotation matrices\n");
test_matrix(ROTATION_NONE, MATRIX_ROTATION_NONE);
test_matrix(ROTATION_YAW_45, MATRIX_ROTATION_YAW_45);
test_matrix(ROTATION_YAW_90, MATRIX_ROTATION_YAW_90);
@ -109,13 +87,13 @@ static void test_vector(enum Rotation rotation, Vector3f v1, bool show=true)
v2 = m * v2;
diff = v1 - v2;
if (diff.length() > 1.0e-6) {
Serial.printf("rotation vector %u incorrect\n", (unsigned)rotation);
Serial.printf("%u %f %f %f\n",
hal.console->printf("rotation vector %u incorrect\n", (unsigned)rotation);
hal.console->printf("%u %f %f %f\n",
(unsigned)rotation,
v2.x, v2.y, v2.z);
}
if (show) {
Serial.printf("%u %f %f %f\n",
hal.console->printf("%u %f %f %f\n",
(unsigned)rotation,
v1.x, v1.y, v1.z);
}
@ -150,7 +128,7 @@ static void test_vector(enum Rotation rotation)
// test rotation of vectors
static void test_vectors(void)
{
Serial.println("testing rotation of vectors\n");
hal.console->println("testing rotation of vectors\n");
test_vector(ROTATION_NONE);
test_vector(ROTATION_YAW_45);
test_vector(ROTATION_YAW_90);
@ -182,10 +160,10 @@ static void test_combinations(void)
r2 = (enum Rotation)((uint8_t)r2+1)) {
r3 = rotation_combination(r1, r2, &found);
if (found) {
Serial.printf("rotation: %u + %u -> %u\n",
hal.console->printf("rotation: %u + %u -> %u\n",
(unsigned)r1, (unsigned)r2, (unsigned)r3);
} else {
Serial.printf("ERROR rotation: no combination for %u + %u\n",
hal.console->printf("ERROR rotation: no combination for %u + %u\n",
(unsigned)r1, (unsigned)r2);
}
}
@ -201,7 +179,7 @@ static void test_rotation_accuracy(void)
int16_t i;
float rot_angle;
Serial.println("\nRotation method accuracy:");
hal.console->println_P(PSTR("\nRotation method accuracy:"));
for( i=0; i<90; i++ ) {
@ -219,7 +197,9 @@ static void test_rotation_accuracy(void)
attitude.to_euler(&roll, &pitch, &yaw);
// display results
Serial.printf_P(PSTR("actual angle: %d\tcalculated angle:%4.2f\n"),(int)i,ToDeg(yaw));
hal.console->printf_P(
PSTR("actual angle: %d\tcalculated angle:%4.2f\n"),
(int)i,ToDeg(yaw));
}
}
@ -228,16 +208,14 @@ static void test_rotation_accuracy(void)
*/
void setup(void)
{
Serial.begin(115200);
Serial.println("rotation unit tests\n");
hal.console->println("rotation unit tests\n");
test_matrices();
test_vectors();
test_combinations();
test_rotation_accuracy();
Serial.println("rotation unit tests done\n");
hal.console->println("rotation unit tests done\n");
}
void
loop(void)
{
}
void loop(void) {}
AP_HAL_MAIN();

3
libraries/AP_Math/location.cpp
View File

@ -20,8 +20,7 @@
/*
* this module deals with calculations involving struct Location
*/
#include <FastSerial.h>
#include <stdlib.h>
#include "AP_Math.h"
// radius of earth in meters