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
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
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;
661 printf(
"abs error cov = %f, close = %f\n", fabs(error_cov),
of_landing_ctrl.close_to_edge);
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; }
743 float gain_scaling = expf(
of_landing_ctrl.divergence_setpoint * t_interval);
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; }
851 float gain_scaling = exp(
of_landing_ctrl.divergence_setpoint * t_interval);
853 float weight_prediction = 0.5;
855 if (gain_scaling <= 1.0f) {
856 pstate = weight_prediction *
of_landing_ctrl.reduction_factor_elc * prediction + (1 - weight_prediction) *
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).
struct abi_struct abi_event
#define FLOW_OPTICFLOW_ID
Core autopilot interface common to all firmwares.
Common flight_plan functions shared between fixedwing and rotorcraft.
static struct uart_periph * dev
static void float_mat_div_scalar(float **o, float **a, const float scalar, const int m, const int n)
Divide 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_mul_scalar(float **o, float **a, const float scalar, const int m, const int n)
Multiply a matrix by a scalar.
#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.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static abi_event optical_flow_ev
void vertical_ctrl_module_run(void)
Run the vertical optical flow hover module.
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
#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)
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
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.
General attitude stabilization interface for rotorcrafts.
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.
void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
union StabilizationSetpoint::@354356152067340015216250042204214023005305374154 sp
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
union ThrustSetpoint::@302344065342070214103253314124156277107357236133 sp
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.