38 #include "generated/modules.h"
39 #include "generated/flight_plan.h"
44 #include "pprzlink/messages.h"
59 nav.exception_flag[flag_num] =
true;
63 #if PERIODIC_TELEMETRY
71 pprz_msg_send_WP_MOVED_ENU(trans,
dev, AC_ID,
97 #if PERIODIC_TELEMETRY
113 if (DL_BLOCK_ac_id(buf) != AC_ID) {
return; }
119 uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
120 if (ac_id != AC_ID) {
return; }
122 uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
124 lla.
lat = DL_MOVE_WP_lat(buf);
125 lla.
lon = DL_MOVE_WP_lon(buf);
250 #ifdef InGeofenceSector
301 #ifdef AP_MODE_FAILSAFE
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
bool autopilot_in_flight(void)
get in_flight flag
Core autopilot interface common to all firmwares.
void common_flight_plan_init(void)
void nav_goto_block(uint8_t b)
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
const uint8_t nb_waypoint
Common code for AP and FBW telemetry.
Device independent GPS code (interface)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_COPY(_a, _b)
#define VECT3_COPY(_a, _b)
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
vector in Latitude, Longitude and Altitude
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
void WEAK ins_reset_local_origin(void)
INS local origin reset.
void WEAK ins_reset_altitude_ref(void)
INS altitude reference reset.
Integrated Navigation System interface.
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
void waypoints_init(void)
initialize global and local waypoints
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
void nav_oval(uint8_t p1, uint8_t p2, float radius)
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
static void nav_goto(struct EnuCoor_f *wp)
static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
static void nav_circle(struct EnuCoor_f *wp_center, float radius)
static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
struct FloatVect2 pos_diff
Paparazzi fixed point algebra.
vector in East North Up coordinates Units: meters
void nav_home(void)
Home mode navigation.
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
void nav_set_heading_current(void)
Set heading to the current yaw angle.
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
const float max_dist_from_home
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
bool nav_is_in_flight(void)
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
struct RotorcraftNavigation nav
void nav_init(void)
Navigation Initialisation.
void nav_parse_MOVE_WP(uint8_t *buf)
void nav_register_circle(navigation_circle nav_circle)
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
Registering functions.
void nav_parse_BLOCK(uint8_t *buf)
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
const float max_dist2_from_home
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
void nav_set_failsafe(void)
struct EnuCoor_f speed
speed setpoint (in m/s)
float dist2_to_home
squared distance to home waypoint
navigation_approaching nav_approaching
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
navigation_circle nav_circle
float radius
radius setpoint (in meters)
navigation_oval_init nav_oval_init
float heading
heading setpoint (in radians)
struct EnuCoor_f carrot
carrot position (also used for GCS display)
navigation_route nav_route
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
struct EnuCoor_f target
final target position (in meters)
bool too_far_from_home
too_far flag
float failsafe_mode_dist2
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
void set_exception_flag(uint8_t flag_num)
Rover navigation functions.
void(* nav_rover_oval_init)(void)
void(* nav_rover_goto)(struct EnuCoor_f *wp)
void(* nav_rover_circle)(struct EnuCoor_f *wp_center, float radius)
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
#define NAV_MODE_WAYPOINT
Nav modes.
void(* nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
void(* nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
bool(* nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
General Navigation structure.
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.