31#include "generated/airframe.h"
33#define ASSERT_EQUIVALENCE(primary, backup) \
34 _Static_assert(SERVO_##primary##_MIN == SERVO_##backup##_MIN, \
35 #primary " and " #backup " must have same MIN value"); \
36 _Static_assert(SERVO_##primary##_NEUTRAL == SERVO_##backup##_NEUTRAL, \
37 #primary " and " #backup " must have same NEUTRAL value"); \
38 _Static_assert(SERVO_##primary##_MAX == SERVO_##backup##_MAX, \
39 #primary " and " #backup " must have same MAX value")
46#ifdef SERVO_BROTATION_MECH_IDX
51#ifndef ROTWING_QUAD_MIN_RPM
52#define ROTWING_QUAD_MIN_RPM 400
56#ifndef ROTWING_PUSH_MIN_RPM
57#define ROTWING_PUSH_MIN_RPM 300
61#ifndef ROTWING_RPM_TIMEOUT
62#define ROTWING_RPM_TIMEOUT 1.0
66#ifndef ROTWING_QUAD_IDLE_MIN_THRUST
67#define ROTWING_QUAD_IDLE_MIN_THRUST 100
71#ifndef ROTWING_QUAD_IDLE_TIMEOUT
72#define ROTWING_QUAD_IDLE_TIMEOUT 0.4
76#ifndef ROTWING_FW_SKEW_ANGLE
77#define ROTWING_FW_SKEW_ANGLE 85.0
81#ifndef ROTWING_QUAD_SKEW_ANGLE
82#define ROTWING_QUAD_SKEW_ANGLE 25.0
86#ifndef ROTWING_TRANSITION_MAX_BANK
87#define ROTWING_TRANSITION_MAX_BANK RadOfDeg(20)
91#ifndef ROTWING_TRANSITION_MAX_CLIMB_SPEED
92#define ROTWING_TRANSITION_MAX_CLIMB_SPEED 0.5
96#ifndef ROTWING_TRANSITION_MAX_DESCEND_SPEED
97#define ROTWING_TRANSITION_MAX_DESCEND_SPEED -0.5
101#ifndef ROTWING_SKEW_BACK_MARGIN
102#define ROTWING_SKEW_BACK_MARGIN 5.0
106#ifndef ROTWING_SKEW_REF_MODEL_MAX_DIFF
107#define ROTWING_SKEW_REF_MODEL_MAX_DIFF 5.f
111#ifndef ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE
112#define ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE 30.0
116#ifndef ROTWING_FW_STALL_TIMEOUT
117#define ROTWING_FW_STALL_TIMEOUT 0.5
120#ifndef ROTWING_STATE_MIN_FW_DIST
121#define ROTWING_STATE_MIN_FW_DIST 200.0
125#ifndef ROTWING_SKEW_SWEEP_MIN
126#define ROTWING_SKEW_SWEEP_MIN 0.f
129#ifndef ROTWING_SKEW_SWEEP_MAX
130#define ROTWING_SKEW_SWEEP_MAX 60.f
133#ifndef ROTWING_SKEW_SWEEP_FREQ
134#define ROTWING_SKEW_SWEEP_FREQ 0.1f
138#ifndef SERVO_BMOTOR_PUSH_IDX
139#define SERVO_BMOTOR_PUSH_IDX SERVO_MOTOR_PUSH_IDX
143#ifndef SERVO_BROTATION_MECH_IDX
144#define SERVO_BROTATION_MECH_IDX SERVO_ROTATION_MECH_IDX
148#ifndef ROTWING_STATE_ACT_FEEDBACK_ID
149#define ROTWING_STATE_ACT_FEEDBACK_ID ABI_BROADCAST
155#ifndef GUIDANCE_INDI_FWD_CLIMB_SPEED
156#define GUIDANCE_INDI_FWD_CLIMB_SPEED 4.0
162#ifndef GUIDANCE_INDI_FWD_DESCEND_SPEED
163#define GUIDANCE_INDI_FWD_DESCEND_SPEED -4.0
169#ifndef GUIDANCE_INDI_QUAD_CLIMB_SPEED
170#define GUIDANCE_INDI_QUAD_CLIMB_SPEED 2.0
176#ifndef GUIDANCE_INDI_QUAD_DESCEND_SPEED
177#define GUIDANCE_INDI_QUAD_DESCEND_SPEED -2.0
184#if PERIODIC_TELEMETRY
201 pprz_msg_send_ROTATING_WING_STATE(trans,
dev, AC_ID,
244 for (
int i = 0; i < 5; i++) {
255#if PERIODIC_TELEMETRY
270 static float last_idle_time = 0;
306 static float last_stall_time = 0;
310 Bound(meas_skew_angle, 0, 90);
313 last_stall_time = current_time;
356 float skew_max_airspeed = ROTWING_QUAD_MAX_AIRSPEED + (ROTWING_FW_MAX_AIRSPEED - ROTWING_QUAD_MAX_AIRSPEED) * meas_skew_angle /
ROTWING_FW_SKEW_ANGLE;
358 Bound(skew_max_airspeed, ROTWING_QUAD_MAX_AIRSPEED, ROTWING_FW_MAX_AIRSPEED);
362 skew_max_airspeed = ROTWING_FW_MAX_AIRSPEED;
385 if (meas_airspeed < ROTWING_SKEW_DOWN_AIRSPEED) {
387 }
else if (meas_airspeed < ROTWING_SKEW_UP_AIRSPEED) {
389 }
else if (meas_airspeed < ROTWING_QUAD_MAX_AIRSPEED) {
410 rotwing_state.max_airspeed = fmax(ROTWING_QUAD_MAX_AIRSPEED, skew_min_airspeed);
442 nav_max_deceleration_sp = ROTWING_FW_MAX_DECELERATION * meas_skew_angle / 90.f + ROTWING_QUAD_MAX_DECELERATION * (90.f - meas_skew_angle) / 90.f;
450 static float rotwing_state_skew_p_cmd = -
MAX_PPRZ;
451 static float rotwing_state_skew_d_cmd = 0;
453 float speed_sp = ROTWING_SKEW_REF_MODEL_P_GAIN * (servo_pprz_cmd - rotwing_state_skew_p_cmd);
454 BoundAbs(
speed_sp, ROTWING_SKEW_REF_MODEL_MAX_SPEED);
455 rotwing_state_skew_d_cmd += ROTWING_SKEW_REF_MODEL_D_GAIN * (
speed_sp - rotwing_state_skew_d_cmd);
456 rotwing_state_skew_p_cmd += rotwing_state_skew_d_cmd;
457 BoundAbs(rotwing_state_skew_p_cmd,
MAX_PPRZ);
460#if (ROTWING_SKEW_REF_MODEL || USE_NPS)
465#ifdef COMMAND_ROT_MECH
472#ifdef COMMAND_ROT_MECH
479 feedback.
idx = SERVO_ROTATION_MECH_IDX;
494 for(
uint8_t i = 0; i < 5; i++) {
518 for (
int i = 0; i < num_act_message; i++) {
519 int idx = feedback_msg[i].idx;
524 float skew_angle_rad = 0.5 * M_PI - feedback_msg[i].position;
525 rotwing_state.meas_skew_angle_deg = DegOfRad(skew_angle_rad);
530 if (feedback_msg[i].
set.
rpm) {
531 if ((
idx == SERVO_MOTOR_FRONT_IDX) || (
idx == SERVO_BMOTOR_FRONT_IDX)) {
534 }
else if ((
idx == SERVO_MOTOR_RIGHT_IDX) || (
idx == SERVO_BMOTOR_RIGHT_IDX)) {
537 }
else if ((
idx == SERVO_MOTOR_BACK_IDX) || (
idx == SERVO_BMOTOR_BACK_IDX)) {
540 }
else if ((
idx == SERVO_MOTOR_LEFT_IDX) || (
idx == SERVO_BMOTOR_LEFT_IDX)) {
582 return (!skew_timeout && skew_angle_match);
606 static struct FloatVect3 x_axis = {.x = 1, .y = 0, .z = 0};
607 static struct FloatVect3 z_axis = {.x = 0, .y = 0, .z = 1};
617 float body_to_wp_angle_rad = atan2f(
y,
x);
619 if (body_to_wp_angle_rad >= 0.f) {
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
#define ACT_FEEDBACK_BOARD_ID
struct pprz_autopilot autopilot
Global autopilot structure.
Hardware independent code for commands handling.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
static struct uart_periph * dev
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_DOT_PRODUCT(_v1, _v2)
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
static void stateSetFixedWingFlight(bool fixed_wing)
Set if flying as a fixed wing (e.g. a hybrid in forward flight).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
struct FloatVect3 speed_sp
#define GUIDANCE_INDI_QUAD_DESCEND_SPEED
Descend speed when navigation is doing direct lines.
void guidance_set_max_bank_angle(float max_bank)
#define GUIDANCE_INDI_FWD_DESCEND_SPEED
Descend speed when navigation is making turns instead of direct lines.
void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd)
#define GUIDANCE_INDI_QUAD_CLIMB_SPEED
Climb speed when navigation is doing direct lines.
#define GUIDANCE_INDI_FWD_CLIMB_SPEED
Climb speed when navigation is making turns instead of direct lines.
void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed)
void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd)
float gi_unbounded_airspeed_sp
Hardware independent API for actuators (servos, motor controllers).
bool position
Position is set.
struct act_feedback_t::act_feedback_set_t set
Bitset registering what is set as feedback.
uint8_t idx
General index of the actuators (generated in airframe.h).
float position
In radians.
float nav_max_deceleration_sp
vector in East North Up coordinates Units: meters
void preflight_error(struct preflight_result_t *result, const char *fmt,...)
Register a preflight error used inside the preflight checking functions.
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
Rotorcraft specific autopilot interface and initialization.
struct HorizontalGuidance guidance_h
#define GUIDANCE_H_MODE_NONE
#define GUIDANCE_H_MAX_BANK
Max bank controlled by guidance.
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
bool rotwing_state_pusher_motor_running(void)
static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act)
#define ROTWING_RPM_TIMEOUT
#define ROTWING_SKEW_REF_MODEL_MAX_DIFF
bool rotwing_state_hover_motors_running(void)
#define ROTWING_FW_STALL_TIMEOUT
#define ROTWING_SKEW_SWEEP_MAX
#define ROTWING_FW_SKEW_ANGLE
#define ROTWING_TRANSITION_MAX_BANK
#define ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE
static void rotwing_state_skew_sweep(bool run, float time)
Sweep the skew angle setpoint between skew_sweep.min_deg and skew_sweep.max_deg at skew_sweep....
#define ROTWING_STATE_MIN_FW_DIST
void rotwing_state_init(void)
#define ROTWING_PUSH_MIN_RPM
#define ROTWING_TRANSITION_MAX_DESCEND_SPEED
#define ROTWING_TRANSITION_MAX_CLIMB_SPEED
bool rotwing_state_hover_motors_idling(void)
Check if hover motors are idling (COMMAND_THRUST < ROTWING_QUAD_IDLE_MIN_THRUST) for ROTWING_QUAD_IDL...
#define ROTWING_SKEW_BACK_MARGIN
#define SERVO_BMOTOR_PUSH_IDX
#define ROTWING_SKEW_SWEEP_FREQ
bool rotwing_state_skew_angle_valid(void)
#define ROTWING_SKEW_SWEEP_MIN
#define ROTWING_QUAD_IDLE_MIN_THRUST
#define SERVO_BROTATION_MECH_IDX
#define ROTWING_QUAD_SKEW_ANGLE
#define ASSERT_EQUIVALENCE(primary, backup)
#define ROTWING_QUAD_MIN_RPM
static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
bool rotwing_state_choose_state_by_dist(uint8_t wp_id)
#define ROTWING_STATE_ACT_FEEDBACK_ID
ABI binding feedback data.
abi_event rotwing_state_feedback_ev
#define ROTWING_QUAD_IDLE_TIMEOUT
bool rotwing_state_choose_circle_direction(uint8_t wp_id)
struct rotwing_state_t rotwing_state
void rotwing_state_periodic(void)
void rotwing_state_set(enum rotwing_states_t state)
#define ROTWING_SKEW_ANGLE_STEP
@ ROTWING_STATE_REQUEST_HOVER
@ ROTWING_STATE_FORCE_HOVER
@ ROTWING_STATE_REQUEST_FW
static const ShellCommand commands[]
struct Stabilization stabilization
#define STABILIZATION_MODE_ATTITUDE
#define STABILIZATION_ATT_SUBMODE_FORWARD
#define STABILIZATION_MODE_NONE
Stabilization modes.
int16_t actuators_pprz[INDI_NUM_ACT+1]
PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if...
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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.