AP ( AutoPilot ) tasks. More...

#include <math.h>
#include "firmwares/fixedwing/main_ap.h"
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "inter_mcu.h"
#include "link_mcu.h"
#include "subsystems/gps.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "state.h"
#include "firmwares/fixedwing/autopilot.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include <CTRL_TYPE_H>
#include "firmwares/fixedwing/nav.h"
#include "generated/flight_plan.h"
#include "subsystems/datalink/datalink.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/datalink/telemetry.h"
#include "subsystems/settings.h"
#include "generated/modules.h"
#include "generated/settings.h"
#include "rc_settings.h"
#include "subsystems/abi.h"
#include "led.h"
#include "modules/nav/nav_geofence.h"

Include dependency graph for main_ap.c:

Go to the source code of this file.

Macros
#define	MODULES_C

#define	ABI_C

#define	COMMAND_ROLL_TRIM 0

#define	COMMAND_PITCH_TRIM 0

#define	COMMAND_YAW_TRIM 0

#define	TELEMETRY_FREQUENCY 60

#define	RC_LOST_MODE PPRZ_MODE_HOME
	mode to enter when RC is lost in PPRZ_MODE_MANUAL or PPRZ_MODE_AUTO1 More...

#define	CATASTROPHIC_BAT_KILL_DELAY 5
	Maximum time allowed for catastrophic battery level before going into kill mode. More...

#define	KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
	Maximum distance from HOME waypoint before going into kill mode. More...

#define	MIN_SPEED_FOR_TAKEOFF 5.
	Default minimal speed for takeoff in m/s. More...

Functions
void	init_ap (void)

void	handle_periodic_tasks_ap (void)

static uint8_t	pprz_mode_update (void)
	Update paparazzi mode. More...

static uint8_t	mcu1_status_update (void)

static void	copy_from_to_fbw (void)
	Send back uncontrolled channels. More...

static void	telecommand_task (void)
	Function to be called when a message from FBW is available. More...

void	reporting_task (void)
	Send a series of initialisation messages followed by a stream of periodic ones. More...

void	navigation_task (void)
	Compute desired_course. More...

void	attitude_loop (void)

void	sensors_task (void)
	Run at PERIODIC_FREQUENCY (60Hz if not defined) More...

void	monitor_task (void)
	monitor stuff run at 1Hz More...

void	event_task_ap (void)

Variables
static uint8_t	mcu1_ppm_cpt

tid_t	modules_tid
	id for modules_periodic_task() timer More...

tid_t	telemetry_tid
	id for telemetry_periodic() timer More...

tid_t	sensors_tid
	id for sensors_task() timer More...

tid_t	attitude_tid
	id for attitude_loop() timer More...

tid_t	navigation_tid
	id for navigation_task() timer More...

tid_t	monitor_tid
	id for monitor_task() timer More...

Detailed Description

AP ( AutoPilot ) tasks.

This process is reponsible for the collecting the different sensors data, calling the appropriate estimation algorithms and running the different control loops.

Definition in file main_ap.c.

Macro Definition Documentation

#define ABI_C

Definition at line 33 of file main_ap.c.

#define CATASTROPHIC_BAT_KILL_DELAY 5

Maximum time allowed for catastrophic battery level before going into kill mode.

Definition at line 633 of file main_ap.c.

Referenced by monitor_task().

#define COMMAND_PITCH_TRIM 0

Definition at line 100 of file main_ap.c.

Referenced by init_ap().

#define COMMAND_ROLL_TRIM 0

Definition at line 96 of file main_ap.c.

Referenced by init_ap().

#define COMMAND_YAW_TRIM 0

Definition at line 104 of file main_ap.c.

Referenced by init_ap().

#define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)

Maximum distance from HOME waypoint before going into kill mode.

Definition at line 638 of file main_ap.c.

Referenced by monitor_task().

#define MIN_SPEED_FOR_TAKEOFF 5.

Default minimal speed for takeoff in m/s.

Definition at line 643 of file main_ap.c.

Referenced by monitor_task().

#define MODULES_C

Definition at line 31 of file main_ap.c.

#define RC_LOST_MODE PPRZ_MODE_HOME

mode to enter when RC is lost in PPRZ_MODE_MANUAL or PPRZ_MODE_AUTO1

Definition at line 375 of file main_ap.c.

Referenced by telecommand_task().

#define TELEMETRY_FREQUENCY 60

Definition at line 119 of file main_ap.c.

Referenced by init_ap(), and main_init().

Function Documentation

void attitude_loop ( void )

Definition at line 554 of file main_ap.c.

References ap_state, autopilot_flight_time, h_ctl_aileron_setpoint, h_ctl_attitude_loop(), h_ctl_elevator_setpoint, h_ctl_pitch_setpoint, inter_mcu_received_ap, kill_throttle, launch, link_mcu_send(), MAX_PPRZ, nav_pitch, nav_throttle_setpoint, pprz_mode, PPRZ_MODE_AUTO2, PPRZ_MUTEX_LOCK, PPRZ_MUTEX_UNLOCK, TRIM_PPRZ, TRIM_UPPRZ, v_ctl_climb_loop(), v_ctl_landing_loop(), v_ctl_mode, V_CTL_MODE_AUTO_CLIMB, V_CTL_MODE_AUTO_THROTTLE, V_CTL_MODE_LANDING, v_ctl_pitch_setpoint, v_ctl_throttle_setpoint, v_ctl_throttle_slew(), v_ctl_throttle_slewed, and vsupply.

Referenced by event_task_ap(), handle_periodic_tasks_ap(), and sim_periodic_task().

Here is the call graph for this function:

Here is the caller graph for this function:

static void copy_from_to_fbw ( void )

inlinestatic

Send back uncontrolled channels.

Definition at line 360 of file main_ap.c.

References ap_state, fbw_state, PPRZ_MUTEX_LOCK, PPRZ_MUTEX_UNLOCK, and RADIO_YAW.

Referenced by telecommand_task().

Here is the caller graph for this function:

void event_task_ap ( void )

Definition at line 679 of file main_ap.c.

References attitude_loop(), BaroEvent, DatalinkEvent, inter_mcu_received_fbw, link_mcu_event_task(), mcu_event(), and telecommand_task().

Referenced by sim_periodic_task().

Here is the call graph for this function:

Here is the caller graph for this function:

void handle_periodic_tasks_ap ( void )

Definition at line 264 of file main_ap.c.

References attitude_loop(), baro_periodic(), LED_PERIODIC, monitor_task(), navigation_task(), reporting_task(), sensors_task(), and sys_time_check_and_ack_timer().

Here is the call graph for this function:

void init_ap ( void )

Todo:: , properly implement or remove

init done in main_fbw in single MCU

Todo:: : properly implement/fix a triggered attitude loop

start interrupt task

Definition at line 174 of file main_ap.c.

References ABI_BROADCAST, ahrs_aligner_init(), ahrs_init(), ap_state, audio_telemetry_init(), autopilot_init(), baro_init(), BARO_PERIODIC_FREQUENCY, Imu::body_to_imu, COMMAND_PITCH_TRIM, COMMAND_ROLL_TRIM, COMMAND_YAW_TRIM, CONTROL_FREQUENCY, downlink_init(), h_ctl_init(), imu, IO0DIR, IO0SET, link_mcu_init(), mcu_int_enable, nav_init(), NAVIGATION_FREQUENCY, orientationGetQuat_f(), PPRZ_MUTEX_LOCK, PPRZ_MUTEX_UNLOCK, settings_init(), stateInit(), sys_time_register_timer(), TELEMETRY_FREQUENCY, and v_ctl_init().

Referenced by sim_init().

Here is the call graph for this function:

Here is the caller graph for this function:

static uint8_t mcu1_status_update ( void )

inlinestatic

Definition at line 346 of file main_ap.c.

References mcu1_status.

Referenced by telecommand_task().

Here is the caller graph for this function:

void monitor_task ( void )

monitor stuff run at 1Hz

Definition at line 647 of file main_ap.c.

References autopilot_flight_time, CATASTROPHIC_BAT_KILL_DELAY, datalink_time, DefaultChannel, DefaultDevice, dist2_to_home, KILL_MODE_DISTANCE, kill_throttle, launch, MIN_SPEED_FOR_TAKEOFF, sys_time::nb_sec, Square, stateGetHorizontalSpeedNorm_f(), and vsupply.

Referenced by handle_periodic_tasks_ap(), and sim_monitor_task().

Here is the call graph for this function:

Here is the caller graph for this function:

void navigation_task ( void )

Compute desired_course.

Definition at line 490 of file main_ap.c.

References autopilot_send_mode(), callTCAS(), common_nav_periodic_task_4Hz(), DefaultChannel, DefaultDevice, gps_lost, h_ctl_auto1_rate, h_ctl_course_loop(), lateral_mode, LATERAL_MODE_COURSE, launch, nav_home(), nav_periodic_task(), nav_without_gps(), pprz_mode, PPRZ_MODE_AUTO2, PPRZ_MODE_GPS_OUT_OF_ORDER, PPRZ_MODE_HOME, SEND_NAVIGATION, v_ctl_altitude_loop(), v_ctl_mode, and V_CTL_MODE_AUTO_ALT.

Referenced by handle_periodic_tasks_ap(), and sim_nav_task().

Here is the call graph for this function:

Here is the caller graph for this function:

static uint8_t pprz_mode_update ( void )

inlinestatic

Update paparazzi mode.

Definition at line 308 of file main_ap.c.

References ModeUpdate, pprz_mode, PPRZ_MODE_AUTO1, PPRZ_MODE_AUTO2, PPRZ_MODE_GPS_OUT_OF_ORDER, PPRZ_MODE_HOME, PPRZ_MODE_MANUAL, PPRZ_MODE_OF_PULSE, RADIO_MODE, THRESHOLD2, and UNLOCKED_HOME_MODE.

Referenced by telecommand_task().

Here is the caller graph for this function:

void reporting_task ( void )

Send a series of initialisation messages followed by a stream of periodic ones.

Called at 60Hz.

Definition at line 462 of file main_ap.c.

References DefaultChannel, DefaultDevice, DefaultPeriodic, and send_autopilot_version().

Referenced by handle_periodic_tasks_ap(), and sim_periodic_task().

Here is the call graph for this function:

Here is the caller graph for this function:

void sensors_task ( void )

Run at PERIODIC_FREQUENCY (60Hz if not defined)

Definition at line 619 of file main_ap.c.

References update_ahrs_from_sim().

Referenced by handle_periodic_tasks_ap(), and sim_periodic_task().

Here is the call graph for this function:

Here is the caller graph for this function:

static void telecommand_task ( void )

inlinestatic

Function to be called when a message from FBW is available.

In AUTO1 mode, compute roll setpoint and pitch setpoint from RADIO_ROLL and RADIO_PITCH

Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL]

Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH]

Else asynchronously set by h_ctl_course_loop()

In AUTO1, throttle comes from RADIO_THROTTLE In MANUAL, the value is copied to get it in the telemetry

else asynchronously set by v_ctl_climb_loop();

Definition at line 381 of file main_ap.c.

References autopilot_flight_time, autopilot_send_mode(), copy_from_to_fbw(), current, datalink_lost(), energy, fbw_state, FLOAT_OF_PPRZ, h_ctl_pitch_setpoint, h_ctl_roll_setpoint, higher_than_max_altitude(), launch, mcu1_status_update(), pprz_mode, PPRZ_MODE_AUTO1, PPRZ_MODE_AUTO2, PPRZ_MODE_GPS_OUT_OF_ORDER, PPRZ_MODE_HOME, PPRZ_MODE_MANUAL, pprz_mode_update(), PPRZ_MUTEX_LOCK, PPRZ_MUTEX_UNLOCK, RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, RC_LOST_MODE, rc_settings(), THROTTLE_THRESHOLD_TAKEOFF, too_far_from_home, v_ctl_throttle_setpoint, and vsupply.

Referenced by event_task_ap().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

tid_t attitude_tid

id for attitude_loop() timer

Definition at line 153 of file main_ap.c.

uint8_t mcu1_ppm_cpt

static

Definition at line 146 of file main_ap.c.

tid_t modules_tid

id for modules_periodic_task() timer

Definition at line 150 of file main_ap.c.

tid_t monitor_tid

id for monitor_task() timer

Definition at line 155 of file main_ap.c.

tid_t navigation_tid

id for navigation_task() timer

Definition at line 154 of file main_ap.c.

tid_t sensors_tid

id for sensors_task() timer

Definition at line 152 of file main_ap.c.

tid_t telemetry_tid

id for telemetry_periodic() timer

Definition at line 151 of file main_ap.c.

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

Function Documentation

Variable Documentation