Flight core (see fc_core.h). More...

#include "fc_core.h"
#include "app_link.h"
#include "params.h"
#include "proto_v2.h"
#include "spsc.h"
#include "alt_ctrl.h"
#include <string.h>
#include <math.h>

Include dependency graph for fc_core.c:

Macros
#define	DEG2RAD 0.017453292519943295f

#define	OK() result(c, PV2_ACK_OK, PV2_R_NONE)

#define	REJ(why) result(c, PV2_ACK_REJECTED, (why))

#define	INVALID(w) result(c, PV2_ACK_INVALID, (w))

Functions
	SPSC_DEFINE (s_cmdq, fc_cmd_t, 8u)

	SPSC_DEFINE (s_resq, fc_res_t, 8u)

	SPSC_DEFINE (s_evq, fc_event_t, 16u)

	SPSC_DEFINE (s_batq, fc_batt_sig_t, 8u)

void	fc_init (const fc_config_t *cfg)

int	fc_batt_signal (const fc_batt_sig_t *b)
	0 = queued, -1 = full

void	fc_batt_heartbeat (uint32_t now)
	every BattTask step

void	fc_set_sensors (uint16_t sensors)
	sensors detected at run time (a late expansion board): atomic store

void	fc_setpoint (fc_setpoint_t *out)
	last setpoints, same context

uint8_t	fc_is_core_op (uint8_t op)
	ops executed by the core

int	fc_post (const fc_cmd_t *c)
	0 = queued, -1 = full

int	fc_take_result (fc_res_t *r)
	1 if a result was taken

int	fc_take_event (fc_event_t *e)

void	fc_set_ground_info (uint8_t ready, uint8_t batt_fail, uint8_t calibrating)
	ground information computed by the AppTask (atomic byte stores)

void	fc_snapshot (fc_state_t *out)

static void	event (uint8_t level, uint16_t code, int32_t arg)

static uint8_t	has (uint16_t bits)

static uint8_t	gnss (void)

static uint8_t	home_set (void)

static uint8_t	alt_sensor (void)

static uint8_t	alt_ok (void)

static uint8_t	indoor (void)

static float	clampf (float v, float lo, float hi)

static float	ceiling_h_kind (int32_t *kind)

static float	ceiling_h (void)

static void	lift_off (void)

static void	touch_down (uint32_t now)

static uint8_t	hold_mode (void)

static void	test_stop (void)

static void	disarm (void)

static uint8_t	batt_unusable (void)

static uint8_t	batt_owns_auto (void)

static uint8_t	fs_action (int p_idx)

static void	result (const fc_cmd_t *c, uint8_t res, uint8_t why)

static void	exec (const fc_cmd_t *c, uint32_t now, uint8_t est_running)

static void	update_sticks (uint32_t now)

static int	stick_moved (void)

static void	app_link_failsafe (uint32_t now)

static void	battery (uint32_t now)

static void	air_ground (uint32_t now, uint32_t dt)

static void	flow_failsafe (uint32_t dt)

static void	setpoints (uint32_t now, uint32_t dt_ms)

static void	publish (void)

void	fc_service (uint32_t now, uint8_t est_running, const fc_vert_t *v)
	v: NULL = no vertical estimate (in air only via g_fc_dbg_in_air)

Variables
volatile uint8_t	g_fc_dbg_in_air

static fc_config_t	s_cfg

static volatile uint16_t	s_sensors

static volatile uint8_t	s_checks_ready

static volatile uint8_t	s_batt_fail

static volatile uint8_t	s_calibrating

static uint8_t	s_armed

static uint8_t	s_mode = PV2_MODE_ANGLE

static uint8_t	s_auto

static uint8_t	s_failsafe

static uint8_t	s_in_air

static uint8_t	s_fs_long

static uint8_t	s_fs_short_hold

static uint8_t	s_took_off

static uint32_t	s_arm_ms

static uint32_t	s_landed_ms

static uint32_t	s_flight_ms

static uint32_t	s_last_ms

static uint8_t	s_have_last

static appl_rc_t	s_rc

static uint32_t	s_rc_ms

static uint8_t	s_have_rc

static float	s_last_thr = -1.0f

static float	s_stick [4]

static uint8_t	s_rc_stale = 1

static uint32_t	s_silence

static fc_vert_t	s_v

static uint8_t	s_have_v

static float	s_h_ref

static float	s_h_ground

static float	s_h_sp

static float	s_to_target

static uint32_t	s_to_t0

static uint8_t	s_to_lifted

static uint8_t	s_dbg_forced

static uint8_t	s_landed

static uint8_t	s_geofence

static uint8_t	s_geofence_ev

static uint32_t	s_touch_ms

static uint32_t	s_flow_bad_ms

static alt_ctrl_t	s_ac

static fc_setpoint_t	s_sp

static uint8_t	s_batt_level

static uint8_t	s_batt_present

static uint8_t	s_batt_lock

static uint8_t	s_batt_stale

static uint8_t	s_batt_have_hb

static volatile uint32_t	s_batt_hb_ms

static volatile uint8_t	s_batt_hb_set

static uint8_t	s_test_motor

static uint8_t	s_test_pct

static uint32_t	s_test_until

static volatile uint32_t	s_snap_seq

static fc_state_t	s_snap

Detailed Description

Flight core (see fc_core.h).

Behavioural reference: the app simulator src/sim/simDrone.ts (exec(), stepFlight()).

Definition in file fc_core.c.

Macro Definition Documentation

◆ DEG2RAD

#define DEG2RAD 0.017453292519943295f

Definition at line 15 of file fc_core.c.

◆ OK

#define OK ( ) result(c, PV2_ACK_OK, PV2_R_NONE)

◆ REJ

#define REJ ( why ) result(c, PV2_ACK_REJECTED, (why))

◆ INVALID

#define INVALID ( w ) result(c, PV2_ACK_INVALID, (w))

Function Documentation

◆ SPSC_DEFINE() [1/4]

SPSC_DEFINE	(	s_cmdq	,
		fc_cmd_t	,
		8u
	)

◆ SPSC_DEFINE() [2/4]

SPSC_DEFINE	(	s_resq	,
		fc_res_t	,
		8u
	)

◆ SPSC_DEFINE() [3/4]

SPSC_DEFINE	(	s_evq	,
		fc_event_t	,
		16u
	)

◆ SPSC_DEFINE() [4/4]

SPSC_DEFINE	(	s_batq	,
		fc_batt_sig_t	,
		8u
	)

◆ fc_init()

void fc_init ( const fc_config_t * cfg )

Definition at line 71 of file fc_core.c.

References alt_ctrl_init(), fc_config_t::batt_monitor, g_fc_dbg_in_air, fc_config_t::n_motors, PV2_BATT_OK, PV2_MODE_ANGLE, s_ac, s_armed, s_auto, s_batt_fail, s_batt_have_hb, s_batt_hb_set, s_batt_level, s_batt_lock, s_batt_present, s_batt_stale, s_calibrating, s_cfg, s_checks_ready, s_dbg_forced, s_failsafe, s_flight_ms, s_flow_bad_ms, s_fs_long, s_fs_short_hold, s_geofence, s_geofence_ev, s_have_last, s_have_rc, s_have_v, s_in_air, s_landed, s_mode, s_sensors, s_sp, s_test_motor, s_took_off, s_touch_ms, fc_config_t::sensors, and spsc_reset().

Referenced by app_start().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fc_batt_signal()

int fc_batt_signal ( const fc_batt_sig_t * b )

0 = queued, -1 = full

Definition at line 94 of file fc_core.c.

References b, and spsc_push().

Referenced by batt_task_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fc_batt_heartbeat()

void fc_batt_heartbeat ( uint32_t now )

every BattTask step

Definition at line 95 of file fc_core.c.

References s_batt_hb_ms, s_batt_hb_set, and SPSC_BARRIER.

Referenced by batt_task_step().

Here is the caller graph for this function:

◆ fc_set_sensors()

void fc_set_sensors ( uint16_t sensors )

sensors detected at run time (a late expansion board): atomic store

Definition at line 97 of file fc_core.c.

References s_sensors.

Referenced by app_init(), and app_step().

Here is the caller graph for this function:

◆ fc_setpoint()

void fc_setpoint ( fc_setpoint_t * out )

last setpoints, same context

Definition at line 98 of file fc_core.c.

References s_sp.

Referenced by flight_service().

Here is the caller graph for this function:

◆ fc_is_core_op()

uint8_t fc_is_core_op ( uint8_t op )

ops executed by the core

Definition at line 100 of file fc_core.c.

References FC_OP_MOTOR_STOP, PV2_OP_ARM, PV2_OP_DISARM, PV2_OP_HOLD, PV2_OP_KILL, PV2_OP_LAND, PV2_OP_MOTOR_TEST, PV2_OP_RTH, PV2_OP_SET_MODE, and PV2_OP_TAKEOFF.

Referenced by commands().

Here is the caller graph for this function:

◆ fc_post()

int fc_post ( const fc_cmd_t * c )

0 = queued, -1 = full

Definition at line 112 of file fc_core.c.

References spsc_push().

Referenced by commands().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fc_take_result()

int fc_take_result ( fc_res_t * r )

1 if a result was taken

Definition at line 113 of file fc_core.c.

References spsc_pop().

Referenced by commands().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fc_take_event()

int fc_take_event ( fc_event_t * e )

Definition at line 114 of file fc_core.c.

References spsc_pop().

Referenced by events().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fc_set_ground_info()

void fc_set_ground_info	(	uint8_t	ready,
		uint8_t	batt_fail,
		uint8_t	calibrating
	)

ground information computed by the AppTask (atomic byte stores)

Definition at line 116 of file fc_core.c.

References s_batt_fail, s_calibrating, and s_checks_ready.

Referenced by app_step().

Here is the caller graph for this function:

◆ fc_snapshot()

void fc_snapshot ( fc_state_t * out )

Definition at line 121 of file fc_core.c.

References s_snap, s_snap_seq, and SPSC_BARRIER.

Referenced by armed_now(), batt_task_step(), build_checks(), log_records(), log_task_step(), send_flight(), and send_motors().

Here is the caller graph for this function:

◆ event()

static void event	(	uint8_t	level,
		uint16_t	code,
		int32_t	arg
	)

static

Definition at line 135 of file fc_core.c.

References fc_config_t::on_event, s_cfg, and spsc_push().

Referenced by air_ground(), app_link_failsafe(), battery(), disarm(), exec(), flow_failsafe(), setpoints(), and touch_down().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ has()

static uint8_t has ( uint16_t bits )

static

Definition at line 142 of file fc_core.c.

References s_sensors.

Referenced by air_ground(), alt_sensor(), exec(), flow_failsafe(), gnss(), indoor(), and setpoints().

Here is the caller graph for this function:

◆ gnss()

static uint8_t gnss ( void )

static

Definition at line 143 of file fc_core.c.

References has(), and PV2_S_GNSS.

Referenced by battery(), exec(), flow_failsafe(), fs_action(), and indoor().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ home_set()

static uint8_t home_set ( void )

static

Definition at line 144 of file fc_core.c.

Referenced by battery(), exec(), and fs_action().

Here is the caller graph for this function:

◆ alt_sensor()

static uint8_t alt_sensor ( void )

static

Definition at line 145 of file fc_core.c.

References has(), PV2_S_BARO, and PV2_S_TOF.

Referenced by alt_ok(), and exec().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ alt_ok()

static uint8_t alt_ok ( void )

static

Definition at line 146 of file fc_core.c.

References alt_sensor(), s_have_v, s_v, and fc_vert_t::valid.

Referenced by air_ground(), exec(), and setpoints().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ indoor()

static uint8_t indoor ( void )

static

Definition at line 147 of file fc_core.c.

References gnss(), has(), and PV2_S_TOF.

Referenced by ceiling_h_kind().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ clampf()

static float clampf	(	float	v,
		float	lo,
		float	hi
	)

static

Definition at line 148 of file fc_core.c.

Referenced by publish(), and setpoints().

Here is the caller graph for this function:

◆ ceiling_h_kind()

static float ceiling_h_kind ( int32_t * kind )

static

Definition at line 151 of file fc_core.c.

References fc_vert_t::h, indoor(), P_GF_CEILING, P_GF_MAX_ALT, PARAM, s_h_ref, s_v, fc_vert_t::tof_h, and fc_vert_t::tof_valid.

Referenced by ceiling_h(), and setpoints().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ceiling_h()

static float ceiling_h ( void )

static

Definition at line 163 of file fc_core.c.

References ceiling_h_kind().

Referenced by exec().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ lift_off()

static void lift_off ( void )

static

Definition at line 165 of file fc_core.c.

References s_in_air, s_landed, s_took_off, and s_touch_ms.

Referenced by air_ground(), and exec().

Here is the caller graph for this function:

◆ touch_down()

static void touch_down ( uint32_t now )

static

Definition at line 170 of file fc_core.c.

References event(), PV2_AUTO_LAND, PV2_AUTO_NONE, PV2_AUTO_RTH, PV2_E_LANDED, PV2_EV_INFO, s_auto, s_in_air, s_landed, s_landed_ms, and s_touch_ms.

Referenced by air_ground().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ hold_mode()

static uint8_t hold_mode ( void )

static

Definition at line 179 of file fc_core.c.

References PV2_MODE_ALT_HOLD, PV2_MODE_POS_HOLD, and s_mode.

Referenced by air_ground(), exec(), setpoints(), and update_sticks().

Here is the caller graph for this function:

◆ test_stop()

static void test_stop ( void )

static

Definition at line 181 of file fc_core.c.

References fc_config_t::motor_test, s_cfg, and s_test_motor.

Referenced by disarm(), exec(), and fc_service().

Here is the caller graph for this function:

◆ disarm()

static void disarm ( void )

static

Definition at line 187 of file fc_core.c.

References event(), fc_config_t::motors_arm, PV2_AUTO_NONE, PV2_E_DISARMED, PV2_EV_INFO, s_armed, s_auto, s_batt_lock, s_cfg, s_dbg_forced, s_failsafe, s_fs_long, s_fs_short_hold, s_geofence, s_in_air, and test_stop().

Referenced by app_link_failsafe(), exec(), and fc_service().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ batt_unusable()

static uint8_t batt_unusable ( void )

static

Definition at line 204 of file fc_core.c.

References fc_config_t::batt_monitor, PV2_BATT_CRITICAL, s_batt_level, s_batt_stale, and s_cfg.

Referenced by exec().

Here is the caller graph for this function:

◆ batt_owns_auto()

static uint8_t batt_owns_auto ( void )

static

Definition at line 209 of file fc_core.c.

References PV2_AUTO_LAND, PV2_AUTO_RTH, PV2_FS_BATTERY, s_auto, and s_failsafe.

Referenced by app_link_failsafe().

Here is the caller graph for this function:

◆ fs_action()

static uint8_t fs_action ( int p_idx )

static

Definition at line 215 of file fc_core.c.

References gnss(), home_set(), PARAM, PV2_AUTO_HOLD, PV2_AUTO_LAND, and PV2_AUTO_RTH.

Referenced by app_link_failsafe().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ result()

static void result	(	const fc_cmd_t *	c,
		uint8_t	res,
		uint8_t	why
	)

static

Definition at line 224 of file fc_core.c.

References FC_OP_MOTOR_STOP, fc_cmd_t::op, fc_cmd_t::seq, and spsc_push().

Referenced by ackc_begin(), ackc_finish(), exec(), and pv2_ack().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ exec()

static void exec	(	const fc_cmd_t *	c,
		uint32_t	now,
		uint8_t	est_running
	)

static

Definition at line 231 of file fc_core.c.

References alt_ctrl_reset(), alt_ok(), alt_sensor(), batt_unusable(), ceiling_h(), disarm(), event(), FC_ARM_RC_FRESH_MS, FC_OP_MOTOR_STOP, gnss(), fc_vert_t::h, has(), hold_mode(), home_set(), INVALID, fc_cmd_t::len, lift_off(), fc_config_t::motor_test, fc_config_t::motors_arm, fc_config_t::motors_built, ms, fc_config_t::n_motors, OK, fc_cmd_t::op, fc_cmd_t::p, P_ARM_REQUIRE_CHECKS, P_BEGINNER, P_TAKEOFF_ALT, PARAM, PV2_ACK_UNSUPPORTED, PV2_ARM_MAGIC0, PV2_ARM_MAGIC1, PV2_AUTO_HOLD, PV2_AUTO_LAND, PV2_AUTO_NONE, PV2_AUTO_RTH, PV2_AUTO_TAKEOFF, PV2_E_ARMED, PV2_E_MODE_CHANGED, PV2_E_MOTOR_KILLED, PV2_E_RTH_STARTED, PV2_E_TAKEOFF_STARTED, PV2_EV_CRITICAL, PV2_EV_INFO, PV2_MODE_ACRO, PV2_MODE_ALT_HOLD, PV2_MODE_POS_HOLD, PV2_OP_ARM, PV2_OP_DISARM, PV2_OP_HOLD, PV2_OP_KILL, PV2_OP_LAND, PV2_OP_MOTOR_TEST, PV2_OP_RTH, PV2_OP_SET_MODE, PV2_OP_TAKEOFF, PV2_PROPS_OFF_MAGIC, PV2_R_ARMED, PV2_R_BATTERY_LOW, PV2_R_CALIBRATING, PV2_R_CHECKS_FAILING, PV2_R_NO_GPS, PV2_R_NO_HOME, PV2_R_NONE, PV2_R_NOT_ARMED, PV2_R_NOT_LANDED, PV2_R_OUT_OF_RANGE, PV2_R_PROPS_CONFIRM_MISSING, PV2_R_SENSOR_MISSING, PV2_R_THROTTLE_NOT_LOW, PV2_R_USB_CONNECTED, pv2_rd_u16(), PV2_S_FLOW, PV2_S_GNSS, REJ, result(), s_ac, s_arm_ms, s_armed, s_auto, s_batt_fail, s_batt_lock, s_calibrating, s_cfg, s_checks_ready, s_failsafe, s_flight_ms, s_fs_long, s_geofence_ev, s_h_ground, s_h_ref, s_h_sp, s_have_rc, s_have_v, s_in_air, s_landed, s_mode, s_rc, s_rc_ms, s_test_motor, s_test_pct, s_test_until, s_to_lifted, s_to_t0, s_to_target, s_took_off, s_touch_ms, s_v, test_stop(), appl_rc_t::thr, fc_config_t::usb_host, and fc_vert_t::valid.

Referenced by fc_service().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_sticks()

static void update_sticks ( uint32_t now )

static

Definition at line 379 of file fc_core.c.

References appl_rc_read(), FC_RC_STALE_MS, hold_mode(), appl_rc_t::pitch, appl_rc_t::roll, s_armed, s_have_rc, s_last_thr, s_rc, s_rc_ms, s_rc_stale, s_stick, appl_rc_t::thr, and appl_rc_t::yaw.

Referenced by fc_service().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stick_moved()

static int stick_moved ( void )

static

Definition at line 400 of file fc_core.c.

References FC_STICK_CANCEL, appl_rc_t::pitch, appl_rc_t::roll, s_rc, s_rc_stale, appl_rc_t::thr, and appl_rc_t::yaw.

Referenced by fc_service().

Here is the caller graph for this function:

◆ app_link_failsafe()

static void app_link_failsafe ( uint32_t now )

static

Definition at line 408 of file fc_core.c.

References appl_silence_ms(), batt_owns_auto(), disarm(), event(), FC_FS_LONG_MS, FC_FS_SHORT_MS, FC_GROUND_DISARM_MS, FC_LINK_BACK_MS, fs_action(), P_FS_APP_LONG, P_FS_APP_SHORT, PARAM, PV2_AUTO_HOLD, PV2_AUTO_LAND, PV2_AUTO_NONE, PV2_E_FAILSAFE_APP_LINK, PV2_EV_ERROR, PV2_FS_APP_LINK, s_armed, s_auto, s_failsafe, s_fs_long, s_fs_short_hold, s_in_air, and s_silence.

Referenced by fc_service().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ battery()

static void battery ( uint32_t now )

static

Definition at line 447 of file fc_core.c.

References b, fc_config_t::batt_monitor, event(), FC_BATT_STALE_MS, gnss(), home_set(), PV2_AUTO_LAND, PV2_AUTO_RTH, PV2_BATT_CRITICAL, PV2_BATT_EMERGENCY, PV2_E_BATTERY_CRITICAL, PV2_E_BATTERY_EMERGENCY, PV2_E_BATTERY_LOW, PV2_E_BATTERY_MONITOR_LOST, PV2_E_FAILSAFE_BATTERY, PV2_EV_CRITICAL, PV2_EV_ERROR, PV2_EV_WARN, PV2_FS_BATTERY, s_armed, s_auto, s_batt_have_hb, s_batt_hb_ms, s_batt_hb_set, s_batt_level, s_batt_lock, s_batt_present, s_batt_stale, s_cfg, s_failsafe, s_fs_short_hold, s_in_air, s_touch_ms, SPSC_BARRIER, and spsc_pop().

Referenced by fc_service().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ air_ground()

static void air_ground	(	uint32_t	now,
		uint32_t	dt
	)

static

Definition at line 491 of file fc_core.c.

References alt_ok(), event(), FC_LIFTOFF_DH, FC_LIFTOFF_TIMEOUT_MS, FC_TOUCH_MS, FC_TOUCH_MS_NO_TOF, FC_TOUCH_VZ, g_fc_dbg_in_air, fc_vert_t::h, has(), hold_mode(), lift_off(), PV2_AUTO_LAND, PV2_AUTO_NONE, PV2_AUTO_RTH, PV2_AUTO_TAKEOFF, PV2_E_MODE_CHANGED, PV2_E_TAKEOFF_ABORTED, PV2_EV_INFO, PV2_EV_WARN, PV2_MODE_ALT_HOLD, PV2_MODE_POS_HOLD, PV2_S_FLOW, PV2_S_GNSS, PV2_S_TOF, s_arm_ms, s_armed, s_auto, s_dbg_forced, s_h_ground, s_h_sp, s_in_air, s_mode, s_stick, s_to_lifted, s_to_t0, s_to_target, s_took_off, s_touch_ms, s_v, fc_vert_t::tof_ground, fc_vert_t::tof_h, fc_vert_t::tof_valid, touch_down(), and fc_vert_t::vz.

Referenced by fc_service().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ flow_failsafe()

static void flow_failsafe ( uint32_t dt )

static

Definition at line 549 of file fc_core.c.

References event(), FC_FLOW_BACK_Q, FC_FLOW_CHECK_H, FC_FLOW_LOST_MS, FC_FLOW_LOST_Q, fc_vert_t::flow_q, fc_vert_t::flow_usable, gnss(), fc_vert_t::h, has(), P_FS_FLOW_LOST, PARAM, PV2_AUTO_LAND, PV2_AUTO_TAKEOFF, PV2_E_FLOW_LOST, PV2_EV_WARN, PV2_FS_FLOW, PV2_MODE_ALT_HOLD, PV2_MODE_POS_HOLD, PV2_S_FLOW, s_armed, s_auto, s_failsafe, s_flow_bad_ms, s_h_sp, s_have_v, s_in_air, s_mode, s_v, fc_vert_t::tof_ground, fc_vert_t::tof_h, and fc_vert_t::tof_valid.

Referenced by fc_service().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setpoints()

static void setpoints	(	uint32_t	now,
		uint32_t	dt_ms
	)

static

Definition at line 573 of file fc_core.c.

References alt_ctrl_step(), alt_ctrl_vz_sp(), alt_ok(), ceiling_h_kind(), clampf(), fc_vert_t::cos_tilt, DEG2RAD, event(), FC_LAND_SLOW_VZ, FC_TAKEOFF_VZ, fc_vert_t::flow_usable, fc_vert_t::h, has(), hold_mode(), P_BEGINNER, P_LAND_SPEED, P_MAX_CLIMB, P_MAX_TILT, P_MAX_YAW_RATE, PARAM, fc_setpoint_t::pos_hold, PV2_AUTO_FAILSAFE, PV2_AUTO_HOLD, PV2_AUTO_LAND, PV2_AUTO_NONE, PV2_AUTO_RTH, PV2_AUTO_TAKEOFF, PV2_E_GEOFENCE_BREACH, PV2_EV_WARN, PV2_FS_FLOW, PV2_FS_GEOFENCE, PV2_MODE_POS_HOLD, PV2_S_TOF, s_ac, s_armed, s_auto, s_failsafe, s_geofence, s_geofence_ev, s_h_sp, s_have_v, s_in_air, s_mode, s_sp, s_stick, s_to_lifted, s_to_t0, s_to_target, s_v, sp, alt_ctrl_t::thr_max, fc_vert_t::tof_ground, fc_vert_t::tof_h, fc_vert_t::tof_valid, and fc_vert_t::vz.

Referenced by fc_service().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ publish()

static void publish ( void )

static

Definition at line 668 of file fc_core.c.

References fc_state_t::alt_rel_cm, fc_state_t::armed, fc_state_t::autom, fc_state_t::batt_level, fc_config_t::batt_monitor, fc_state_t::batt_stale, clampf(), fc_state_t::failsafe, FC_FS_SHORT_MS, fc_state_t::flight_time_s, fc_state_t::fs_long, fc_state_t::geofence, fc_vert_t::h, fc_state_t::in_air, fc_state_t::input, fc_state_t::landed, fc_state_t::mode, fc_state_t::motor_out_pct, PV2_INPUT_APP, PV2_INPUT_NONE, fc_state_t::rc_stale, s_armed, s_auto, s_batt_level, s_batt_stale, s_cfg, s_failsafe, s_flight_ms, s_fs_long, s_geofence, s_h_ref, s_have_v, s_in_air, s_landed, s_mode, s_rc_stale, s_silence, s_snap, s_snap_seq, s_stick, s_test_motor, s_test_pct, s_took_off, s_v, fc_state_t::silence_ms, SPSC_BARRIER, fc_state_t::stick, fc_state_t::test_motor, fc_vert_t::valid, fc_vert_t::vz, and fc_state_t::vz_cms.

Referenced by fc_service().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fc_service()

void fc_service	(	uint32_t	now,
		uint8_t	est_running,
		const fc_vert_t *	v
	)

v: NULL = no vertical estimate (in air only via g_fc_dbg_in_air)

Definition at line 699 of file fc_core.c.

References air_ground(), app_link_failsafe(), battery(), disarm(), exec(), FC_FS_SHORT_MS, FC_LINK_BACK_MS, FC_NO_TAKEOFF_MS, FC_STICK_CANCEL, flow_failsafe(), fc_vert_t::flow_q, P_AUTO_DISARM, P_RTH_STICK_CANCEL, PARAM, appl_rc_t::pitch, publish(), PV2_AUTO_HOLD, PV2_AUTO_NONE, PV2_AUTO_RTH, PV2_FS_APP_LINK, appl_rc_t::roll, s_arm_ms, s_armed, s_auto, s_failsafe, s_flight_ms, s_have_last, s_have_v, s_in_air, s_landed_ms, s_last_ms, s_rc, s_rc_stale, s_silence, s_test_motor, s_test_until, s_took_off, s_v, setpoints(), spsc_pop(), stick_moved(), test_stop(), update_sticks(), and fc_vert_t::valid.

Referenced by flight_service(), and ImuTask().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ g_fc_dbg_in_air

volatile uint8_t g_fc_dbg_in_air

Definition at line 17 of file fc_core.c.

Referenced by air_ground(), and fc_init().

◆ s_cfg

fc_config_t s_cfg

static

Definition at line 24 of file fc_core.c.

Referenced by batt_unusable(), battery(), disarm(), event(), exec(), fc_init(), publish(), and test_stop().

◆ s_sensors

volatile uint16_t s_sensors

static

Definition at line 25 of file fc_core.c.

Referenced by fc_init(), fc_set_sensors(), and has().

◆ s_checks_ready

volatile uint8_t s_checks_ready

static

Definition at line 28 of file fc_core.c.

Referenced by exec(), fc_init(), and fc_set_ground_info().

◆ s_batt_fail

volatile uint8_t s_batt_fail

static

Definition at line 28 of file fc_core.c.

Referenced by exec(), fc_init(), and fc_set_ground_info().

◆ s_calibrating

volatile uint8_t s_calibrating

static

Definition at line 28 of file fc_core.c.

Referenced by exec(), fc_init(), and fc_set_ground_info().

◆ s_armed

uint8_t s_armed

static

Definition at line 31 of file fc_core.c.

Referenced by air_ground(), app_link_failsafe(), battery(), disarm(), exec(), fc_init(), fc_service(), flow_failsafe(), publish(), setpoints(), and update_sticks().

◆ s_mode

uint8_t s_mode = PV2_MODE_ANGLE

static

Definition at line 31 of file fc_core.c.

Referenced by air_ground(), exec(), fc_init(), flow_failsafe(), hold_mode(), publish(), and setpoints().

◆ s_auto

uint8_t s_auto

static

Definition at line 31 of file fc_core.c.

Referenced by air_ground(), app_link_failsafe(), batt_owns_auto(), battery(), disarm(), exec(), fc_init(), fc_service(), flow_failsafe(), publish(), setpoints(), and touch_down().

◆ s_failsafe

uint8_t s_failsafe

static

Definition at line 31 of file fc_core.c.

Referenced by app_link_failsafe(), batt_owns_auto(), battery(), disarm(), exec(), fc_init(), fc_service(), flow_failsafe(), publish(), and setpoints().

◆ s_in_air

uint8_t s_in_air

static

Definition at line 31 of file fc_core.c.

Referenced by air_ground(), app_link_failsafe(), battery(), disarm(), exec(), fc_init(), fc_service(), flow_failsafe(), lift_off(), publish(), setpoints(), and touch_down().

◆ s_fs_long

uint8_t s_fs_long

static

Definition at line 32 of file fc_core.c.

Referenced by app_link_failsafe(), disarm(), exec(), fc_init(), and publish().

◆ s_fs_short_hold

uint8_t s_fs_short_hold

static

Definition at line 32 of file fc_core.c.

Referenced by app_link_failsafe(), battery(), disarm(), and fc_init().

◆ s_took_off

uint8_t s_took_off

static

Definition at line 33 of file fc_core.c.

Referenced by air_ground(), exec(), fc_init(), fc_service(), lift_off(), and publish().

◆ s_arm_ms

uint32_t s_arm_ms

static

Definition at line 34 of file fc_core.c.

Referenced by air_ground(), exec(), and fc_service().

◆ s_landed_ms

uint32_t s_landed_ms

static

Definition at line 34 of file fc_core.c.

Referenced by fc_service(), and touch_down().

◆ s_flight_ms

uint32_t s_flight_ms

static

Definition at line 34 of file fc_core.c.

Referenced by exec(), fc_init(), fc_service(), and publish().

◆ s_last_ms

uint32_t s_last_ms

static

Definition at line 34 of file fc_core.c.

Referenced by fc_service().

◆ s_have_last

uint8_t s_have_last

static

Definition at line 35 of file fc_core.c.

Referenced by fc_init(), and fc_service().

◆ s_rc

appl_rc_t s_rc

static

Definition at line 36 of file fc_core.c.

Referenced by exec(), fc_service(), stick_moved(), and update_sticks().

◆ s_rc_ms

uint32_t s_rc_ms

static

Definition at line 37 of file fc_core.c.

Referenced by exec(), and update_sticks().

◆ s_have_rc

uint8_t s_have_rc

static

Definition at line 38 of file fc_core.c.

Referenced by exec(), fc_init(), and update_sticks().

◆ s_last_thr

float s_last_thr = -1.0f

static

Definition at line 39 of file fc_core.c.

Referenced by update_sticks().

◆ s_stick

float s_stick[4]

static

Definition at line 40 of file fc_core.c.

Referenced by air_ground(), publish(), setpoints(), and update_sticks().

◆ s_rc_stale

uint8_t s_rc_stale = 1

static

Definition at line 41 of file fc_core.c.

Referenced by fc_service(), publish(), stick_moved(), and update_sticks().

◆ s_silence

uint32_t s_silence

static

Definition at line 42 of file fc_core.c.

Referenced by app_link_failsafe(), fc_service(), and publish().

◆ s_v

fc_vert_t s_v

static

Definition at line 45 of file fc_core.c.

Referenced by air_ground(), alt_ok(), ceiling_h_kind(), exec(), fc_service(), flow_failsafe(), publish(), and setpoints().

◆ s_have_v

uint8_t s_have_v

static

Definition at line 46 of file fc_core.c.

Referenced by alt_ok(), exec(), fc_init(), fc_service(), flow_failsafe(), publish(), and setpoints().

◆ s_h_ref

float s_h_ref

static

Definition at line 47 of file fc_core.c.

Referenced by ceiling_h_kind(), exec(), and publish().

◆ s_h_ground

float s_h_ground

static

Definition at line 48 of file fc_core.c.

Referenced by air_ground(), and exec().

◆ s_h_sp

float s_h_sp

static

Definition at line 49 of file fc_core.c.

Referenced by air_ground(), exec(), flow_failsafe(), and setpoints().

◆ s_to_target

float s_to_target

static

Definition at line 50 of file fc_core.c.

Referenced by air_ground(), exec(), and setpoints().

◆ s_to_t0

uint32_t s_to_t0

static

Definition at line 51 of file fc_core.c.

Referenced by air_ground(), exec(), and setpoints().

◆ s_to_lifted

uint8_t s_to_lifted

static

Definition at line 52 of file fc_core.c.

Referenced by air_ground(), exec(), and setpoints().

◆ s_dbg_forced

uint8_t s_dbg_forced

static

Definition at line 52 of file fc_core.c.

Referenced by air_ground(), disarm(), and fc_init().

◆ s_landed

uint8_t s_landed

static

Definition at line 52 of file fc_core.c.

Referenced by exec(), fc_init(), lift_off(), publish(), and touch_down().

◆ s_geofence

uint8_t s_geofence

static

Definition at line 52 of file fc_core.c.

Referenced by disarm(), fc_init(), publish(), and setpoints().

◆ s_geofence_ev

uint8_t s_geofence_ev

static

Definition at line 52 of file fc_core.c.

Referenced by exec(), fc_init(), and setpoints().

◆ s_touch_ms

uint32_t s_touch_ms

static

Definition at line 53 of file fc_core.c.

Referenced by air_ground(), battery(), exec(), fc_init(), lift_off(), and touch_down().

◆ s_flow_bad_ms

uint32_t s_flow_bad_ms

static

Definition at line 53 of file fc_core.c.

Referenced by fc_init(), and flow_failsafe().

◆ s_ac

alt_ctrl_t s_ac

static

Definition at line 54 of file fc_core.c.

Referenced by exec(), fc_init(), and setpoints().

◆ s_sp

fc_setpoint_t s_sp

static

Definition at line 55 of file fc_core.c.

Referenced by fc_init(), fc_setpoint(), and setpoints().

◆ s_batt_level

uint8_t s_batt_level

static

Definition at line 58 of file fc_core.c.

Referenced by batt_unusable(), battery(), fc_init(), and publish().

◆ s_batt_present

uint8_t s_batt_present

static

Definition at line 58 of file fc_core.c.

Referenced by battery(), and fc_init().

◆ s_batt_lock

uint8_t s_batt_lock

static

Definition at line 58 of file fc_core.c.

Referenced by battery(), disarm(), exec(), and fc_init().

◆ s_batt_stale

uint8_t s_batt_stale

static

Definition at line 58 of file fc_core.c.

Referenced by batt_unusable(), battery(), fc_init(), and publish().

◆ s_batt_have_hb

uint8_t s_batt_have_hb

static

Definition at line 58 of file fc_core.c.

Referenced by battery(), and fc_init().

◆ s_batt_hb_ms

volatile uint32_t s_batt_hb_ms

static

Definition at line 59 of file fc_core.c.

Referenced by battery(), and fc_batt_heartbeat().

◆ s_batt_hb_set

volatile uint8_t s_batt_hb_set

static

Definition at line 60 of file fc_core.c.

Referenced by battery(), fc_batt_heartbeat(), and fc_init().

◆ s_test_motor

uint8_t s_test_motor

static

Definition at line 63 of file fc_core.c.

Referenced by exec(), fc_init(), fc_service(), publish(), and test_stop().

◆ s_test_pct

uint8_t s_test_pct

static

Definition at line 63 of file fc_core.c.

Referenced by exec(), and publish().

◆ s_test_until

uint32_t s_test_until

static

Definition at line 64 of file fc_core.c.

Referenced by exec(), and fc_service().

◆ s_snap_seq

volatile uint32_t s_snap_seq

static

Definition at line 67 of file fc_core.c.

Referenced by fc_snapshot(), and publish().

◆ s_snap

fc_state_t s_snap

static

Definition at line 68 of file fc_core.c.

Referenced by fc_snapshot(), and publish().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ DEG2RAD

◆ OK

◆ REJ

◆ INVALID

Function Documentation

◆ SPSC_DEFINE() [1/4]

◆ SPSC_DEFINE() [2/4]

◆ SPSC_DEFINE() [3/4]

◆ SPSC_DEFINE() [4/4]

◆ fc_init()

◆ fc_batt_signal()

◆ fc_batt_heartbeat()

◆ fc_set_sensors()

◆ fc_setpoint()

◆ fc_is_core_op()

◆ fc_post()

◆ fc_take_result()

◆ fc_take_event()

◆ fc_set_ground_info()

◆ fc_snapshot()

◆ event()

◆ has()

◆ gnss()

◆ home_set()

◆ alt_sensor()

◆ alt_ok()

◆ indoor()

◆ clampf()

◆ ceiling_h_kind()

◆ ceiling_h()

◆ lift_off()

◆ touch_down()

◆ hold_mode()

◆ test_stop()

◆ disarm()

◆ batt_unusable()

◆ batt_owns_auto()

◆ fs_action()

◆ result()

◆ exec()

◆ update_sticks()

◆ stick_moved()

◆ app_link_failsafe()

◆ battery()

◆ air_ground()

◆ flow_failsafe()

◆ setpoints()

◆ publish()

◆ fc_service()

Variable Documentation

◆ g_fc_dbg_in_air

◆ s_cfg

◆ s_sensors

◆ s_checks_ready

◆ s_batt_fail

◆ s_calibrating

◆ s_armed

◆ s_mode

◆ s_auto

◆ s_failsafe

◆ s_in_air

◆ s_fs_long

◆ s_fs_short_hold

◆ s_took_off

◆ s_arm_ms

◆ s_landed_ms

◆ s_flight_ms

◆ s_last_ms

◆ s_have_last

◆ s_rc

◆ s_rc_ms

◆ s_have_rc

◆ s_last_thr

◆ s_stick

◆ s_rc_stale