AP_Math: port to work on coreless arduino (AP_HAL)

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
 

libraries/AP_Math/examples/eulers/eulers.pde

@@ -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);
+#include <AP_HAL_AVR.h>
-
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
-#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
-
-
-
 
 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",
+            hal.console->printf_P(
-                          ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));
+                    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",
+            hal.console->printf_P(
-                          ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));
+                    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);
+    hal.console->println("euler unit tests\n");
-    Serial.println("euler unit tests\n");
 
     test_conversion(0, PI, 0);
 
@@ -280,7 +261,6 @@ void setup(void)
     test_matrix_rotate();
 }
 
-void
+void loop(void){}
-loop(void)
+
-{
+AP_HAL_MAIN();
-}

libraries/AP_Math/examples/location/location.pde

@@ -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
+#include <AP_HAL_AVR.h>
-// all of this is needed to build with SITL
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
- #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);
 
 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",
+    hal.console->printf("location_offset took %u usec\n",
-                  micros() - t1);
+                  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
+void loop(void){}
-loop(void)
+
-{
+AP_HAL_MAIN();
-}

libraries/AP_Math/examples/polygon/polygon.pde

@@ -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);
+    hal.console->println("polygon unit tests\n");
-    Serial.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));
+        result = Polygon_outside(test_points[i].point,
-        Serial.printf_P(PSTR("%10f,%10f  %s  %s\n"),
+                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:");
+    hal.console->println("Speed test:");
-    start_time = micros();
+    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))));
+    hal.console->printf("%u usec/call\n", (unsigned)((hal.scheduler->micros() 
-    Serial.println(all_passed ? "ALL TESTS PASSED" : "TEST FAILED");
+                    - start_time)/(count*ARRAY_LENGTH(test_points))));
+    hal.console->println(all_passed ? "ALL TESTS PASSED" : "TEST FAILED");
 }
 
-void
+void loop(void){}
-loop(void)
+
-{
+AP_HAL_MAIN();
-}

libraries/AP_Math/examples/rotations/rotations.pde

@@ -2,37 +2,15 @@
 //
 // Unit tests for the AP_Math rotations code
 //
-
+#include <stdlib.h>
-#include <FastSerial.h>
 #include <AP_Common.h>
+#include <AP_HAL.h>
 #include <AP_Param.h>
 #include <AP_Math.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> // 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);
+        hal.console->printf("rotation vector %u incorrect\n", (unsigned)rotation);
-        Serial.printf("%u  %f %f %f\n",
+        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);
+    hal.console->println("rotation unit tests\n");
-    Serial.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
+void loop(void) {}
-loop(void)
+
-{
+AP_HAL_MAIN();
-}

libraries/AP_Math/location.cpp

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