79 #include "generated/airframe.h"
99 #define OFL_AGL_ID ABI_BROADCAST
104 #ifndef OFL_OPTICAL_FLOW_ID
105 #define OFL_OPTICAL_FLOW_ID ABI_BROADCAST
111 #define OFL_PGAIN 0.40
115 #define OFL_IGAIN 0.01
119 #define OFL_DGAIN 0.0
122 #ifndef OFL_PGAIN_HORIZONTAL_FACTOR
123 #define OFL_PGAIN_HORIZONTAL_FACTOR 0.05
126 #ifndef OFL_IGAIN_HORIZONTAL_FACTOR
127 #define OFL_IGAIN_HORIZONTAL_FACTOR 0.1
130 #ifndef OFL_ROLL_TRIM
131 #define OFL_ROLL_TRIM 0.0f
134 #ifndef OFL_PITCH_TRIM
135 #define OFL_PITCH_TRIM 0.0f
138 #ifndef OFL_VISION_METHOD
139 #define OFL_VISION_METHOD 1
142 #ifndef OFL_CONTROL_METHOD
143 #define OFL_CONTROL_METHOD 0
146 #ifndef OFL_COV_METHOD
147 #define OFL_COV_METHOD 0
151 #ifndef OFL_COV_WINDOW_SIZE
152 #define OFL_COV_WINDOW_SIZE 30
155 #ifndef OFL_COV_LANDING_LIMIT
156 #define OFL_COV_LANDING_LIMIT 2.2
159 #ifndef OFL_COV_SETPOINT
160 #define OFL_COV_SETPOINT -0.0075
164 #define OFL_LP_CONST 0.02
167 #ifndef OFL_P_LAND_THRESHOLD
168 #define OFL_P_LAND_THRESHOLD 0.15
171 #ifndef OFL_ELC_OSCILLATE
172 #define OFL_ELC_OSCILLATE true
175 #ifndef OFL_CLOSE_TO_EDGE
176 #define OFL_CLOSE_TO_EDGE 0.025
180 #ifndef OFL_PGAIN_ADAPTIVE
181 #define OFL_PGAIN_ADAPTIVE 0.50
184 #ifndef OFL_IGAIN_ADAPTIVE
185 #define OFL_IGAIN_ADAPTIVE 0.50
188 #ifndef OFL_DGAIN_ADAPTIVE
189 #define OFL_DGAIN_ADAPTIVE 0.50
193 #define OFL_OMEGA_LR 0.0
197 #define OFL_OMEGA_FB 0.0
200 #ifndef OFL_ACTIVE_MOTION
201 #define OFL_ACTIVE_MOTION 0
204 #ifndef OFL_FRONT_DIV_THRESHOLD
205 #define OFL_FRONT_DIV_THRESHOLD 0.3
210 #define HORIZONTAL_RATE_CONTROL 0
214 #define ACTIVE_RATES 0
219 #define MINIMUM_GAIN 0.1
222 #define INCREASE_GAIN_PER_SECOND 0.10
257 #define RECURSIVE_LEARNING 0
264 #define TEXTON_DISTRIBUTION_PATH /data/ftp/internal_000
302 pprz_msg_send_DIVERGENCE(trans,
dev, AC_ID,
528 printf(
"LOADING WEIGHTS!\n");
542 printf(
"LEARNING WEIGHTS!\n");
548 printf(
"Time to learn: %f\n", end - start);
558 float new_flow_x = 0.0;
559 float new_flow_y = 0.0;
598 static const float max_div_dt = 0.20f;
673 static const float phase_0_set_point = 0.0f;
723 if (t_interval < 0) { t_interval = 0.0f; }
740 if (t_interval < 0) { t_interval = 0.0f; }
787 float phase_0_set_point = 0.0f;
811 float phase_0_set_point = 0.0f;
848 if (t_interval < 0) { t_interval = 0.0f; }
853 float weight_prediction = 0.5;
855 if (gain_scaling <= 1.0f) {
931 float period_factor = 0.2;
932 float sine_threshold = 0.75;
933 float omega_set_point = 20;
944 float period_factor = 0.2;
945 float sine_threshold = 0.9;
946 float trim_set_point = 2.0f;
965 float add_active_sine = 0.0f;
968 float sine_period = 0.05;
969 float sine_amplitude = RadOfDeg(1.0);
971 add_active_sine = sine_amplitude * sinf((1.0f / sine_period) * sine_time * 2 * M_PI);
972 printf(
"add_active_sine = %f\n", add_active_sine);
980 BoundAbs(pitch_cmd, RadOfDeg(10.0));
981 BoundAbs(roll_cmd, RadOfDeg(10.0));
1002 uint32_t thrust = 0.85 * nominal_throttle;
1003 Bound(thrust, 0.6 * nominal_throttle, 0.9 *
MAX_PPRZ);
1129 float quality
UNUSED,
float size_divergence)
1153 if (dt_flow_front > 0) {
1188 printf(
"Logging textons!\n");
1215 int i, j, read_result;
1219 printf(
"Loading textons from %s\n", filename);
1230 if (read_result == EOF) {
break; }
1232 if (read_result == EOF) {
break; }
1234 if (read_result == EOF) {
break; }
1239 if (read_result == EOF) {
break; }
1242 if (read_result == EOF) {
break; }
1247 printf(
"There was an error opening %s\n", filename);
1257 printf(
"Loading distribution\n");
1261 printf(
"Learning!\n");
1269 printf(
"Saving!\n");
1273 printf(
"Learned! Fit error = %f\n", fit_error);
1297 float augmented_sample[D_1];
1299 augmented_sample[
D] = 1.0f;
1302 for (d = 0; d < D_1; d++) {
1319 for (sam = 0; sam < count; sam++) {
1323 for (d = 0; d <
D; d++) {
1324 augmented_sample[d] = samples[sam][d];
1332 float sum_abs_err = 0;
1333 for (sam = 0; sam < count; sam++) {
1335 sum_abs_err += fabs(prediction - targets[sam]);
1337 (*fit_error) = sum_abs_err / count;
1353 float _u[1][length_sample];
1354 float _uT[length_sample][1];
1355 float _phi[1][length_sample];
1356 float _phiT[length_sample][1];
1357 float _k[length_sample][1];
1358 float _ke[length_sample][1];
1359 float prediction, error;
1360 float _u_P_uT[1][1];
1362 float _O[length_sample][length_sample];
1366 for (i = 0; i < length_sample; i++) {
1367 _u[0][i] = sample[i];
1368 _uT[i][0] = sample[i];
1382 MAT_MUL(1, length_sample, length_sample, phi, u,
P);
1384 for (i = 0; i < length_sample; i++) {
1385 phiT[i][0] = phi[0][i];
1388 MAT_MUL(1, length_sample, 1, u_P_uT, phi, uT);
1389 scalar = u_P_uT[0][0];
1394 error =
target - prediction;
1397 for (i = 0; i < length_sample; i++) {
1401 MAT_MUL(length_sample, 1, length_sample, O, k, phi);
1402 MAT_SUB(length_sample, length_sample,
P,
P, O);
1434 float _AA[count][D_1];
1436 float _targets_all[count][1];
1439 for (sam = 0; sam < count; sam++) {
1440 for (d = 0; d <
D; d++) {
1441 AA[sam][d] = samples[sam][d];
1448 targets_all[sam][0] = targets[sam];
1454 float _parameters[D_1][1];
1463 float _bb[count][1];
1472 MAT_SUB(count, 1, C, bb, targets_all);
1474 for (sam = 0; sam < count; sam++) {
1475 *fit_error += fabs(C[sam][0]);
1477 *fit_error /= count;
1479 for (d = 0; d < D_1; d++) {
1494 sum +=
weights[i] * distribution[i];
1528 printf(
"No weights file!\n");
1534 if (read_result == EOF) {
break; }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
#define FLOW_OPTICFLOW_ID
Core autopilot interface common to all firmwares.
Common flight_plan functions shared between fixedwing and rotorcraft.
if(GpsFixValid() &&e_identification_started)
static void float_mat_mul_scalar(float **o, float **a, float scalar, int m, int n)
Multiply a matrix by a scalar.
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
static void float_mat_div_scalar(float **o, float **a, float scalar, int m, int n)
Divide a matrix by a scalar.
int32_t phi
in rad with INT32_ANGLE_FRAC
#define ANGLE_BFP_OF_REAL(_af)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Adaptation block of the vertical guidance.
void recursive_least_squares_batch(float *targets, float **samples, uint8_t D, uint16_t count, float *params, float *fit_error)
Recursively fit a linear model from samples to target values - batch mode, possibly for initializatio...
void learn_from_file(void)
#define TEXTON_DISTRIBUTION_PATH
void fit_linear_model_OF(float *targets, float **samples, uint8_t D, uint16_t count, float *params, float *fit_error)
Fit a linear model from samples to target values.
#define OFL_PGAIN_ADAPTIVE
static abi_event optical_flow_ev
#define OFL_IGAIN_HORIZONTAL_FACTOR
float * text_dists[MAX_SAMPLES_LEARNING]
float lp_divergence_front
static int32_t PID_divergence_control(float divergence_setpoint, float P, float I, float D, float dt)
Determine and set the thrust for constant divergence control.
float sonar_OF[MAX_SAMPLES_LEARNING]
#define RECURSIVE_LEARNING
void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Function definitions Callback function of the ground altitude.
static void reset_all_vars(void)
Reset all variables:
#define OFL_FRONT_DIV_THRESHOLD
static void set_cov_div(int32_t thrust)
Set the covariance of the divergence and the thrust / past divergence This funciton should only be ca...
float start_setpoint_ramp
static FILE * distribution_logger
float divergence_setpoint
void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
#define HORIZONTAL_RATE_CONTROL
#define OFL_OPTICAL_FLOW_ID
#define INCREASE_GAIN_PER_SECOND
#define OFL_CLOSE_TO_EDGE
void optical_flow_landing_init(void)
Initialize the optical flow landing module.
#define OFL_CONTROL_METHOD
#define OFL_VISION_METHOD
float texton_distribution_stereoboard[n_ts]
#define OFL_ELC_OSCILLATE
static FILE * weights_file
void guidance_module_enter(void)
Entering the module (user switched to module)
void save_texton_distribution(void)
void guidance_module_run(bool in_flight)
void vertical_ctrl_module_run(bool in_flight)
Run the optical flow landing module.
static void update_errors(float error, float dt)
Updates the integral and differential errors for PID control and sets the previous error.
float divergence_vision_dt
float past_divergence_history[OFL_COV_WINDOW_SIZE]
#define OFL_DGAIN_ADAPTIVE
#define OFL_COV_WINDOW_SIZE
unsigned int n_read_samples
float cov_divs_log[MAX_SAMPLES_LEARNING]
float predict_gain(float *distribution)
#define OFL_P_LAND_THRESHOLD
void recursive_least_squares(float target, float *sample, uint8_t length_sample, UNUSED float *params)
float thrust_history[OFL_COV_WINDOW_SIZE]
struct OpticalFlowLanding of_landing_ctrl
void load_texton_distribution(void)
float divergence_history[OFL_COV_WINDOW_SIZE]
#define OFL_COV_LANDING_LIMIT
float gains[MAX_SAMPLES_LEARNING]
#define OFL_ACTIVE_MOTION
float previous_divergence_setpoint
float module_active_time_sec
#define OFL_IGAIN_ADAPTIVE
static abi_event agl_ev
The altitude ABI event.
#define OFL_PGAIN_HORIZONTAL_FACTOR
static void send_divergence(struct transport_tx *trans, struct link_device *dev)
static uint32_t final_landing_procedure(void)
Execute a final landing procedure.
This module implements optical flow landings in which the divergence is kept constant.
#define MAX_SAMPLES_LEARNING
float omega_LR
Set point for the left-right ventral flow.
float dgain_adaptive
D-gain for adaptive gain control.
float roll_trim
Roll trim angle in degrees.
float igain
I-gain for constant divergence control.
float pitch_trim
Pitch trim angle in degrees.
float igain_adaptive
I-gain for adaptive gain control.
float omega_FB
Set point for the front-back ventral flow.
float cov_set_point
for adaptive gain control, setpoint of the covariance (oscillations)
float dgain
D-gain for constant divergence control.
float cov_limit
for fixed gain control, what is the cov limit triggering the landing
float pgain
P-gain for constant divergence control (from divergence error to thrust)
float front_div_threshold
Threshold when the front div gets above this value, it will command a horizontal stop.
float reduction_factor_elc
reduction factor - after oscillation, how much to reduce the gain...
float ramp_duration
ramp duration in seconds for when the divergence setpoint changes
float vel
vertical velocity as determined with sonar (only used when using "fake" divergence)
float divergence_setpoint
setpoint for constant divergence approach
float pgain_adaptive
P-gain for adaptive gain control.
bool elc_oscillate
Whether or not to oscillate at beginning of elc to find optimum gain.
float t_transition
how many seconds the drone has to be oscillating in order to transition to the hover phase with reduc...
uint32_t VISION_METHOD
whether to use vision (1) or Optitrack / sonar (0)
bool load_weights
load the weights that were learned before
float lp_const
low-pass value for divergence
float d_err
difference of error for the D-gain
float divergence
Divergence estimate.
volatile bool learn_gains
set to true if the robot needs to learn a mapping from texton distributions to the p-gain
uint32_t active_motion
Method for actively inducing motion.
float lp_factor_prediction
low-pass value for the predicted P-value
float sum_err
integration of the error for I-gain
float igain_horizontal_factor
factor multiplied with the vertical I-gain for horizontal ventral-flow-based control
float agl_lp
low-pass version of agl
float lp_cov_div_factor
low-pass factor for the covariance of divergence in order to trigger the second landing phase in the ...
uint32_t CONTROL_METHOD
type of divergence control: 0 = fixed gain, 1 = adaptive gain, 2 = exponential time landing control....
float nominal_thrust
nominal thrust around which the PID-control operates
uint32_t COV_METHOD
method to calculate the covariance: between thrust and div (0) or div and div past (1)
float previous_err
Previous divergence tracking error.
uint32_t delay_steps
number of delay steps for div past
float agl
agl = height from sonar (only used when using "fake" divergence)
bool use_bias
if true, a bias 1.0f will be added to the feature vector (texton distribution) - this typically leads...
uint32_t window_size
number of time steps in "window" used for getting the covariance
float p_land_threshold
if during the exponential landing the gain reaches this value, the final landing procedure is trigger...
float pgain_horizontal_factor
factor multiplied with the vertical P-gain for horizontal ventral-flow-based control
float close_to_edge
if abs(cov_div - reference cov_div) < close_to_edge, then texton distributions and gains are stored f...
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
void pprz_svd_solve_float(float **x, float **u, float *w, float **v, float **b, int m, int n, int l)
SVD based linear solver.
int pprz_svd_float(float **a, float *w, float **v, int m, int n)
SVD decomposition.
Matrix decompositions in floating point.
Simple matrix helper macros.
#define MAT_MUL(_i, _k, _j, C, A, B)
#define MAT_SUB(_i, _j, C, A, B)
float covariance_f(float *arr1, float *arr2, uint32_t n_elements)
Compute the covariance of two arrays V(X) = E[(X-E[X])(Y-E[Y])] = E[XY] - E[X]E[Y] where E[X] is the ...
struct VerticalGuidance guidance_v
Vertical guidance for rotorcrafts.
float nominal_throttle
nominal throttle for hover.
General attitude stabilization interface for rotorcrafts.
void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
struct Stabilization stabilization
struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers)
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
General stabilization interface for rotorcrafts.
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
static const struct usb_device_descriptor dev
union StabilizationSetpoint::@278 sp
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
union ThrustSetpoint::@284 sp
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
float * texton_distribution
Takes an image and represents the texture and colors in the image with a texton histogram.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.