41 lines
1.7 KiB
C
41 lines
1.7 KiB
C
|
#ifndef ROUTE_H
|
||
|
|
#define ROUTE_H
|
||
|
|
#include <stdint.h>
|
||
|
|
|
||
|
|
// Simulated drive around a closed loop of waypoints, so the beacon looks like a
|
||
|
|
// car going round the block instead of a parked one.
|
||
|
|
//
|
||
|
|
// The car follows straight lines between the points and goes from the last one
|
||
|
|
// back to the first, forever. For street-following, the points are the street
|
||
|
|
// geometry from tools/make_route.py (main/route_points.h). Speed is a function
|
||
|
|
// of where the car is on a segment: it cruises, brakes ahead of each bend down to
|
||
|
|
// the speed that bend allows, and accelerates away after it. Heading is the
|
||
|
|
// bearing of the current segment.
|
||
|
|
//
|
||
|
|
// Uses only standard headers, so it also compiles on the host for testing.
|
||
|
|
|
||
|
|
typedef struct {
|
||
|
|
int32_t latitude_tenmicrodeg; // 1/10 microdegree, same units as the CAM
|
||
|
|
int32_t longitude_tenmicrodeg;
|
||
|
|
} route_point_t;
|
||
|
|
|
||
|
|
typedef struct {
|
||
|
|
int32_t latitude_tenmicrodeg;
|
||
|
|
int32_t longitude_tenmicrodeg;
|
||
|
|
uint16_t speed_cm_s; // SpeedValue units (0.01 m/s)
|
||
|
|
uint16_t heading_ddeg; // HeadingValue units (0.1 deg, 0..3599, 0 = north, clockwise)
|
||
|
|
int segment; // index of the route point the car last passed
|
||
|
|
} route_state_t;
|
||
|
|
|
||
|
|
#define ROUTE_MAX_POINTS 512 // keep in step with MAX_POINTS in tools/make_route.py
|
||
|
|
|
||
|
|
// `points` must stay valid for as long as the route is used. Returns 0, or -1
|
||
|
|
// if n is out of range (2..ROUTE_MAX_POINTS) or a segment has zero length.
|
||
|
|
// min_corner_mps is the slowest the car ever goes (hairpins, U-turns).
|
||
|
|
int route_init(const route_point_t *points, int n, double cruise_mps, double min_corner_mps);
|
||
|
|
|
||
|
|
// Moves the car on by dt_s seconds and writes its new position into `out`.
|
||
|
|
void route_step(double dt_s, route_state_t *out);
|
||
|
|
|
||
|
|
#endif
|