Autopilot. More...

#include <stdint.h>
#include "firmwares/rotorcraft/autopilot.h"
#include "mcu_periph/uart.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_rate.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "generated/settings.h"
#include "subsystems/gps.h"
#include "pprz_version.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_set_mode (uint8_t new_autopilot_mode)

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

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

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

void	autopilot_check_in_flight (bool_t motors_on)

void	autopilot_set_motors_on (bool_t 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_t	autopilot_in_flight

uint32_t	autopilot_in_flight_counter

uint16_t	autopilot_flight_time
	flight time in seconds. More...

bool_t	autopilot_motors_on

bool_t	kill_throttle

bool_t	autopilot_rc

bool_t	autopilot_power_switch

bool_t	autopilot_ground_detected

bool_t	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 93 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 88 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 98 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 83 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 116 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 122 of file autopilot.c.

Referenced by autopilot_periodic().

#define MODE_STARTUP AP_MODE_KILL

Definition at line 138 of file autopilot.c.

Referenced by autopilot_init(), and autopilot_set_mode().

#define NAV_PRESCALER (PERIODIC_FREQUENCY / NAV_FREQ)

Definition at line 333 of file autopilot.c.

Referenced by autopilot_periodic().

#define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)

Definition at line 599 of file autopilot.c.

Referenced by ap_mode_of_3way_switch().

#define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)

Definition at line 600 of file autopilot.c.

Referenced by ap_mode_of_3way_switch().

#define UNLOCKED_HOME_MODE TRUE

Definition at line 144 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 102 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 603 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_t motors_on )

Definition at line 552 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, FALSE, stabilization_cmd, stateGetAccelNed_f(), stateGetSpeedNed_f(), and TRUE.

Referenced by failsafe_check().

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t 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 539 of file autopilot.c.

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

Referenced by dl_parse_msg(), and mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t 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 516 of file autopilot.c.

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t 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.