mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
uncrustify libraries/AP_Math/examples/location/location.pde
This commit is contained in:
uncrustify
Pat Hickey
parent
a40691e874
commit
f11f64108a
@ -9,81 +9,81 @@
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
// all of this is needed to build with SITL
|
||||
#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 <Filter.h>
|
||||
#include <SITL.h>
|
||||
#include <I2C.h>
|
||||
#include <SPI.h>
|
||||
#include <AP_Declination.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 <Filter.h>
|
||||
#include <SITL.h>
|
||||
#include <I2C.h>
|
||||
#include <SPI.h>
|
||||
#include <AP_Declination.h>
|
||||
Arduino_Mega_ISR_Registry isr_registry;
|
||||
AP_Baro_BMP085_HIL barometer;
|
||||
AP_Compass_HIL compass;
|
||||
AP_Baro_BMP085_HIL barometer;
|
||||
AP_Compass_HIL compass;
|
||||
SITL sitl;
|
||||
#endif
|
||||
|
||||
FastSerialPort(Serial, 0);
|
||||
|
||||
static const struct {
|
||||
Vector2f wp1, wp2, location;
|
||||
bool passed;
|
||||
Vector2f wp1, wp2, location;
|
||||
bool passed;
|
||||
} test_points[] = {
|
||||
{ Vector2f(-35.3647759314918, 149.16265692810987),
|
||||
Vector2f(-35.36279922658029, 149.16352169591426),
|
||||
Vector2f(-35.36214956969903, 149.16461410046492), true },
|
||||
{ Vector2f(-35.36438601157189, 149.16613916088568),
|
||||
Vector2f(-35.364432558610254, 149.16287313113048),
|
||||
Vector2f(-35.36491510034746, 149.16365837225004), false },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(0, 1),
|
||||
Vector2f(0, 2), true },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(0, 2),
|
||||
Vector2f(0, 1), false },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(1, 0),
|
||||
Vector2f(2, 0), true },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(2, 0),
|
||||
Vector2f(1, 0), false },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(-1, 1),
|
||||
Vector2f(-2, 2), true },
|
||||
{ Vector2f(-35.3647759314918, 149.16265692810987),
|
||||
Vector2f(-35.36279922658029, 149.16352169591426),
|
||||
Vector2f(-35.36214956969903, 149.16461410046492), true },
|
||||
{ Vector2f(-35.36438601157189, 149.16613916088568),
|
||||
Vector2f(-35.364432558610254, 149.16287313113048),
|
||||
Vector2f(-35.36491510034746, 149.16365837225004), false },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(0, 1),
|
||||
Vector2f(0, 2), true },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(0, 2),
|
||||
Vector2f(0, 1), false },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(1, 0),
|
||||
Vector2f(2, 0), true },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(2, 0),
|
||||
Vector2f(1, 0), false },
|
||||
{ Vector2f(0, 0),
|
||||
Vector2f(-1, 1),
|
||||
Vector2f(-2, 2), true },
|
||||
};
|
||||
|
||||
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
|
||||
|
||||
static struct Location location_from_point(Vector2f pt)
|
||||
{
|
||||
struct Location loc = {0};
|
||||
loc.lat = pt.x * 1.0e7;
|
||||
loc.lng = pt.y * 1.0e7;
|
||||
return loc;
|
||||
struct Location loc = {0};
|
||||
loc.lat = pt.x * 1.0e7;
|
||||
loc.lng = pt.y * 1.0e7;
|
||||
return loc;
|
||||
}
|
||||
|
||||
static void test_passed_waypoint(void)
|
||||
{
|
||||
Serial.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);
|
||||
return;
|
||||
}
|
||||
}
|
||||
Serial.println("waypoint tests OK");
|
||||
Serial.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);
|
||||
return;
|
||||
}
|
||||
}
|
||||
Serial.println("waypoint tests OK");
|
||||
}
|
||||
|
||||
static void test_one_offset(struct Location &loc,
|
||||
static void test_one_offset(struct Location &loc,
|
||||
float ofs_north, float ofs_east,
|
||||
float dist, float bearing)
|
||||
{
|
||||
@ -93,7 +93,7 @@ static void test_one_offset(struct Location &loc,
|
||||
loc2 = loc;
|
||||
uint32_t t1 = micros();
|
||||
location_offset(&loc2, ofs_north, ofs_east);
|
||||
Serial.printf("location_offset took %u usec\n",
|
||||
Serial.printf("location_offset took %u usec\n",
|
||||
micros() - t1);
|
||||
dist2 = get_distance(&loc, &loc2);
|
||||
bearing2 = get_bearing_cd(&loc, &loc2) * 0.01;
|
||||
@ -126,18 +126,18 @@ static void test_offset(void)
|
||||
|
||||
loc.lat = -35*1.0e7;
|
||||
loc.lng = 149*1.0e7;
|
||||
|
||||
for (uint8_t i=0; i<ARRAY_LENGTH(test_offsets); i++) {
|
||||
test_one_offset(loc,
|
||||
test_offsets[i].ofs_north,
|
||||
test_offsets[i].ofs_east,
|
||||
|
||||
for (uint8_t i=0; i<ARRAY_LENGTH(test_offsets); i++) {
|
||||
test_one_offset(loc,
|
||||
test_offsets[i].ofs_north,
|
||||
test_offsets[i].ofs_east,
|
||||
test_offsets[i].distance,
|
||||
test_offsets[i].bearing);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
polygon tests
|
||||
* polygon tests
|
||||
*/
|
||||
void setup(void)
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user