mjs513
Senior Member+
Hi all,
I have been trying to implement a waypoint navigation method that will generate intermediate flyby waypoints that is based on what was implemented in Ardupilot. What I did was to take the sections needed to generate the points and put them in a library. That is seems to work ok at least everything compiles and runs. But in my debugging process to test the code I am running into a problem that I think is a conversion issue (don't know it that's it or something else in the code). Anyway I will try and walk through the code pieces to demonstrate what I am talking about. Again I am using a T3.5 (also tried T3.2 with same results), TD1.40 and IDE1.8.5 on a Windows 10 machine.
first I setting up a series of test points like so:
vector2f comes from ardupilot vector2 library and is based on templates. This is a portion of the lib:
vector2.h:
In the ino file I have:
stuct Location is defined in location.h as:
When I do some debug prints:
setting
results in:
which should be
I am attaching everything in a couple of zip files - just in case.
View attachment Flyby_Waypoint.zip View attachment AP_Math_freeimu.zip
Any help would be appreciated
thanks
Mike
I have been trying to implement a waypoint navigation method that will generate intermediate flyby waypoints that is based on what was implemented in Ardupilot. What I did was to take the sections needed to generate the points and put them in a library. That is seems to work ok at least everything compiles and runs. But in my debugging process to test the code I am running into a problem that I think is a conversion issue (don't know it that's it or something else in the code). Anyway I will try and walk through the code pieces to demonstrate what I am talking about. Again I am using a T3.5 (also tried T3.2 with same results), TD1.40 and IDE1.8.5 on a Windows 10 machine.
first I setting up a series of test points like so:
Code:
Vector2f test_points1[] = {
Vector2f(-117.041076, 32.844287),
Vector2f(-117.037579, 32.844269),
Vector2f(-117.038373, 32.841664),
Vector2f(-117.035937, 32.840222),
Vector2f(-117.033448, 32.842854),
Vector2f(-117.032762, 32.845774),
Vector2f(-117.039585, 32.846820)
};
vector2f comes from ardupilot vector2 library and is based on templates. This is a portion of the lib:
vector2.h:
Code:
#ifndef VECTOR2_H
#define VECTOR2_H
#include <math.h>
template <typename T>
struct Vector2
{
T x, y;
// trivial ctor
Vector2<T>() {
x = y = 0;
}
// setting ctor
Vector2<T>(const T x0, const T y0) : x(x0), y(y0) {
}
// function call operator
void operator ()(const T x0, const T y0)
{
x= x0; y= y0;
}
....
typedef Vector2<int16_t> Vector2i;
typedef Vector2<uint16_t> Vector2ui;
typedef Vector2<int32_t> Vector2l;
typedef Vector2<uint32_t> Vector2ul;
typedef Vector2<float> Vector2f;
In the ino file I have:
Code:
struct Location loc;
static struct Location location_from_point(Vector2f pt)
{
loc.lat = pt.x * 1.0e7f;
loc.lng = pt.y * 1.0e7f;
loc.alt = 0;
return loc;
}
stuct Location is defined in location.h as:
Code:
struct Location
{
long lat = 0;
long lng = 0;
long alt = 0;
};
When I do some debug prints:
Code:
test_points1[0].x = -117.04107666 when it should read -117.041076, very small delta - probably ok
Code:
struct Location Aloc = location_from_point(test_points1[i]);
Code:
A(1)=-1170410752; A(2)=328442880;
Code:
-1170410760, 328442870
I am attaching everything in a couple of zip files - just in case.
View attachment Flyby_Waypoint.zip View attachment AP_Math_freeimu.zip
Any help would be appreciated
thanks
Mike