47 lines
821 B
C++
47 lines
821 B
C++
#pragma once
|
|
|
|
#include "Eigen/Core"
|
|
#include "freertos/idf_additions.h"
|
|
#include <cstdint>
|
|
|
|
#ifdef PS
|
|
#undef PS
|
|
#endif
|
|
|
|
#ifdef F
|
|
#undef F
|
|
#endif
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
#define WAYPOINT_COUNT 8
|
|
|
|
struct waypoint {
|
|
Eigen::Vector3f coords; // lon, lat, alt
|
|
bool active; // active or to be skipped
|
|
};
|
|
|
|
struct drone_nav {
|
|
waypoint waypoints[WAYPOINT_COUNT];
|
|
uint8_t current_waypoint;
|
|
|
|
void set_active_mask(uint8_t mask) {
|
|
for (int i = 0; i < WAYPOINT_COUNT; i++) {
|
|
this->waypoints[i].active = (mask & (1 << i)) != 0;
|
|
}
|
|
}
|
|
|
|
uint8_t get_active_mask() {
|
|
uint8_t mask = 0;
|
|
for (int i = 0; i < WAYPOINT_COUNT; i++) {
|
|
if (waypoints[i].active) {
|
|
mask |= (1 << i);
|
|
}
|
|
}
|
|
return mask;
|
|
}
|
|
};
|
|
|
|
inline SemaphoreHandle_t nav_mutex = NULL;
|
|
inline drone_nav nav_man;
|