From 7dbe198e5c96eed48e2d2ed3b2a66874d6ff1173 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Tue, 18 Sep 2012 11:08:18 -0700 Subject: [PATCH] AP_Math: port to work on coreless arduino (AP_HAL) --- libraries/AP_Math/AP_Math.h | 18 +++++ libraries/AP_Math/examples/eulers/Arduino.h | 0 libraries/AP_Math/examples/eulers/eulers.pde | 78 +++++++------------ .../AP_Math/examples/eulers/nocore.inoflag | 0 libraries/AP_Math/examples/location/Arduino.h | 0 .../AP_Math/examples/location/location.pde | 51 ++++-------- .../AP_Math/examples/location/nocore.inoflag | 0 libraries/AP_Math/examples/polygon/Arduino.h | 0 .../AP_Math/examples/polygon/nocore.inoflag | 0 .../AP_Math/examples/polygon/polygon.pde | 38 ++++----- .../AP_Math/examples/rotations/Arduino.h | 0 .../AP_Math/examples/rotations/nocore.inoflag | 0 .../AP_Math/examples/rotations/rotations.pde | 68 ++++++---------- libraries/AP_Math/location.cpp | 3 +- 14 files changed, 104 insertions(+), 152 deletions(-) create mode 100644 libraries/AP_Math/examples/eulers/Arduino.h create mode 100644 libraries/AP_Math/examples/eulers/nocore.inoflag create mode 100644 libraries/AP_Math/examples/location/Arduino.h create mode 100644 libraries/AP_Math/examples/location/nocore.inoflag create mode 100644 libraries/AP_Math/examples/polygon/Arduino.h create mode 100644 libraries/AP_Math/examples/polygon/nocore.inoflag create mode 100644 libraries/AP_Math/examples/rotations/Arduino.h create mode 100644 libraries/AP_Math/examples/rotations/nocore.inoflag diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index c7a6f7eee8..300bd81bbd 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -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 diff --git a/libraries/AP_Math/examples/eulers/Arduino.h b/libraries/AP_Math/examples/eulers/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Math/examples/eulers/eulers.pde b/libraries/AP_Math/examples/eulers/eulers.pde index 72fb6c9cec..fe29b991b3 100644 --- a/libraries/AP_Math/examples/eulers/eulers.pde +++ b/libraries/AP_Math/examples/eulers/eulers.pde @@ -3,38 +3,14 @@ // Unit tests for the AP_Math euler code // -#include +#include #include #include #include +#include -FastSerialPort(Serial, 0); - -#ifdef DESKTOP_BUILD -// all of this is needed to build with SITL - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include -Arduino_Mega_ISR_Registry isr_registry; -AP_Baro_BMP085_HIL barometer; -AP_Compass_HIL compass; -#endif - - - +#include +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; iprintln("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; kprintln("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; kprintln("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(); diff --git a/libraries/AP_Math/examples/eulers/nocore.inoflag b/libraries/AP_Math/examples/eulers/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Math/examples/location/Arduino.h b/libraries/AP_Math/examples/location/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Math/examples/location/location.pde b/libraries/AP_Math/examples/location/location.pde index 2f549fe2fb..c9a11a8701 100644 --- a/libraries/AP_Math/examples/location/location.pde +++ b/libraries/AP_Math/examples/location/location.pde @@ -3,36 +3,13 @@ // Unit tests for the AP_Math polygon code // -#include #include #include +#include #include -#ifdef DESKTOP_BUILD -// all of this is needed to build with SITL - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include -Arduino_Mega_ISR_Registry isr_registry; -AP_Baro_BMP085_HIL barometer; -AP_Compass_HIL compass; -SITL sitl; -#endif - -FastSerialPort(Serial, 0); +#include +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; iprintf("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(); diff --git a/libraries/AP_Math/examples/location/nocore.inoflag b/libraries/AP_Math/examples/location/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Math/examples/polygon/Arduino.h b/libraries/AP_Math/examples/polygon/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Math/examples/polygon/nocore.inoflag b/libraries/AP_Math/examples/polygon/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Math/examples/polygon/polygon.pde b/libraries/AP_Math/examples/polygon/polygon.pde index 56f23196a8..a52f964e86 100644 --- a/libraries/AP_Math/examples/polygon/polygon.pde +++ b/libraries/AP_Math/examples/polygon/polygon.pde @@ -3,12 +3,13 @@ // Unit tests for the AP_Math polygon code // -#include #include #include #include +#include +#include -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; iprintf_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; iprintf("%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(); diff --git a/libraries/AP_Math/examples/rotations/Arduino.h b/libraries/AP_Math/examples/rotations/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Math/examples/rotations/nocore.inoflag b/libraries/AP_Math/examples/rotations/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index b093dffa97..0fc7630aac 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -2,37 +2,15 @@ // // Unit tests for the AP_Math rotations code // - -#include +#include #include +#include #include #include +#include // ArduPilot Mega Declination Helper Library -FastSerialPort(Serial, 0); - -#ifdef DESKTOP_BUILD -// all of this is needed to build with SITL - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include // ArduPilot Mega Declination Helper Library - #include - #include - #include - #include -Arduino_Mega_ISR_Registry isr_registry; -AP_Baro_BMP085_HIL barometer; -AP_Compass_HIL compass; -#endif - +#include +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(); diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index 9c9a6948b9..0671d54f1f 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -20,8 +20,7 @@ /* * this module deals with calculations involving struct Location */ - -#include +#include #include "AP_Math.h" // radius of earth in meters