Autopilot. More...

#include <stdint.h>
#include "firmwares/rotorcraft/autopilot.h"
#include "mcu_periph/uart.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/radio_control.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
#include "subsystems/electrical.h"
#include "subsystems/settings.h"
#include "subsystems/datalink/telemetry.h"
#include "firmwares/rotorcraft/navigation.h"
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "generated/settings.h"
#include "subsystems/gps.h"
#include "pprz_version.h"
#include "modules/nav/nav_geofence.h"
#include "autopilot_arming_yaw.h"

Include dependency graph for autopilot.c:

Go to the source code of this file.

Macros
#define	AUTOPILOT_IN_FLIGHT_TIME 20
	time steps for in_flight detection (at 20Hz, so 20=1second) More...

#define	AUTOPILOT_IN_FLIGHT_MIN_SPEED 0.2
	minimum vertical speed for in_flight condition in m/s More...

#define	AUTOPILOT_IN_FLIGHT_MIN_ACCEL 2.0
	minimum vertical acceleration for in_flight condition in m/s^2 More...

#define	AUTOPILOT_IN_FLIGHT_MIN_THRUST 500
	minimum thrust for in_flight condition in pprz_t units (max = 9600) More...

#define	FAILSAFE_DESCENT_SPEED 1.5
	Set descent speed in failsafe mode. More...

#define	FAILSAFE_MODE_TOO_FAR_FROM_HOME AP_MODE_FAILSAFE
	Mode that is set when the plane is really too far from home. More...

#define	MODE_STARTUP AP_MODE_KILL

#define	UNLOCKED_HOME_MODE TRUE

#define	NAV_PRESCALER (PERIODIC_FREQUENCY / NAV_FREQ)

#define	THRESHOLD_1_PPRZ (MIN_PPRZ / 2)

#define	THRESHOLD_2_PPRZ (MAX_PPRZ / 2)

Functions
static int	ahrs_is_aligned (void)

void	send_autopilot_version (struct transport_tx trans, struct link_device dev)

static void	send_alive (struct transport_tx trans, struct link_device dev)

static void	send_attitude (struct transport_tx trans, struct link_device dev)

static void	send_status (struct transport_tx trans, struct link_device dev)

static void	send_energy (struct transport_tx trans, struct link_device dev)

static void	send_fp (struct transport_tx trans, struct link_device dev)

static void	send_rc (struct transport_tx trans, struct link_device dev)

static void	send_rotorcraft_rc (struct transport_tx trans, struct link_device dev)

static void	send_dl_value (struct transport_tx trans, struct link_device dev)

static void	send_rotorcraft_cmd (struct transport_tx trans, struct link_device dev)

void	autopilot_init (void)
	Autopilot inititalization. More...

void	autopilot_periodic (void)

void	autopilot_SetModeHandler (float mode)
	AP mode setting handler. More...

void	autopilot_set_mode (uint8_t new_autopilot_mode)

bool	autopilot_guided_goto_ned (float x, float y, float z, float heading)
	Set position and heading setpoints in GUIDED mode. More...

bool	autopilot_guided_goto_ned_relative (float dx, float dy, float dz, float dyaw)
	Set position and heading setpoints wrt. More...

bool	autopilot_guided_goto_body_relative (float dx, float dy, float dz, float dyaw)
	Set position and heading setpoints wrt. More...

bool	autopilot_guided_move_ned (float vx, float vy, float vz, float heading)
	Set velocity and heading setpoints in GUIDED mode. More...

void	autopilot_guided_update (uint8_t flags, float x, float y, float z, float yaw)
	Set guided setpoints using flag mask in GUIDED mode. More...

void	autopilot_check_in_flight (bool motors_on)

void	autopilot_set_motors_on (bool motors_on)

static uint8_t	ap_mode_of_3way_switch (void)
	get autopilot mode as set by RADIO_MODE 3-way switch More...

void	autopilot_on_rc_frame (void)
	Get autopilot mode from two 2way switches. More...

Variables
uint8_t	autopilot_mode

uint8_t	autopilot_mode_auto2

bool	autopilot_in_flight

uint32_t	autopilot_in_flight_counter

uint16_t	autopilot_flight_time
	flight time in seconds. More...

bool	autopilot_motors_on

bool	kill_throttle

bool	autopilot_rc

bool	autopilot_power_switch

bool	autopilot_ground_detected

bool	autopilot_detect_ground_once

Detailed Description

Autopilot.

Definition in file autopilot.c.

Macro Definition Documentation

#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL 2.0

minimum vertical acceleration for in_flight condition in m/s^2

Definition at line 100 of file autopilot.c.

Referenced by autopilot_check_in_flight().

#define AUTOPILOT_IN_FLIGHT_MIN_SPEED 0.2

minimum vertical speed for in_flight condition in m/s

Definition at line 95 of file autopilot.c.

Referenced by autopilot_check_in_flight().

#define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500

minimum thrust for in_flight condition in pprz_t units (max = 9600)

Definition at line 105 of file autopilot.c.

Referenced by autopilot_check_in_flight().

#define AUTOPILOT_IN_FLIGHT_TIME 20

time steps for in_flight detection (at 20Hz, so 20=1second)

Definition at line 90 of file autopilot.c.

Referenced by autopilot_check_in_flight().

#define FAILSAFE_DESCENT_SPEED 1.5

Set descent speed in failsafe mode.

Definition at line 123 of file autopilot.c.

Referenced by autopilot_set_mode().

#define FAILSAFE_MODE_TOO_FAR_FROM_HOME AP_MODE_FAILSAFE

Mode that is set when the plane is really too far from home.

Definition at line 129 of file autopilot.c.

Referenced by autopilot_periodic().

#define MODE_STARTUP AP_MODE_KILL

Definition at line 145 of file autopilot.c.

Referenced by autopilot_init(), and autopilot_set_mode().

#define NAV_PRESCALER (PERIODIC_FREQUENCY / NAV_FREQ)

Definition at line 343 of file autopilot.c.

Referenced by autopilot_periodic().

#define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)

Definition at line 716 of file autopilot.c.

Referenced by ap_mode_of_3way_switch().

#define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)

Definition at line 717 of file autopilot.c.

Referenced by ap_mode_of_3way_switch().

#define UNLOCKED_HOME_MODE TRUE

Definition at line 151 of file autopilot.c.

Referenced by autopilot_on_rc_frame(), and pprz_mode_update().

Function Documentation

static int ahrs_is_aligned ( void )

inlinestatic

Definition at line 109 of file autopilot.c.

References stateIsAttitudeValid().

Referenced by autopilot_on_rc_frame(), autopilot_set_mode(), and autopilot_set_motors_on().

Here is the call graph for this function:

Here is the caller graph for this function:

static uint8_t ap_mode_of_3way_switch ( void )

static

get autopilot mode as set by RADIO_MODE 3-way switch

Definition at line 720 of file autopilot.c.

References autopilot_mode_auto2, MODE_AUTO1, MODE_MANUAL, radio_control, RADIO_MODE, THRESHOLD_1_PPRZ, THRESHOLD_2_PPRZ, and RadioControl::values.

Referenced by autopilot_on_rc_frame().

Here is the caller graph for this function:

void autopilot_check_in_flight ( bool motors_on )

Definition at line 669 of file autopilot.c.

References autopilot_in_flight, autopilot_in_flight_counter, AUTOPILOT_IN_FLIGHT_MIN_ACCEL, AUTOPILOT_IN_FLIGHT_MIN_SPEED, AUTOPILOT_IN_FLIGHT_MIN_THRUST, AUTOPILOT_IN_FLIGHT_TIME, stabilization_cmd, stateGetAccelNed_f(), and stateGetSpeedNed_f().

Referenced by failsafe_check().

Here is the call graph for this function:

Here is the caller graph for this function:

bool autopilot_guided_goto_body_relative	(	float	dx,
		float	dy,
		float	dz,
		float	dyaw
	)

Set position and heading setpoints wrt.

current position AND heading in GUIDED mode.

Parameters

dx	relative position (body frame, forward) in meters.
dy	relative position (body frame, right) in meters.
dz	relative position (body frame, down) in meters.
dyaw	Offset relative to current heading setpoint in radians.

Returns: TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 573 of file autopilot.c.

References AP_MODE_GUIDED, autopilot_guided_goto_ned(), autopilot_mode, heading, FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), stateIsLocalCoordinateValid(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

bool autopilot_guided_goto_ned	(	float	x,
		float	y,
		float	z,
		float	heading
	)

Set position and heading setpoints in GUIDED mode.

Parameters

x	North position (local NED frame) in meters.
y	East position (local NED frame) in meters.
z	Down position (local NED frame) in meters.
heading	Setpoint in radians.

Returns: TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 550 of file autopilot.c.

References AP_MODE_GUIDED, autopilot_mode, guidance_h_set_guided_heading(), guidance_h_set_guided_pos(), and guidance_v_set_guided_z().

Referenced by autopilot_guided_goto_body_relative(), autopilot_guided_goto_ned_relative(), and mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

bool autopilot_guided_goto_ned_relative	(	float	dx,
		float	dy,
		float	dz,
		float	dyaw
	)

Set position and heading setpoints wrt.

current position in GUIDED mode.

Parameters

dx	Offset relative to current north position (local NED frame) in meters.
dy	Offset relative to current east position (local NED frame) in meters.
dz	Offset relative to current down position (local NED frame) in meters.
dyaw	Offset relative to current heading setpoint in radians.

Returns: TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 561 of file autopilot.c.

References AP_MODE_GUIDED, autopilot_guided_goto_ned(), autopilot_mode, heading, FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), stateIsLocalCoordinateValid(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

bool autopilot_guided_move_ned	(	float	vx,
		float	vy,
		float	vz,
		float	heading
	)

Set velocity and heading setpoints in GUIDED mode.

Parameters

vx	North velocity (local NED frame) in meters/sec.
vy	East velocity (local NED frame) in meters/sec.
vz	Down velocity (local NED frame) in meters/sec.
heading	Setpoint in radians.

Returns: TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 586 of file autopilot.c.

References AP_MODE_GUIDED, autopilot_mode, guidance_h_set_guided_heading(), guidance_h_set_guided_vel(), and guidance_v_set_guided_vz().

Referenced by mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

void autopilot_guided_update	(	uint8_t	flags,
		float	x,
		float	y,
		float	z,
		float	yaw
	)

Set guided setpoints using flag mask in GUIDED mode.

Parameters

flags	Bits 0-3 are used to determine the axis system to be used. If bits 0 and 1 are clear then the coordinates are set in absolute NE coordinates. If bit 1 is set bit 0 is ignored. Bits 5-7 define whether the setpoints should be used as position or velocity. Bit flags are defined as follows: bit 0: x,y as offset coordinates bit 1: x,y in body coordinates bit 2: z as offset coordinates bit 3: yaw as offset coordinates bit 4: free bit 5: x,y as vel bit 6: z as vel bit 7: yaw as rate
x	North position/velocity in meters or meters/sec.
y	East position/velocity in meters or meters/sec.
z	Down position/velocity in meters or meters/sec.
yaw	Heading or heading rate setpoint in radians or radians/sec.