Basic guidance for rover. More...

#include "firmwares/rover/guidance/rover_guidance.h"
#include "generated/airframe.h"
#include "generated/autopilot_core_guidance.h"
#include "state.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for rover_guidance.c:

Go to the source code of this file.

Macros
#define	AUTOPILOT_CORE_GUIDANCE_C

#define	MAX_POS_ERR 10.f

#define	MAX_SPEED_ERR 10.f

#define	MAX_INTEGRAL_CMD (MAX_PPRZ / 10.f)

#define	PROXIMITY_DIST 0.5f

Functions
void	rover_guidance_init (void)

void	rover_guidance_periodic (void)

static float	compute_pid (struct RoverGuidancePID *pid)

void	rover_guidance_run (float *heading_sp)

void	rover_guidance_enter (void)

void	rover_guidance_set_speed_igain (uint32_t igain)

void	rover_guidance_set_turn_igain (uint32_t igain)

Variables
struct RoverGuidance	rover_guidance

Detailed Description

Basic guidance for rover.

Implement standard PID control loop to track a navigation target. Guidance "modes" are using the autopilot generation with the "guidance" state machine.

Definition in file rover_guidance.c.

Macro Definition Documentation

◆ AUTOPILOT_CORE_GUIDANCE_C

#define AUTOPILOT_CORE_GUIDANCE_C

Definition at line 28 of file rover_guidance.c.

◆ MAX_INTEGRAL_CMD

#define MAX_INTEGRAL_CMD (MAX_PPRZ / 10.f)

Definition at line 80 of file rover_guidance.c.

◆ MAX_POS_ERR

#define MAX_POS_ERR 10.f

Definition at line 78 of file rover_guidance.c.

◆ MAX_SPEED_ERR

#define MAX_SPEED_ERR 10.f

Definition at line 79 of file rover_guidance.c.

◆ PROXIMITY_DIST

#define PROXIMITY_DIST 0.5f

Definition at line 81 of file rover_guidance.c.

Function Documentation

◆ compute_pid()

static float compute_pid ( struct RoverGuidancePID * pid )

static

Definition at line 73 of file rover_guidance.c.

References RoverGuidancePID::d, RoverGuidancePID::d_err, RoverGuidancePID::err, RoverGuidancePID::i, RoverGuidancePID::p, and RoverGuidancePID::sum_err.

Referenced by rover_guidance_run().

Here is the caller graph for this function:

◆ rover_guidance_enter()

void rover_guidance_enter ( void )

Definition at line 124 of file rover_guidance.c.

References RoverGuidanceSetpoint::heading, RoverGuidanceSetpoint::mask, FloatEulers::psi, rover_guidance, RoverGuidance::sp, and stateGetNedToBodyEulers_f().

Here is the call graph for this function:

◆ rover_guidance_init()

void rover_guidance_init ( void )

Definition at line 42 of file rover_guidance.c.

References RoverGuidancePID::d, RoverGuidancePID::d_err, RoverGuidancePID::err, FLOAT_VECT2_ZERO, RoverGuidanceSetpoint::heading, RoverGuidancePID::i, RoverGuidancePID::p, RoverGuidanceSetpoint::pos, rover_guidance, RoverGuidance::sp, RoverGuidance::speed_pid, RoverGuidancePID::sum_err, and RoverGuidance::turn_pid.

◆ rover_guidance_periodic()

void rover_guidance_periodic ( void )

Definition at line 67 of file rover_guidance.c.

◆ rover_guidance_run()

void rover_guidance_run ( float * heading_sp )

Definition at line 83 of file rover_guidance.c.

References RoverGuidance::cmd, compute_pid(), RoverGuidancePID::d_err, RoverGuidancePID::err, float_vect2_norm(), RoverGuidanceSetpoint::heading, MAX_POS_ERR, MAX_PPRZ, MAX_SPEED_ERR, RoverGuidanceControl::motor_speed, RoverGuidanceControl::motor_turn, RoverGuidanceSetpoint::pos, PROXIMITY_DIST, FloatEulers::psi, FloatRates::r, rover_guidance, RoverGuidance::sp, RoverGuidance::speed_pid, stateGetBodyRates_f(), stateGetHorizontalSpeedNorm_f(), stateGetNedToBodyEulers_f(), stateGetPositionEnu_f(), RoverGuidancePID::sum_err, TRIM_PPRZ, RoverGuidance::turn_pid, VECT2_DIFF, FloatVect2::x, and FloatVect2::y.

Here is the call graph for this function:

◆ rover_guidance_set_speed_igain()

void rover_guidance_set_speed_igain ( uint32_t igain )

Definition at line 169 of file rover_guidance.c.

References RoverGuidancePID::i, rover_guidance, RoverGuidance::speed_pid, and RoverGuidancePID::sum_err.

◆ rover_guidance_set_turn_igain()

void rover_guidance_set_turn_igain ( uint32_t igain )

Definition at line 175 of file rover_guidance.c.

References RoverGuidancePID::i, rover_guidance, RoverGuidancePID::sum_err, and RoverGuidance::turn_pid.

Variable Documentation

◆ rover_guidance

struct RoverGuidance rover_guidance

Definition at line 1 of file rover_guidance.c.

Referenced by rover_guidance_enter(), rover_guidance_init(), rover_guidance_run(), rover_guidance_set_speed_igain(), and rover_guidance_set_turn_igain().

Macros

Functions

Variables