Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
rover_guidance_steering.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com>
3  * Hector García <noeth3r@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
22 #define AUTOPILOT_CORE_GUIDANCE_C
23 
26 
27 #include "generated/airframe.h"
28 
31 #include "autopilot.h"
32 #include "navigation.h"
33 #include "state.h"
34 
35 #include "filters/pid.h" // Used for p+i speed controller
36 
37 #include <math.h>
38 #include <stdio.h>
39 
40 // Guidance control main variables
42 
43 static struct PID_f rover_pid;
44 static float time_step;
45 static float last_speed_cmd;
47 
50 {
54 
55  last_speed_cmd = 0.0;
57 
60  guidance_control.kp = 10;
61  guidance_control.ki = 100;
62 
64 
65  // Based on autopilot state machine frequency
66  time_step = 1.f/PERIODIC_FREQUENCY;
67 }
68 
70 // Steering control (GVF)
71 void rover_guidance_steering_heading_ctrl(float omega) //GVF give us this omega
72 {
73  float delta = 0.0;
74 
75  // Speed is bounded to avoid GPS noise while driving at small velocity
76  float speed = BoundSpeed(stateGetHorizontalSpeedNorm_f());
77 
78  if (fabs(omega)>0.0) {
79  delta = DegOfRad(-atanf(omega*DRIVE_SHAFT_DISTANCE/speed));
80  }
81 
83 }
84 
85 // Speed control (feed feed forward + propotional + integral controler) (PID)
87 {
88  // - Looking for setting update
91  }
94  //reset_pid_f(&rover_pid);
95  }
96 
97  // - Updating PID
100 
102 }
103 
104 
107 {
108  // Reset speed PID
109  if (rover_pid.sum != 0) {
111  }
112 }
113 
115 {
116  guidance_control.cmd.delta = 0.0;
117  guidance_control.cmd.speed = 0.0;
118 }
Core autopilot interface common to all firmwares.
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
#define MAX_PPRZ
Definition: paparazzi.h:8
Several forms of PID controllers.
float sum
integral of input
Definition: pid.h:45
float g[3]
controller gains (Kp, Kd, Ki)
Definition: pid.h:46
static float get_pid_f(struct PID_f *pid)
Get current value of the PID command.
Definition: pid.h:89
static void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki)
Set gains of the PID struct.
Definition: pid.h:113
static void reset_pid_f(struct PID_f *pid)
Reset PID struture, gains left unchanged.
Definition: pid.h:98
static float update_pid_f(struct PID_f *pid, float value, float dt)
Update PID with a new value and return new command.
Definition: pid.h:68
static void init_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki, float max_sum)
Definition: pid.h:50
Simple PID structure floating point.
Definition: pid.h:42
Generic interface for radio control modules.
#define AP_MODE_KILL
Static autopilot modes.
void rover_guidance_steering_kill(void)
static uint8_t last_ap_mode
rover_ctrl guidance_control
Mandatory dependencies header.
void rover_guidance_steering_speed_ctrl(void)
static float time_step
void rover_guidance_steering_init(void)
INIT function.
static struct PID_f rover_pid
void rover_guidance_steering_pid_reset(void)
PID RESET function.
static float last_speed_cmd
void rover_guidance_steering_heading_ctrl(float omega)
CTRL functions.
#define BoundDelta(delta)
MACROS.
#define BoundThrottle(throttle)
#define SR_MEASURED_KF
#define BoundSpeed(speed)
#define DRIVE_SHAFT_DISTANCE
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98