rotwing_state.c File Reference

#include "modules/rotwing_drone/rotwing_state.h"
#include "firmwares/rotorcraft/autopilot_firmware.h"
#include "modules/core/commands.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for rotwing_state.c:

Go to the source code of this file.

Macros
#define	ROTWING_QUAD_MIN_RPM   400
#define	ROTWING_PUSH_MIN_RPM   300
#define	ROTWING_RPM_TIMEOUT   1.0
#define	ROTWING_QUAD_IDLE_MIN_THRUST   100
#define	ROTWING_QUAD_IDLE_TIMEOUT   0.4
#define	ROTWING_FW_SKEW_ANGLE   85.0
#define	ROTWING_QUAD_SKEW_ANGLE   25.0
#define	ROTWING_TRANSITION_MAX_BANK   RadOfDeg(20)
#define	ROTWING_TRANSITION_MAX_CLIMB_SPEED   0.5
#define	ROTWING_TRANSITION_MAX_DESCEND_SPEED   -0.5
#define	ROTWING_SKEW_BACK_MARGIN   5.0
#define	ROTWING_SKEW_REF_MODEL_MAX_DIFF   5.f
#define	ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE   30.0
#define	ROTWING_FW_STALL_TIMEOUT   0.5
#define	ROTWING_STATE_MIN_FW_DIST   200.0
#define	SERVO_BMOTOR_PUSH_IDX   SERVO_MOTOR_PUSH_IDX
#define	SERVO_BROTATION_MECH_IDX   SERVO_ROTATION_MECH_IDX
#define	ROTWING_STATE_ACT_FEEDBACK_ID   ABI_BROADCAST
	ABI binding feedback data.

Functions
static void	rotwing_state_feedback_cb (uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act)
static void	send_rotating_wing_state (struct transport_tx *trans, struct link_device *dev)
void	rotwing_state_init (void)
bool	rotwing_state_hover_motors_idling (void)
	Check if hover motors are idling (COMMAND_THRUST < ROTWING_QUAD_IDLE_MIN_THRUST) for ROTWING_QUAD_IDLE_TIMEOUT time.
void	rotwing_state_periodic (void)
static void	rotwing_state_feedback_cb (uint8_t sender_id, struct act_feedback_t UNUSED *feedback_msg, uint8_t UNUSED num_act_message)
bool	rotwing_state_hover_motors_running (void)
bool	rotwing_state_pusher_motor_running (void)
bool	rotwing_state_skew_angle_valid (void)
void	rotwing_state_set (enum rotwing_states_t state)
bool	rotwing_state_choose_circle_direction (uint8_t wp_id)
bool	rotwing_state_choose_state_by_dist (uint8_t wp_id)

Variables
abi_event	rotwing_state_feedback_ev
struct rotwing_state_t	rotwing_state

Macro Definition Documentation

ROTWING_FW_SKEW_ANGLE

#define ROTWING_FW_SKEW_ANGLE   85.0

Definition at line 60 of file rotwing_state.c.

ROTWING_FW_STALL_TIMEOUT

#define ROTWING_FW_STALL_TIMEOUT   0.5

Definition at line 100 of file rotwing_state.c.

ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE

#define ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE   30.0

Definition at line 95 of file rotwing_state.c.

ROTWING_PUSH_MIN_RPM

#define ROTWING_PUSH_MIN_RPM   300

Definition at line 40 of file rotwing_state.c.

ROTWING_QUAD_IDLE_MIN_THRUST

#define ROTWING_QUAD_IDLE_MIN_THRUST   100

Definition at line 50 of file rotwing_state.c.

ROTWING_QUAD_IDLE_TIMEOUT

#define ROTWING_QUAD_IDLE_TIMEOUT   0.4

Definition at line 55 of file rotwing_state.c.

ROTWING_QUAD_MIN_RPM

#define ROTWING_QUAD_MIN_RPM   400

Definition at line 35 of file rotwing_state.c.

ROTWING_QUAD_SKEW_ANGLE

#define ROTWING_QUAD_SKEW_ANGLE   25.0

Definition at line 65 of file rotwing_state.c.

ROTWING_RPM_TIMEOUT

#define ROTWING_RPM_TIMEOUT   1.0

Definition at line 45 of file rotwing_state.c.

ROTWING_SKEW_BACK_MARGIN

#define ROTWING_SKEW_BACK_MARGIN   5.0

Definition at line 85 of file rotwing_state.c.

ROTWING_SKEW_REF_MODEL_MAX_DIFF

#define ROTWING_SKEW_REF_MODEL_MAX_DIFF   5.f

Definition at line 90 of file rotwing_state.c.

ROTWING_STATE_ACT_FEEDBACK_ID

#define ROTWING_STATE_ACT_FEEDBACK_ID   ABI_BROADCAST

ABI binding feedback data.

Definition at line 119 of file rotwing_state.c.

ROTWING_STATE_MIN_FW_DIST

#define ROTWING_STATE_MIN_FW_DIST   200.0

Definition at line 104 of file rotwing_state.c.

ROTWING_TRANSITION_MAX_BANK

#define ROTWING_TRANSITION_MAX_BANK   RadOfDeg(20)

Definition at line 70 of file rotwing_state.c.

ROTWING_TRANSITION_MAX_CLIMB_SPEED

#define ROTWING_TRANSITION_MAX_CLIMB_SPEED   0.5

Definition at line 75 of file rotwing_state.c.

ROTWING_TRANSITION_MAX_DESCEND_SPEED

#define ROTWING_TRANSITION_MAX_DESCEND_SPEED   -0.5

Definition at line 80 of file rotwing_state.c.

SERVO_BMOTOR_PUSH_IDX

#define SERVO_BMOTOR_PUSH_IDX   SERVO_MOTOR_PUSH_IDX

Definition at line 109 of file rotwing_state.c.

SERVO_BROTATION_MECH_IDX

#define SERVO_BROTATION_MECH_IDX   SERVO_ROTATION_MECH_IDX

Definition at line 114 of file rotwing_state.c.

Function Documentation

rotwing_state_choose_circle_direction()

bool rotwing_state_choose_circle_direction

(

uint8_t

wp_id

)

Definition at line 482 of file rotwing_state.c.

References float_rmat_transp_vmult(), foo, stateGetNedToBodyRMat_f(), stateGetPositionNed_f(), VECT3_CROSS_PRODUCT, VECT3_DIFF, VECT3_DOT_PRODUCT, waypoints, FloatVect3::x, NedCoor_f::x, point::x, FloatVect3::y, NedCoor_f::y, point::y, and NedCoor_f::z.

Here is the call graph for this function:

rotwing_state_choose_state_by_dist()

bool rotwing_state_choose_state_by_dist

(

uint8_t

wp_id

)

Definition at line 517 of file rotwing_state.c.

References AP_MODE_NAV, autopilot, dist2_to_wp, foo, get_dist2_to_point(), pprz_autopilot::mode, ROTWING_STATE_MIN_FW_DIST, ROTWING_STATE_REQUEST_FW, ROTWING_STATE_REQUEST_HOVER, rotwing_state_set(), and waypoints.

Here is the call graph for this function:

rotwing_state_feedback_cb() [1/2]

static void rotwing_state_feedback_cb ( uint8_t  sender_id, struct act_feedback_t *  feedback_msg, uint8_t  num_act  )

static

Referenced by rotwing_state_init().

Here is the caller graph for this function:

rotwing_state_feedback_cb() [2/2]

static void rotwing_state_feedback_cb ( uint8_t  sender_id, struct act_feedback_t UNUSED *  feedback_msg, uint8_t UNUSED  num_act_message  )

static

Definition at line 405 of file rotwing_state.c.

References foo, get_sys_time_float(), idx, rotwing_state_t::meas_rpm, rotwing_state_t::meas_rpm_time, rotwing_state_t::meas_skew_angle_deg, rotwing_state_t::meas_skew_angle_time, act_feedback_t::act_feedback_set_t::position, rotwing_state, act_feedback_t::act_feedback_set_t::rpm, SERVO_BMOTOR_PUSH_IDX, SERVO_BROTATION_MECH_IDX, and act_feedback_t::set.

Here is the call graph for this function:

rotwing_state_hover_motors_idling()

bool rotwing_state_hover_motors_idling

(

void

)

Check if hover motors are idling (COMMAND_THRUST < ROTWING_QUAD_IDLE_MIN_THRUST) for ROTWING_QUAD_IDLE_TIMEOUT time.

Returns

true if hover motors are idling, false otherwise

Definition at line 186 of file rotwing_state.c.

References Stabilization::cmd, foo, get_sys_time_float(), ROTWING_QUAD_IDLE_MIN_THRUST, ROTWING_QUAD_IDLE_TIMEOUT, and stabilization.

Referenced by guidance_indi_hybrid_set_wls_settings(), rotwing_state_periodic(), and send_rotating_wing_state().

Here is the call graph for this function:

Here is the caller graph for this function:

rotwing_state_hover_motors_running()

bool rotwing_state_hover_motors_running

(

void

)

Definition at line 442 of file rotwing_state.c.

References foo, get_sys_time_float(), rotwing_state_t::meas_rpm, rotwing_state_t::meas_rpm_time, ROTWING_QUAD_MIN_RPM, ROTWING_RPM_TIMEOUT, and rotwing_state.

Referenced by compute_accel_from_speed_sp(), G1G2_oneloop(), guidance_indi_hybrid_set_wls_settings(), rotwing_state_periodic(), and send_rotating_wing_state().

Here is the call graph for this function:

Here is the caller graph for this function:

rotwing_state_init()

void rotwing_state_init

(

void

)

Definition at line 153 of file rotwing_state.c.

References rotwing_state_t::cruise_airspeed, DefaultPeriodic, rotwing_state_t::fail_hover_motor, rotwing_state_t::fail_pusher_motor, rotwing_state_t::fail_skew_angle, foo, rotwing_state_t::force_skew, rotwing_state_t::fw_min_airspeed, rotwing_state_t::hover_motors_enabled, rotwing_state_t::meas_rpm, rotwing_state_t::meas_rpm_time, rotwing_state_t::meas_skew_angle_deg, rotwing_state_t::meas_skew_angle_time, rotwing_state_t::nav_state, rotwing_state_t::ref_model_skew_angle_deg, register_periodic_telemetry(), rotwing_state, ROTWING_STATE_ACT_FEEDBACK_ID, rotwing_state_feedback_cb(), rotwing_state_feedback_ev, ROTWING_STATE_FORCE_HOVER, send_rotating_wing_state(), rotwing_state_t::skew_cmd, rotwing_state_t::sp_skew_angle_deg, and rotwing_state_t::state.

Here is the call graph for this function:

rotwing_state_periodic()

void rotwing_state_periodic

(

void

)

Definition at line 200 of file rotwing_state.c.

References ACT_FEEDBACK_BOARD_ID, AP_MODE_FAILSAFE, Stabilization::att_submode, autopilot, commands, rotwing_state_t::cruise_airspeed, rotwing_state_t::fail_hover_motor, rotwing_state_t::fail_pusher_motor, rotwing_state_t::fail_skew_angle, foo, rotwing_state_t::force_skew, rotwing_state_t::fw_min_airspeed, get_sys_time_float(), gi_unbounded_airspeed_sp, guidance_h, GUIDANCE_H_MAX_BANK, GUIDANCE_H_MODE_NONE, GUIDANCE_INDI_FWD_CLIMB_SPEED, GUIDANCE_INDI_FWD_DESCEND_SPEED, GUIDANCE_INDI_QUAD_CLIMB_SPEED, GUIDANCE_INDI_QUAD_DESCEND_SPEED, guidance_set_max_bank_angle(), guidance_set_max_climb_speed(), guidance_set_max_descend_speed(), guidance_set_min_max_airspeed(), rotwing_state_t::hover_motors_enabled, act_feedback_t::idx, rotwing_state_t::max_airspeed, MAX_PPRZ, rotwing_state_t::meas_rpm, rotwing_state_t::meas_rpm_time, rotwing_state_t::meas_skew_angle_deg, rotwing_state_t::min_airspeed, pprz_autopilot::mode, HorizontalGuidance::mode, Stabilization::mode, nav_max_deceleration_sp, rotwing_state_t::nav_state, act_feedback_t::act_feedback_set_t::position, act_feedback_t::position, rotwing_state_t::ref_model_skew_angle_deg, ROTWING_FW_SKEW_ANGLE, ROTWING_FW_STALL_TIMEOUT, ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE, ROTWING_PUSH_MIN_RPM, ROTWING_QUAD_MIN_RPM, ROTWING_QUAD_SKEW_ANGLE, ROTWING_SKEW_ANGLE_STEP, ROTWING_SKEW_BACK_MARGIN, rotwing_state, ROTWING_STATE_FORCE_FW, ROTWING_STATE_FORCE_HOVER, ROTWING_STATE_FREE, rotwing_state_hover_motors_idling(), rotwing_state_hover_motors_running(), rotwing_state_pusher_motor_running(), ROTWING_STATE_REQUEST_FW, ROTWING_STATE_REQUEST_HOVER, ROTWING_TRANSITION_MAX_BANK, ROTWING_TRANSITION_MAX_CLIMB_SPEED, ROTWING_TRANSITION_MAX_DESCEND_SPEED, act_feedback_t::set, rotwing_state_t::skew_cmd, rotwing_state_t::sp_skew_angle_deg, speed_sp, stabilization, STABILIZATION_ATT_SUBMODE_FORWARD, STABILIZATION_MODE_ATTITUDE, STABILIZATION_MODE_NONE, rotwing_state_t::state, and stateGetAirspeed_f().

Here is the call graph for this function:

rotwing_state_pusher_motor_running()

bool rotwing_state_pusher_motor_running

(

void

)

Definition at line 459 of file rotwing_state.c.

References get_sys_time_float(), rotwing_state_t::meas_rpm, rotwing_state_t::meas_rpm_time, ROTWING_PUSH_MIN_RPM, ROTWING_RPM_TIMEOUT, and rotwing_state.

Referenced by compute_accel_from_speed_sp(), guidance_indi_hybrid_set_wls_settings(), rotwing_state_periodic(), and send_rotating_wing_state().

Here is the call graph for this function:

Here is the caller graph for this function:

rotwing_state_set()

void rotwing_state_set

(

enum rotwing_states_t

state

)

Definition at line 476 of file rotwing_state.c.

References rotwing_state_t::nav_state, rotwing_state, and state.

Referenced by rotwing_state_choose_state_by_dist().

Here is the caller graph for this function:

rotwing_state_skew_angle_valid()

bool rotwing_state_skew_angle_valid

(

void

)

Definition at line 468 of file rotwing_state.c.

References foo, get_sys_time_float(), rotwing_state_t::meas_skew_angle_deg, rotwing_state_t::meas_skew_angle_time, rotwing_state_t::ref_model_skew_angle_deg, ROTWING_RPM_TIMEOUT, ROTWING_SKEW_REF_MODEL_MAX_DIFF, and rotwing_state.

Referenced by send_rotating_wing_state().

Here is the call graph for this function:

Here is the caller graph for this function:

send_rotating_wing_state()

static void send_rotating_wing_state ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 126 of file rotwing_state.c.

References dev, foo, rotwing_state_t::force_skew, gi_unbounded_airspeed_sp, rotwing_state_t::hover_motors_enabled, rotwing_state_t::max_airspeed, rotwing_state_t::meas_skew_angle_deg, rotwing_state_t::min_airspeed, rotwing_state_t::nav_state, rotwing_state, rotwing_state_hover_motors_idling(), rotwing_state_hover_motors_running(), rotwing_state_pusher_motor_running(), rotwing_state_skew_angle_valid(), rotwing_state_t::sp_skew_angle_deg, rotwing_state_t::state, state, status, and rotwing_bitmask_t::value.

Referenced by rotwing_state_init().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

rotwing_state

struct rotwing_state_t rotwing_state

Definition at line 123 of file rotwing_state.c.

Referenced by G1G2_oneloop(), guidance_indi_hybrid_set_wls_settings(), oneloop_andi_run(), rotwing_state_feedback_cb(), rotwing_state_hover_motors_running(), rotwing_state_init(), rotwing_state_periodic(), rotwing_state_pusher_motor_running(), rotwing_state_set(), rotwing_state_skew_angle_valid(), and send_rotating_wing_state().

rotwing_state_feedback_ev

abi_event rotwing_state_feedback_ev

Definition at line 121 of file rotwing_state.c.

Referenced by rotwing_state_init().