27#include "generated/airframe.h"
54#ifndef GUIDANCE_H_RC_ID
55#define GUIDANCE_H_RC_ID ABI_BROADCAST
64static void send_gh(
struct transport_tx *trans,
struct link_device *
dev)
67 pprz_msg_send_GUIDANCE_H_INT(trans,
dev, AC_ID,
70 &(pos->
x), &(pos->
y));
73static void send_href(
struct transport_tx *trans,
struct link_device *
dev)
75 pprz_msg_send_GUIDANCE_H_REF_INT(trans,
dev, AC_ID,
106#if PERIODIC_TELEMETRY
151#ifndef GUIDANCE_H_SP_MAX_R
152#ifdef STABILIZATION_ATTITUDE_SP_MAX_R
153#define GUIDANCE_H_SP_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
155#define GUIDANCE_H_SP_MAX_R 60.f
159#ifndef GUIDANCE_H_DEADBAND_R
160#define GUIDANCE_H_DEADBAND_R 200
163#define YAW_DEADBAND_EXCEEDED(_rc) \
164 (rc->values[RADIO_YAW] > GUIDANCE_H_DEADBAND_R || \
165 rc->values[RADIO_YAW] < -GUIDANCE_H_DEADBAND_R)
178 rc_sp->
heading += heading_rate * dt;
226#if GUIDANCE_H_USE_SPEED_REF
276#if GUIDANCE_H_USE_REF
277 if (
guidance_h.sp.h_mask == GUIDANCE_H_SP_ALL) {
286 else if (
guidance_h.sp.h_mask == GUIDANCE_H_SP_ACCEL) {
292 else if (
guidance_h.sp.h_mask == GUIDANCE_H_SP_SPEED) {
312 switch (
nav.setpoint_mode) {
346#if GUIDANCE_H_USE_SPEED_REF
353 if (
guidance_h.sp.yaw_mask == GUIDANCE_H_SP_YAW_RATE) {
397 if (
nav.fp_max_speed > 0.f) {
398 guidance_h_SetMaxSpeed(nav.fp_max_speed);
401 struct StabilizationSetpoint sp;
402 STAB_SP_SET_EULERS_ZERO(sp);
405 if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) {
406 return stab_sp_from_quat_f(&nav.quat);
411 struct FloatEulers sp_cmd_f = {
419 return guidance_h_guided_run(in_flight);
423 guidance_h.sp.pos.x = POS_BFP_OF_REAL(nav.carrot.y);
424 guidance_h.sp.pos.y = POS_BFP_OF_REAL(nav.carrot.x);
426 switch (nav.setpoint_mode) {
427 case NAV_SETPOINT_MODE_POS:
428 guidance_h_set_pos(nav.carrot.y, nav.carrot.x);
429 guidance_h_update_reference();
430 guidance_h_set_heading(nav.heading);
431 guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h);
434 case NAV_SETPOINT_MODE_SPEED:
435 guidance_h_set_vel(nav.speed.y, nav.speed.x);
436 guidance_h_update_reference();
437 guidance_h_set_heading(nav.heading);
438 guidance_h_cmd = guidance_h_run_speed(in_flight, &guidance_h);
441 case NAV_SETPOINT_MODE_ACCEL:
442 guidance_h_set_acc(nav.accel.y, nav.accel.x);
443 guidance_h_update_reference();
444 guidance_h_set_heading(nav.heading);
445 guidance_h_cmd = guidance_h_run_accel(in_flight, &guidance_h);
479 if (
guidance_h.sp.h_mask != GUIDANCE_H_SP_POS) {
497 float newvx = cosf(-psi) * vx + sinf(-psi) * vy;
498 float newvy = -sinf(-psi) * vx + cosf(-psi) * vy;
504 if (
guidance_h.sp.h_mask != GUIDANCE_H_SP_SPEED) {
515 float newax = cosf(-psi) * ax + sinf(-psi) * ay;
516 float neway = -sinf(-psi) * ax + cosf(-psi) * ay;
522 if (
guidance_h.sp.h_mask != GUIDANCE_H_SP_ACCEL) {
532 guidance_h.sp.yaw_mask = GUIDANCE_H_SP_YAW_RATE;
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
bool autopilot_in_flight(void)
get in_flight flag
if(GpsFixValid() &&e_identification_started)
static struct uart_periph * dev
#define FLOAT_ANGLE_NORMALIZE(_a)
#define FLOAT_VECT2_ZERO(_v)
#define VECT2_COPY(_a, _b)
int32_t psi
in rad with INT32_ANGLE_FRAC
#define ACCEL_BFP_OF_REAL(_af)
#define SPEED_FLOAT_OF_BFP(_ai)
#define INT_VECT2_ZERO(_v)
#define INT32_VECT2_RSHIFT(_o, _i, _r)
#define POS_BFP_OF_REAL(_af)
#define ACCEL_FLOAT_OF_BFP(_ai)
#define SPEED_BFP_OF_REAL(_af)
vector in North East Down coordinates
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
void gh_set_ref(struct Int32Vect2 pos, struct FloatVect2 speed, struct FloatVect2 accel)
void gh_update_ref_from_accel_sp(struct FloatVect2 accel_sp)
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp)
struct GuidanceHRef gh_ref
#define GH_POS_REF_FRAC
fixedpoint representation: Q26.37 will give a range of 67e3 km and a resolution of 1....
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
struct FloatVect3 sp_accel
struct FloatVect3 speed_sp
Guidance in a module file.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
struct RadioControl radio_control
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
#define RADIO_ROLL
Redefining RADIO_* Do not use with radio.h (ppm rc).
Some helper functions to check RC sticks.
#define THROTTLE_STICK_DOWN_FROM_RC(_rc)
struct StabilizationSetpoint guidance_h_guided_run(bool in_flight)
Run GUIDED mode control.
void guidance_h_set_acc(float ax, float ay)
Set horizontal acceleration setpoint.
struct StabilizationSetpoint guidance_h_from_nav(bool in_flight)
Set horizontal guidance from NAV and run control loop.
struct StabilizationSetpoint guidance_h_cmd
horizontal guidance command.
void guidance_h_set_pos(float x, float y)
Set horizontal position setpoint.
void guidance_h_mode_changed(uint8_t new_mode)
static void send_href(struct transport_tx *trans, struct link_device *dev)
static void send_gh(struct transport_tx *trans, struct link_device *dev)
void guidance_h_hover_enter(void)
static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc)
void guidance_h_set_body_acc(float ax, float ay)
Set body relative horizontal acceleration setpoint.
void guidance_h_set_heading(float heading)
Set heading setpoint.
void guidance_h_init(void)
void guidance_h_set_all(float x, float y, float vx, float vy, float ax, float ay)
Set position speed and acceleration (NED frame).
struct HorizontalGuidance guidance_h
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight, struct RadioControl *rc)
read speed setpoint from RC
static void read_rc_setpoint_heading(struct HorizontalGuidanceRCInput *rc_sp, bool in_flight, struct RadioControl *rc)
void guidance_h_nav_enter(void)
static void guidance_h_update_reference(void)
struct StabilizationSetpoint guidance_h_run(bool in_flight)
void guidance_h_set_vel(float vx, float vy)
Set horizontal velocity setpoint.
void guidance_h_set_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint.
#define YAW_DEADBAND_EXCEEDED(_rc)
static void reset_guidance_reference_from_current_position(void)
#define GUIDANCE_H_SP_MAX_R
void guidance_h_set_heading_rate(float rate)
Set heading rate setpoint.
Horizontal guidance for rotorcrafts.
#define GUIDANCE_H_MODE_NAV
#define GUIDANCE_H_USE_REF
Use horizontal guidance reference trajectory.
void guidance_h_run_enter(void)
#define GUIDANCE_H_MODE_HOVER
#define GUIDANCE_H_MODE_NONE
struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
#define GUIDANCE_H_MODE_GUIDED
#define GUIDANCE_H_USE_SPEED_REF
Use horizontal guidance speed reference.
Vertical guidance for rotorcrafts.
struct RotorcraftNavigation nav
Rotorcraft navigation functions.
#define NAV_SETPOINT_MODE_SPEED
#define NAV_SETPOINT_MODE_ACCEL
#define NAV_HORIZONTAL_MODE_GUIDED
#define NAV_HORIZONTAL_MODE_ATTITUDE
#define NAV_HORIZONTAL_MODE_NONE
#define NAV_SETPOINT_MODE_POS
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which se...
struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
General stabilization interface for rotorcrafts.
#define STAB_SP_SET_EULERS_ZERO(_sp)
API to get/set the generic vehicle states.
union StabilizationSetpoint::@354356152067340015216250042204214023005305374154 sp
static float get_sys_time_float(void)
Get the time in seconds since startup.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.