During a flight, say with a pixhawk, I want to save the current location of the drone in reference to its initial start location. Ideally that would be the x, y, and z positions in meters. I understand that you can save the lat, lon, or IMU/velocity readings using dronekit, then calculate the position. It would be awesome, however, to be able to just call a function that calculates the x, y, and z for you so there is no post-processing.
The following was satisfactory for my purposes (I believe you would need a GPS but not positive):
from dronekit import connect
vehicle = connect("Connection String", baud=57600)
metersNorth = vehicle.location.local_frame.north # used for the y position
metersEast = vehicle.location.local_frame.east # used for the x position
altitude = vehicle.location.local_frame.down # used for the z position
Or in my case I had a rangefinder facing down
z = vehicle.rangefinder.distance
Related
I need help or idea how to make an algorithm for a pool game/program.
I need this as a project for my collage so this needs to be written as simply as possible using only basic and loops,sadly I can't use arrays or functions.
The whole point of the program is to calculate how many steps/hits on the corner of the table will the ball need to reach its starting point (Assumed that ball has infinite velocity and that there are no other balls etc.. Simply we assume that its possible)
We need to look at the table as a coordinate system with (m x n) dimensions, and we need to input starting point of the ball (x1,y1) and the point where the ball will hit the table for the first time (x2,y2) based on that info we need to make an algorithm that will calculate how many hits/steps will the ball need to reach its starting point.
I had an idea to use the fact that ball will have same angle on impact which means that if the ball hits the table on 30 degrees angle it will repel off the table with same angle to the other side. So because the tables is set as a coordinate system I could use atan to calculate the angle and then with that info to calculate the distance from the other point. I made a program with the info I know but this won't work because this is only one calculation (I guess that I will need to use for loop(s) to calculate everything. And I have a big problem in understanding how to set the edge of the table because some of my calculations goes outside the (m x n) space.
I will attach 2 pictures that were given to us as examples and part of the code that I use. I hope that someone can help me and I'm sorry if I made a lot of grammar mistakes, English is not my primary language.
Thanks in advance.
PICTURES :
Orange dot is the starting point, orange line is the first hit that we input,everything else needs to be calculated:
Same as picture one but more complex example:
MY CODE:
#include <stdio.h>
#include <math.h>
#define PI 3.14159265359
int main() {
int i;
double x1,y1,x2,y2;
int n,m;
double angle;
double xPrime,yPrime;
double distance;
double m1;
printf("Enter the staring point (x1,y1) and first hit point(x2,y2) ");
scanf("%lf %lf %lf %lf",&x1,&y1,&x2,&y2);
printf("Enter the dimensions (n x m ) ");
scanf("%d %d",&n,&m);
m1=(y2-y1)/(x2-x1);
angle=atan(m1);
distance =sqrt( pow((x2-x1),2) + pow((y2-y1),2));
xPrim = x2 + (-distanca * cos(ugao));
yPrim = y2 + (distanca * sin(ugao));
return 0;
}
You should consider the X and Y axes separately. The discussion below assumes that the ball initially starts moving in the positive direction on both axes. If it doesn't then flip stuff around to the symmetrical situation so that it does.
Given the table size by M x N, the starting point (x0,y0), and the first hit (x1,y1):
Let T be the time it takes to get from (x0,y0) to (x1,y1), so the initial velocity (in units per T) is (vx,vy) = (x1-x0, y1-y0)
Considering the X axis, the ball will hit a wall at time (M-x0)/vx, measured in units of T. It will get back to the starting X coordinate at time (2M-2x0)/vx. It will hit the other wall at time (2M-x0)/vx and get back to the start position again at time 2M/vx. Then the cycle will repeat.
So, the times at which it will get back to the start position are (2aM - 2x0)/vx and 2aM/vx, for all integer a>0. The times it will hit a wall are (2aM - x0) and ((2a-1)M - x0).
Similarly, on the Y axis, the times it will get back to the start position are (2bN - 2y0)/vy and 2bN/vy, for all integer b>0. The times it will hit a wall are (2bN - y0) and ((2b-1)N - y0).
So, to find times when the ball gets back to the start, solve:
(2aM - 2x0)/vx = (2bN - 2y0)/vy or
(2aM - 2x0)/vx = 2bN/vy or
2aM/vx = (2bN - 2y0)/vy or
2aM/vx = 2bN/vy
for the smallest integers a>0 and b>0.
All of these reduce to linear Diophantine equations that are readily solved: https://en.wikipedia.org/wiki/Diophantine_equation
Once you know how long it takes to get back to the start, you can use the other equations above to calculate how many wall hits were made.
I am trying to figure out a function for recording activity of a value, but still with no success. I will describe everything as detailed as possible to make the comfort you may need to suggest the answer.
I have the function lineto(x, y) that draws a line to x, y position
and changes the offset to the same x, y.
I have the function moveto(x, y) that moves the offset to x, y position.
The initial coordinates of the area are x0:y0 which is the top left corner
What I want to achieve is this:
i.e I specify variable, minimum and maximum and the function
draws a line to the current state of the value, where a full height line will be the maximum. Also assume that height is a variable that represents the height of the graph area (box).
What I have for now is this:
(((current/height)*(range.maximum/height))/height)));
..Which I think.. is a complete nonsense.
So you basically need to translate the interval [minimum, maximum] to the interval [0, height].
Using math :
[minimum, maximum] (-minimum) ->
[0, maximum-minimum] (/(max-min)) ->
[0, 1] -> (* height)
So the formula to represent x from [min,max] space in [0,height] space is
xNew = ( (x-minimum)/(maximum-minimum) )* height; // extra pharathesis for clarity
For your current variable you would have
currentInBox = ( (current - minimum) / (maximum-minimum) ) * height
I think I figured it out with this algorithm:
(((current-range.minimum)*height) / range.maximum-range.minimum)
I used my topercent function partial * 100 / full as a template.
i'm trying to do something in C, I'm building a server that will receive a Latitude and Longitude coordinate, -23.0001, -43.3417 to be exact, i'm trying to make a 10km radius circle around that coordinate, is it possible ? I gave up trying to make a circle and was trying to make a square with 20km sides, where the coordinate I gave is the center, but I keep failing, here is what I'm trying
Quad *cria_quadrado_complex(Coords *b)
{
Quad* a = (Quad*) malloc(sizeof(Quad));
a->x1 = b->x + 0.0433;
a->x2 = b->x + 0.0587;
a->y1 = b->y + 0.0433;
a->y2 = b->y + 0.0490;
return a;
}
the structs used are:
struct coordenadas
{
double x,
y;
};
struct quadrado
{
double x1,
x2,
y1,
y2;
};
typedef struct coordenadas Coords;
typedef struct quadrado Quad;
those 0.0 something values are values I measured from google maps but they are faar from the center and dindn't work, is there a better way to do that ? help
ps -23.0001, -43.3417 are coordinates from google maps
here is an example of what i'm trying to do:
1---------- 1 : top coordinate of the square
-----------
-----C----- C : center coordinate (-23.0001, -43.3417)
-----------
----------2 2 : bottom coordinate of the square
1 and 2 I want to generate automatically with the center coordinates, they will be away from the center so the sides of the square is 10 km
It certainly is possible, but you'll want to get the right formula. A good start might be to read http://www.movable-type.co.uk/scripts/gis-faq-5.1.html.
You don't say why you're doing this in C, and I can't tell if you need to calculate this for an arbitrary lat/lng or just for the one that you give. If you can do it in another language or find a C library that will process KML, and if you only need the one set of coordinates to work, you can use the tool at http://www.freemaptools.com/radius-around-point.htm to generate KML or a Google Maps Static API URL. If you're only concerned about the single pair of coordinates you specify, a hardcoded KML file may be the way to go.
Basically I've got some structs of type Ship which are going to go on a board which can have a variable width and height. The information about the ships is read in from a file, and I just need to know the best way to make sure that none of the ships overlap.
Here is the structure of Ship:
int x // x position of first part of ship
int y // y position of first part of ship
char dir // direction of the ship, either 'N','S','E' or 'W'
int length // length of the ship
Also, what would be a good way to handle the directions. Something cleaner than using a switch statement and using a different condition for each direction.
Any help would be greatly appreciated!
You could keep a boolean array of the entire grid, initially initialized to "false." For each ship, for each location the ship covers, check if the location is "false." If it is,
set it to "true". If not, then some other ship is on the location.
This algorithm is linear in the total area of all the ships, but also requires extra space
proportional to the number of locations on the board.
This is the same thing as a test whether rectangles intersect, I think your code would be simpler if you don't think of these ships as a point,length, and direction but as a rectangle.
So convert this
int x // x position of first part of ship
int y // y position of first part of ship
char dir // direction of the ship, either 'N','S','E' or 'W'
int length // length of the ship
to this (allow negative cx & cy to get N,S,E,W)
int x // x position of first part of ship
int y // y position of first part of ship
int cx // length of the ship in X
int cy // length of the ship in Y
or this
int left // x position of Eastern part of the ship
int top // y position of Northernmost part of ship
int right // x position of Westernmost part of the ship
int bottom // y position of Southernmost part of ship
bool orientation; // so we can tell East from West or North from South.
Then a simple function can determine if two ships intersect.
bool DoShipsIntersect(Ship * a, Ship * b)
{
if ((a->right < b->left) || (b->right < a->left))
return false;
if ((a->bottom < b->top) || (b->bottom < a->top))
return false;
return true;
}
A brute force compare of every ship to every other ship should be quite fast as long as you don't have thousands of ships.
Unless you have a lot of ships then just use a simple brute force algorithm, which will be two nested loops, i.e. O(n^2).
Keep a bitmap of the board where each bit indicates whether there's a ship occupying that tile. For each ship mark the tiles it occupies on the board, and check out if you mark the same bit twice.
(Battleship?)
Make a 2D array ("board") which contains the ID of the ship when one is present. Therefore, when you add the ship you can check in O(length) time whether a space is occupied or not.
O(n * length) time, O(N^2) space.
One way of representing a direction is as a unit vector. This can be 2 integers: dirX and dirY. Eg. dirX=1 for East; or dirY=1 for South.
You can then iterate over all positions occupied by a ship with:
int cx = x;
int cy = y;
for(int i = 0; i < length; i++) {
cx += dirX;
cy += dirY;
}
Or get a bounding-box based on these values:
x
y
x + dirX * (length - 1)
y + dirY * (length - 1)
You might want to use an enum type for your direction. In terms of finding overlaps, create a two dimensional array of booleans initialized to false for your board. Then, for each ship, finding the corresponding entries in the array and, if they are already true, you have an overlap. Otherwise, set those entries to true. If you have placed all your ships and didn't encounter an already true entry, then there is no overlap.
I want to be able to move a particle in a straight line within a 3D environment but I can't think how to work out the next location based on two points within a 3D space?
I have created a struct which represents a particle which has a location and a next location? Would this be suitable to work out the next location to move too? I know how to initially set the next location using the following method:
// Set particle's direction to a random direction
void setDirection(struct particle *p)
{
float xnm = (p->location.x * -1) - p->velocity;
float xnp = p->location.x + p->velocity;
float ynm = (p->location.y * -1) - p->velocity;
float ynp = p->location.y + p->velocity;
float znm = (p->location.z * -1) - p->velocity;
float znp = p->location.z + p->velocity;
struct point3f nextLocation = { randFloat(xnm, xnp), randFloat(ynm, ynp), randFloat(znm, znp) };
p->nextLocation = nextLocation;
}
The structs I have used are:
// Represents a 3D point
struct point3f
{
float x;
float y;
float z;
};
// Represents a particle
struct particle
{
enum TYPES type;
float radius;
float velocity;
struct point3f location;
struct point3f nextLocation;
struct point3f colour;
};
Am I going about this completely the wrong way?
here's all my code http://pastebin.com/m469f73c2
The other answer is a little mathish, it's actually pretty straight forward.
You need a "Velocity" which you are moving. It also has x, y and z coordinates.
In one time period, to move you just add the x velocity to your x position to get your new x position, repeat for y and z.
On top of that, you can have an "Acceleration" (also x,y,z) For instance, your z acceleration could be gravity, a constant.
Every time period your velocity should be recalcualted in the same way, Call velocity x "vx", so vx should become vx + ax, repeat for y and z (again).
It's been a while since math, but that's how I remember it, pretty straight forward unless you need to keep track of units, then it gets a little more interesting (but still not bad)
I'd suggest that a particle should only have one location member -- the current location. Also, the velocity should ideally be a vector of 3 components itself. Create a function (call it move, displace whatever) that takes a particle and a time duration t. This will compute the final position after t units of time has elapsed:
struct point3f move(struct *particle, int time) {
particle->location->x = particle->velocity->x * t;
/* and so on for the other 2 dimensions */
return particle->location;
}
I would recomend two things:
read an article or two on basic vector math for animation. For instance, this is a site that explains 2d vectors for flash.
start simple, start with a 1d point, ie a point only moving along x. Then try adding a second dimension (a 2d point in a 2d space) and third dimension. This might help you get a better understanding of the underlying mechanics.
hope this helps
Think of physics. An object has a position (x, y, z) and a movement vector (a, b, c). Your object should exist at its position; it has a movement vector associated with it that describes its momentum. In the lack of any additional forces on the object, and assuming that your movement vector describes the movement over a time period t, the position of your object at time x will be (x + (at), y + (bt), z + (c*t)).
In short; don't store the current position and the next position. Store the current position and the object's momentum. It's easy enough to "tick the clock" and update the location of the object by simply adding the momentum to the position.
Store velocity as a struct point3f, and then you have something like this:
void move(struct particle * p)
{
p->position.x += p->velocity.x;
p->position.y += p->velocity.y;
p->position.z += p->velocity.z;
}
Essentially the velocity is how much you want the position to change each second/tick/whatever.
You want to implement the vector math X_{i+1} = X_{i} + Vt. For the Xs and V vectors representing position and velocity respectively, and t representing time. I've parameterized the distance along the track by time because I'm a physicist, but it really is the natural thing to do. Normalize the velocity vector if you want to give track distance (i.e. scale V such that V.x*V.x + V.y*V.y + V.z*V.z = 1).
Using the struct above makes it natural to access the elements, but not so convenient to do the addition: arrays are better for that. Like this:
double X[3];
double V[3];
// initialize
for (int i=0; i<3 ++1){
X[i] = X[i] + V[i]*t;
}
With a union, you can get the advantages of both:
struct vector_s{
double x;
double y;
double z;
}
typedef
union vector_u {
struct vector_s s; // s for struct
double a[3]; // a for array
} vector;
If you want to associate both the position and the velocity of with the particle (a very reasonable thing to do) you construct a structure that support two vectors
typedef
struct particle_s {
vector position;
vector velocity;
//...
} particle_t;
and run an update routine that looks roughly like:
void update(particle *p, double dt){
for (int i=0; i<3 ++i){
p->position.a[i] += p->velocity.a[i]*dt;
}
}
Afaik, there are mainly two ways on how you can calculate the new position. One is like the other have explaint to use an explicit velocity. The other possibility is to store the last and the current position and to use the Verlet integration. Both ways have their advantages and disadvantages. You might also take a look on this interresting page.
If you are trying to move along a straight line between two points, you can use the interpolation formula:
P(t) = P1*(1-t) + P2*t
P(t) is the calculated position of the point, t is a scalar ranging from 0 to 1, P1 and P2 are the endpoints, and the addition in the above is vector addition (so you apply this formula separately to the x, y and z components of your points). When t=0, you get P1; when t=1, you get P2, and for intermediate values, you get a point part way along the line between P1 and P2. So t=.5 gives you the midpoint between P1 and P2, t=.333333 gives you the point 1/3 of the way from P1 to P2, etc. Values of t outside the range [0, 1] extrapolate to points along the line outside the segment from P1 to P2.
Using the interpolation formula can be better than computing a velocity and repeatedly adding it if the velocity is small compared to the distance between the points, because you limit the roundoff error.