Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
optical_flow_landing.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015 Guido de Croon.
3 *
4 * This file is part of Paparazzi.
5 *
6 * Paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * Paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with Paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
59
63
65
66// SSL:
71float *weights;
72
73// for "communication" with file logger:
74float cov_div;
75float pstate;
77
78
79#include "generated/airframe.h"
80#include "paparazzi.h"
81#include "modules/core/abi.h"
86
87// used for automated landing:
88#include "autopilot.h"
91
92// for measuring time
93#include "mcu_periph/sys_time.h"
94
95#include "math/pprz_stat.h"
96
97/* Default sonar/agl to use */
98#ifndef OFL_AGL_ID
99#define OFL_AGL_ID ABI_BROADCAST
100#endif
102
103/* Use optical flow estimates */
104#ifndef OFL_OPTICAL_FLOW_ID
105#define OFL_OPTICAL_FLOW_ID ABI_BROADCAST
106#endif
108
109// Other default values:
110#ifndef OFL_PGAIN
111#define OFL_PGAIN 0.40
112#endif
113
114#ifndef OFL_IGAIN
115#define OFL_IGAIN 0.01
116#endif
117
118#ifndef OFL_DGAIN
119#define OFL_DGAIN 0.0
120#endif
121
122#ifndef OFL_PGAIN_HORIZONTAL_FACTOR
123#define OFL_PGAIN_HORIZONTAL_FACTOR 0.05
124#endif
125
126#ifndef OFL_IGAIN_HORIZONTAL_FACTOR
127#define OFL_IGAIN_HORIZONTAL_FACTOR 0.1
128#endif
129
130#ifndef OFL_ROLL_TRIM
131#define OFL_ROLL_TRIM 0.0f
132#endif
133
134#ifndef OFL_PITCH_TRIM
135#define OFL_PITCH_TRIM 0.0f
136#endif
137
138#ifndef OFL_VISION_METHOD
139#define OFL_VISION_METHOD 1
140#endif
141
142#ifndef OFL_CONTROL_METHOD
143#define OFL_CONTROL_METHOD 0
144#endif
145
146#ifndef OFL_COV_METHOD
147#define OFL_COV_METHOD 0
148#endif
149
150// number of time steps used for calculating the covariance (oscillations)
151#ifndef OFL_COV_WINDOW_SIZE
152#define OFL_COV_WINDOW_SIZE 30
153#endif
154
155#ifndef OFL_COV_LANDING_LIMIT
156#define OFL_COV_LANDING_LIMIT 2.2
157#endif
158
159#ifndef OFL_COV_SETPOINT
160#define OFL_COV_SETPOINT -0.0075
161#endif
162
163#ifndef OFL_LP_CONST
164#define OFL_LP_CONST 0.02
165#endif
166
167#ifndef OFL_P_LAND_THRESHOLD
168#define OFL_P_LAND_THRESHOLD 0.15
169#endif
170
171#ifndef OFL_ELC_OSCILLATE
172#define OFL_ELC_OSCILLATE true
173#endif
174
175#ifndef OFL_CLOSE_TO_EDGE
176#define OFL_CLOSE_TO_EDGE 0.025
177#endif
178
179
180#ifndef OFL_PGAIN_ADAPTIVE
181#define OFL_PGAIN_ADAPTIVE 0.50
182#endif
183
184#ifndef OFL_IGAIN_ADAPTIVE
185#define OFL_IGAIN_ADAPTIVE 0.50
186#endif
187
188#ifndef OFL_DGAIN_ADAPTIVE
189#define OFL_DGAIN_ADAPTIVE 0.50
190#endif
191
192#ifndef OFL_OMEGA_LR
193#define OFL_OMEGA_LR 0.0
194#endif
195
196#ifndef OFL_OMEGA_FB
197#define OFL_OMEGA_FB 0.0
198#endif
199
200#ifndef OFL_ACTIVE_MOTION
201#define OFL_ACTIVE_MOTION 0
202#endif
203
204#ifndef OFL_FRONT_DIV_THRESHOLD
205#define OFL_FRONT_DIV_THRESHOLD 0.3
206#endif
207
208// Normally, horizontal control is done via sending angle commands to INDI, so 0 (false)
209// When this is 1 (true),a change in angle will be commanded instead.
210#define HORIZONTAL_RATE_CONTROL 0
211
212// Normally, ACTIVE_RATES = 0, and the estimated angle is immediately used for controlling the error to 0
213// If ACTIVE_RATES = 1, a sine is actively added to the commanded rates, so that there is often a rate
214#define ACTIVE_RATES 0
215
216// Constants
217// minimum value of the P-gain for divergence control
218// adaptive control / exponential gain control will not be able to go lower
219#define MINIMUM_GAIN 0.1
220
221// for exponential gain landing, gain increase per second during the first (hover) phase:
222#define INCREASE_GAIN_PER_SECOND 0.10
223
224// variables retained between module calls
225
226
228
229// horizontal loop:
237
238//float divergence_vision;
241//float cov_div;
242//float pstate, pused;
243float pused;
244float istate;
245float dstate;
251
253
254// *********************************
255// include and define stuff for SSL:
256// *********************************
257#define RECURSIVE_LEARNING 0
258#include <stdio.h>
260// it should now be set to 10 to match the number of textons on the stereoboard... this is extremely ugly.
261#define n_ts 10
263// On Bebop 2:
264#define TEXTON_DISTRIBUTION_PATH /data/ftp/internal_000
265// for RLS, recursive least squares:
266float **P_RLS;
267// forgetting factor:
268float lambda;
269static FILE *distribution_logger = NULL;
270static FILE *weights_file = NULL;
271unsigned int n_read_samples;
272// paparazzi files for doing svd etc.:
279
280// for ramping
285
286// for the exponentially decreasing gain strategy:
292
295
296// struct containing most relevant parameters
298
299// sending the divergence message to the ground station:
300static void send_divergence(struct transport_tx *trans, struct link_device *dev)
301{
302 pprz_msg_send_DIVERGENCE(trans, dev, AC_ID,
305}
306
309void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
310// Callback function of the optical flow estimate:
311void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
312 int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
313
314// common functions for different landing strategies:
315static void set_cov_div(int32_t thrust);
316static int32_t PID_divergence_control(float divergence_setpoint, float P, float I, float D, float dt);
317static void update_errors(float error, float dt);
319
320// resetting all variables to be called for instance when starting up / re-entering module
321static void reset_all_vars(void);
322
328
329void vertical_ctrl_module_run(bool in_flight);
330
335{
336 // filling the of_landing_ctrl struct with default values:
337 of_landing_ctrl.use_bias = true;
338 of_landing_ctrl.agl = 0.0f;
339 of_landing_ctrl.agl_lp = 0.0f;
340 of_landing_ctrl.vel = 0.0f;
341 of_landing_ctrl.divergence_setpoint = 0.0f; // For exponential gain landing, pick a negative value
342 of_landing_ctrl.cov_set_point = OFL_COV_SETPOINT;
343 of_landing_ctrl.cov_limit = fabsf((float)OFL_COV_LANDING_LIMIT);
344 of_landing_ctrl.lp_const = OFL_LP_CONST;
345 Bound(of_landing_ctrl.lp_const, 0.001f, 1.f);
349
350 of_landing_ctrl.pgain_horizontal_factor = OFL_PGAIN_HORIZONTAL_FACTOR;
351 of_landing_ctrl.igain_horizontal_factor = OFL_IGAIN_HORIZONTAL_FACTOR;
352 of_landing_ctrl.pitch_trim = OFL_PITCH_TRIM;
353 of_landing_ctrl.roll_trim = OFL_ROLL_TRIM;
354
355 of_landing_ctrl.divergence = 0.;
356 of_landing_ctrl.previous_err = 0.;
357 of_landing_ctrl.sum_err = 0.0f;
358 of_landing_ctrl.d_err = 0.0f;
359 of_landing_ctrl.nominal_thrust = (float)guidance_v.nominal_throttle / MAX_PPRZ; // copy this value from guidance
360 of_landing_ctrl.VISION_METHOD = OFL_VISION_METHOD;
361 of_landing_ctrl.CONTROL_METHOD = OFL_CONTROL_METHOD;
362 of_landing_ctrl.COV_METHOD = OFL_COV_METHOD;
363 of_landing_ctrl.delay_steps = 15;
365 of_landing_ctrl.pgain_adaptive = OFL_PGAIN_ADAPTIVE;
366 of_landing_ctrl.igain_adaptive = OFL_IGAIN_ADAPTIVE;
367 of_landing_ctrl.dgain_adaptive = OFL_DGAIN_ADAPTIVE;
368 of_landing_ctrl.reduction_factor_elc =
369 0.25f; // TODO: it used to be 0.80 for the oscillation landings... make a separate factor for the predictions.
370 // for exponential gain landing, after detecting oscillations, the gain is multiplied with this factor
371 of_landing_ctrl.lp_cov_div_factor =
372 0.99f; // low pass filtering cov div so that the drone is really oscillating when triggering the descent
373 of_landing_ctrl.t_transition = 2.f;
374 // if the gain reaches this value during an exponential landing, the drone makes the final landing.
375 of_landing_ctrl.p_land_threshold = OFL_P_LAND_THRESHOLD;
376 of_landing_ctrl.elc_oscillate = OFL_ELC_OSCILLATE;
377 of_landing_ctrl.close_to_edge = OFL_CLOSE_TO_EDGE;
378 of_landing_ctrl.lp_factor_prediction = 0.95;
379
380 of_landing_ctrl.omega_FB = OFL_OMEGA_FB;
381 of_landing_ctrl.omega_LR = OFL_OMEGA_LR;
382 of_landing_ctrl.active_motion = OFL_ACTIVE_MOTION;
383
384 of_landing_ctrl.front_div_threshold = OFL_FRONT_DIV_THRESHOLD;
385
386 int i;
387 if (of_landing_ctrl.use_bias) {
388 weights = (float *)calloc(n_textons + 1, sizeof(float));
389 for (i = 0; i <= n_textons; i++) {
390 weights[i] = 0.0f;
391 }
392 } else {
393 weights = (float *)calloc(n_textons, sizeof(float));
394 for (i = 0; i < n_textons; i++) {
395 weights[i] = 0.0f;
396 }
397 }
398
399
400#ifdef RLS_TEXTONS
401 // RLS:
402 P_RLS = (float **)calloc((n_textons + 1), sizeof(float *));
403 for (i = 0; i < n_textons + 1; i++) {
404 P_RLS[i] = (float *)calloc((n_textons + 1), sizeof(float));
405 }
406 int j;
407 for (i = 0; i < n_textons + 1; i++) {
408 for (j = 0; j < n_textons + 1; j++) {
409 if (i == j) {
410 P_RLS[i][j] = 1.0f;
411 } else {
412 P_RLS[i][j] = 0.0f;
413 }
414 }
415 }
416#endif
417
419
420 // Subscribe to the altitude above ground level ABI messages
421 AbiBindMsgAGL(OFL_AGL_ID, &agl_ev, vertical_ctrl_agl_cb);
422 // Subscribe to the optical flow estimator:
423 // register telemetry:
426
428
429 of_landing_ctrl.ramp_duration = 0.50f; // ramp duration in seconds
430 // ramping:
431 ramp = 0;
432 delta_setpoint = 0.0f;
433 ramp_start_time = 0.0f;
434 start_setpoint_ramp = 0.0f;
435
436 lp_flow_x = 0.0f;
437 lp_flow_y = 0.0f;
438 lp_divergence_front = 0.0f;
439
441
442 divergence_front = 0.0f;
443}
444
448static void reset_all_vars(void)
449{
450 optical_flow_x = 0.0;
451 optical_flow_y = 0.0;
452 sum_roll_error = 0.0;
453 sum_pitch_error = 0.0;
454
456
457 thrust_set = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
458
459 cov_div = 0.;
460 normalized_thrust = of_landing_ctrl.nominal_thrust * 100;
461 previous_cov_err = 0.;
466
469
470 ind_hist = 0;
472 uint32_t i;
473 for (i = 0; i < OFL_COV_WINDOW_SIZE; i++) {
474 thrust_history[i] = 0;
475 divergence_history[i] = 0;
476 }
477
478 landing = false;
479
480 elc_phase = 0;
481 elc_time_start = 0;
482 count_covdiv = 0;
483 lp_cov_div = 0.0f;
484
485 pstate = of_landing_ctrl.pgain;
486 pused = pstate;
487 istate = of_landing_ctrl.igain;
488 dstate = of_landing_ctrl.dgain;
489
490 of_landing_ctrl.load_weights = true;
491 of_landing_ctrl.divergence = 0.;
492 of_landing_ctrl.previous_err = 0.;
493 of_landing_ctrl.sum_err = 0.;
494 of_landing_ctrl.d_err = 0.;
495
496 // ramping:
497 ramp = 0;
498 delta_setpoint = 0.0f;
499 ramp_start_time = 0.0f;
500 start_setpoint_ramp = 0.0f;
501
502 lp_flow_x = 0.0f;
503 lp_flow_y = 0.0f;
504 lp_divergence_front = 0.0f;
505}
506
510void vertical_ctrl_module_run(bool in_flight)
511{
512 float div_factor; // factor that maps divergence in pixels as received from vision to 1 / frame
513
514 float dt = vision_time - prev_vision_time;
515
516 // check if new measurement received
517 if (dt <= 1e-5f) {
518 return;
519 }
520
522
523 Bound(of_landing_ctrl.lp_const, 0.001f, 1.f);
524 float lp_factor = dt / of_landing_ctrl.lp_const;
525 Bound(lp_factor, 0.f, 1.f);
526
527 if (of_landing_ctrl.load_weights) {
528 printf("LOADING WEIGHTS!\n");
529 load_weights();
530 of_landing_ctrl.load_weights = false;
531 }
532
533 if (!in_flight) {
534
535 // When not flying and in mode module:
536 // Reset integrators, landing phases, etc.
537 // reset_all_vars(); // commented out to allow us to study the observation variables in-hand, i.e., without flying
538
539
540 // SSL: only learn if not flying - due to use of resources:
541 if (of_landing_ctrl.learn_gains) {
542 printf("LEARNING WEIGHTS!\n");
543 float start = get_sys_time_float();
544 // learn the weights from the file filled with training examples:
546
547 float end = get_sys_time_float();
548 printf("Time to learn: %f\n", end - start);
549 // reset the learn_gains variable to false:
550 of_landing_ctrl.learn_gains = false;
551 // dt is smaller than it actually should be...
552 }
553 }
554
555 /***********
556 * VISION
557 ***********/
558 float new_flow_x = 0.0;
559 float new_flow_y = 0.0;
560 if (of_landing_ctrl.VISION_METHOD == 0) {
561 // SIMULATED DIVERGENCE:
562
563 // USE OPTITRACK HEIGHT
565
566 if (fabsf(of_landing_ctrl.agl - of_landing_ctrl.agl_lp) > 1.0f) {
567 // ignore outliers:
568 of_landing_ctrl.agl = of_landing_ctrl.agl_lp;
569 }
570 // calculate the new low-pass height and the velocity
571 of_landing_ctrl.agl_lp += (of_landing_ctrl.agl - of_landing_ctrl.agl_lp) * lp_factor;
572
573 // only calculate velocity and divergence if dt is large enough:
575
576 // calculate the fake divergence:
577 if (of_landing_ctrl.agl_lp > 1e-5f) {
578 of_landing_ctrl.divergence = of_landing_ctrl.vel / of_landing_ctrl.agl_lp;
579 // TODO: this time scaling should be done in optical flow module
581 if (fabsf(divergence_vision_dt) > 1e-5f) {
582 div_factor = of_landing_ctrl.divergence / divergence_vision_dt;
583 }
584 }
585 } else {
586 // USE REAL VISION OUTPUTS:
587 // TODO: this div_factor depends on the subpixel-factor (automatically adapt?)
588 // TODO: this factor is camera specific and should be implemented in the optical
589 // flow calculator module not here. Additionally, the time scaling should also
590 // be done in the calculator module
591 // div_factor = -1.28f; // magic number comprising field of view etc.
592 div_factor = -2.25f; // in the sim
593 float new_divergence = (divergence_vision * div_factor) / dt;
594 new_flow_x = optical_flow_x / dt;
595 new_flow_y = optical_flow_y / dt;
596
597 // deal with (unlikely) fast changes in divergence:
598 static const float max_div_dt = 0.20f;
599 if (fabsf(new_divergence - of_landing_ctrl.divergence) > max_div_dt) {
600 if (new_divergence < of_landing_ctrl.divergence) { new_divergence = of_landing_ctrl.divergence - max_div_dt; }
601 else { new_divergence = of_landing_ctrl.divergence + max_div_dt; }
602 }
603
604 // low-pass filter the divergence:
605 of_landing_ctrl.divergence += (new_divergence - of_landing_ctrl.divergence) * lp_factor;
607 }
608
609 /***********
610 * CONTROL
611 ***********/
612 // landing indicates whether the drone is already performing a final landing procedure (flare):
613 if (!landing) {
614
615 /*
616 // First seconds, don't do anything crazy:
617 if (module_active_time_sec < 2.5f) {
618 int32_t nominal_throttle = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
619 thrust_set = nominal_throttle;
620 stabilization.cmd[COMMAND_THRUST] = thrust;
621 // keep track of histories and set the covariance
622 set_cov_div(thrust_set);
623 cov_div = 0.0f; // however, cov div is set to 0 to prevent strange issues at module startup.
624 return;
625 }
626 */
627
628 if (of_landing_ctrl.CONTROL_METHOD == 0) {
629 // FIXED GAIN CONTROL, cov_limit for landing:
630
631 // use the divergence for control:
633 of_landing_ctrl.dgain, dt);
634
635 // trigger the landing if the cov div is too high:
636 if (fabsf(cov_div) > of_landing_ctrl.cov_limit) {
638 }
639 } else if (of_landing_ctrl.CONTROL_METHOD == 1) {
640 // ADAPTIVE GAIN CONTROL:
641 // TODO: i-gain and d-gain are currently not adapted
642
643 // adapt the gains according to the error in covariance:
644 float error_cov = of_landing_ctrl.cov_set_point - cov_div;
645 // limit the error_cov, which could else become very large:
646 if (error_cov > fabsf(of_landing_ctrl.cov_set_point)) { error_cov = fabsf(of_landing_ctrl.cov_set_point); }
647 pstate -= (of_landing_ctrl.igain_adaptive * pstate) * error_cov;
649 pused = pstate - (of_landing_ctrl.pgain_adaptive * pstate) * error_cov;
650 // make sure pused does not become too small, nor grows too fast:
652 if (of_landing_ctrl.COV_METHOD == 1 && error_cov > 0.001) {
653 pused = 0.5 * pused;
654 }
655
656 // use the divergence for control:
658 of_landing_ctrl.dgain, dt);
659
660 // SSL: if close enough, store texton inputs:
661 printf("abs error cov = %f, close = %f\n", fabs(error_cov), of_landing_ctrl.close_to_edge);
662 if (fabs(error_cov) < of_landing_ctrl.close_to_edge) {
664 }
665
666 // when to make the final landing:
667 if (pstate < of_landing_ctrl.p_land_threshold) {
669 }
670
671 } else if (of_landing_ctrl.CONTROL_METHOD == 2) {
672 // EXPONENTIAL GAIN CONTROL:
673 static const float phase_0_set_point = 0.0f;
674 if (elc_phase == 0) {
675 // increase the gain till you start oscillating:
676
677 // if not yet oscillating, increase the gains:
678 if (of_landing_ctrl.elc_oscillate && cov_div > of_landing_ctrl.cov_set_point) {
680 float gain_factor = pstate / pused;
681 istate *= gain_factor;
682 dstate *= gain_factor;
683 pused = pstate;
684 }
685
686 // use the divergence for control:
687 thrust_set = PID_divergence_control(phase_0_set_point, pused, istate, dstate, dt);
688
689 // low pass filter cov div and remove outliers:
690 if (fabsf(lp_cov_div - cov_div) < of_landing_ctrl.cov_limit) {
691 lp_cov_div = of_landing_ctrl.lp_cov_div_factor * lp_cov_div + (1 - of_landing_ctrl.lp_cov_div_factor) * cov_div;
692 }
693 // if oscillating, maintain a counter to see if it endures:
694 if (lp_cov_div <= of_landing_ctrl.cov_set_point) {
695 count_covdiv++;
696 } else {
697 count_covdiv = 0;
699 }
700 // if the drone has been oscillating long enough, start landing:
701 if (!of_landing_ctrl.elc_oscillate ||
702 (count_covdiv > 0 && (get_sys_time_float() - elc_time_start) >= of_landing_ctrl.t_transition)) {
703 // next phase:
704 elc_phase = 1;
706
707 // we don't want to oscillate, so reduce the gain:
708 elc_p_gain_start = of_landing_ctrl.reduction_factor_elc * pstate;
709 elc_i_gain_start = of_landing_ctrl.reduction_factor_elc * istate;
710 elc_d_gain_start = of_landing_ctrl.reduction_factor_elc * dstate;
711 count_covdiv = 0;
712 of_landing_ctrl.sum_err = 0.0f;
713 }
714 } else if (elc_phase == 1) {
715 // control divergence to 0 with the reduced gain:
717 pused = pstate;
720
721 float t_interval = get_sys_time_float() - elc_time_start;
722 // this should not happen, but just to be sure to prevent too high gain values:
723 if (t_interval < 0) { t_interval = 0.0f; }
724
725 // use the divergence for control:
726 thrust_set = PID_divergence_control(phase_0_set_point, pused, istate, dstate, dt);
727
728 // if we have been trying to hover stably again for 2 seconds and we move in the same way as the desired divergence, switch to landing:
729 if (t_interval >= 2.0f && of_landing_ctrl.divergence * of_landing_ctrl.divergence_setpoint >= 0.0f) {
730 // next phase:
731 elc_phase = 2;
733 count_covdiv = 0;
734 }
735 } else if (elc_phase == 2) {
736 // land while exponentially decreasing the gain:
737 float t_interval = get_sys_time_float() - elc_time_start;
738
739 // this should not happen, but just to be sure to prevent too high gain values:
740 if (t_interval < 0) { t_interval = 0.0f; }
741
742 // determine the P-gain, exponentially decaying:
743 float gain_scaling = expf(of_landing_ctrl.divergence_setpoint * t_interval);
744 pstate = elc_p_gain_start * gain_scaling;
745 istate = elc_i_gain_start * gain_scaling;
746 dstate = elc_d_gain_start * gain_scaling;
747 pused = pstate;
748
749 // 2 [1/s] ramp to setpoint
750 /*if (fabsf(of_landing_ctrl.divergence_setpoint - divergence_setpoint) > 2.*dt){
751 divergence_setpoint += 2*dt * of_landing_ctrl.divergence_setpoint / fabsf(of_landing_ctrl.divergence_setpoint);
752 } else {
753 divergence_setpoint = of_landing_ctrl.divergence_setpoint;
754 }*/
755
756 // use the divergence for control:
758
759 // when to make the final landing:
760 if (pstate < of_landing_ctrl.p_land_threshold) {
761 elc_phase = 3;
762 }
763 } else {
765 }
766 } else if (of_landing_ctrl.CONTROL_METHOD == 3) {
767
768 // SSL LANDING: use learned weights for setting the gain on the way down:
769
770 // adapt the p-gain with a low-pass filter to the gain predicted by image appearance:
772 // low-pass filtered, downscaled pstate predictions:
773 // TODO: just as with divergence, also take dt into account for low-pass filtering:
774 of_landing_ctrl.pgain = of_landing_ctrl.lp_factor_prediction * of_landing_ctrl.pgain +
775 (1.0f - of_landing_ctrl.lp_factor_prediction) * of_landing_ctrl.reduction_factor_elc * pstate;
776 pused = of_landing_ctrl.pgain;
777 // make sure pused does not become too small, nor grows too fast:
779 // have the i and d gain depend on the p gain:
780 istate = 0.025 * of_landing_ctrl.pgain;
781 dstate = 0.0f;
782 printf("of_landing_ctrl.pgain = %f, divergence = %f, phase = %d\n", of_landing_ctrl.pgain, of_landing_ctrl.divergence,
783 elc_phase);
784
785 if (elc_phase == 0) {
786 // Let the low-pass filter settle first with 0 divergence:
787 float phase_0_set_point = 0.0f;
789
790 // use the divergence for control:
791 thrust_set = PID_divergence_control(phase_0_set_point, pused, istate, dstate, dt);
792
793 // if the low-pass filter of the p-gain has settled and the drone is moving in the right direction:
794 if (module_active_time_sec >= 3.0f && of_landing_ctrl.divergence * of_landing_ctrl.divergence_setpoint >= 0.0f) {
795 // next phase:
796 elc_phase = 1;
797 }
798 } else {
799 // use the chosen divergence setpoint and the measured divergence for control:
801 }
802 if (pused < of_landing_ctrl.p_land_threshold) {
804 }
805 } else if (of_landing_ctrl.CONTROL_METHOD == 4) {
806
807 // Mixed SSL + EXPONENTIAL LANDING: use learned weights for setting the gain on the way down:
808
809 if (elc_phase == 0) {
810
811 float phase_0_set_point = 0.0f;
813 // adapt the p-gain with a low-pass filter to the gain predicted by image appearance:
815 // of_landing_ctrl.pgain = lp_factor_prediction * of_landing_ctrl.pgain + (1.0f - lp_factor_prediction) * of_landing_ctrl.stable_gain_factor * pstate;
816 of_landing_ctrl.pgain = of_landing_ctrl.lp_factor_prediction * of_landing_ctrl.pgain +
817 (1.0f - of_landing_ctrl.lp_factor_prediction) * of_landing_ctrl.reduction_factor_elc * pstate;
818 pused = of_landing_ctrl.pgain;
819 // make sure pused does not become too small, nor grows too fast:
821 // have the i and d gain depend on the p gain:
822 istate = 0.025 * of_landing_ctrl.pgain;
823 dstate = 0.0f;
824 printf("of_landing_ctrl.pgain = %f, divergence = %f\n", of_landing_ctrl.pgain, of_landing_ctrl.divergence);
825
826 // use the divergence for control:
827 thrust_set = PID_divergence_control(phase_0_set_point, pused, istate, dstate, dt);
828
829 // if the low-pass filter of the p-gain has settled and the drone is moving in the right direction:
830 if (module_active_time_sec >= 3.0f && of_landing_ctrl.divergence * of_landing_ctrl.divergence_setpoint >= 0.0f) {
831 // next phase:
832 elc_phase = 1;
834
835 // we don't have to reduce the gain with of_landing_ctrl.reduction_factor_elc, as this is done above for the p-gain already:
839 count_covdiv = 0;
840 of_landing_ctrl.sum_err = 0.0f;
841 }
842 } else if (elc_phase == 1) {
843 // land while exponentially decreasing the gain:
844 float new_time = get_sys_time_float();
845 float t_interval = new_time - elc_time_start;
846
847 // this should not happen, but just to be sure to prevent too high gain values:
848 if (t_interval < 0) { t_interval = 0.0f; }
849
850 // determine the P-gain, weighing between an exponentially decaying one and the predicted one based on textons:
851 float gain_scaling = exp(of_landing_ctrl.divergence_setpoint * t_interval);
852 float prediction = predict_gain(texton_distribution); // low-pass it in of_landing_ctrl.pgain?
853 float weight_prediction = 0.5;
854
855 if (gain_scaling <= 1.0f) {
856 pstate = weight_prediction * of_landing_ctrl.reduction_factor_elc * prediction + (1 - weight_prediction) *
857 elc_p_gain_start * gain_scaling;
858 istate = 0.025 * weight_prediction * of_landing_ctrl.reduction_factor_elc * prediction +
859 (1 - weight_prediction) * elc_i_gain_start * gain_scaling;
860 dstate = 0.0f;
861 } else {
862 // should never come here:
863 pstate = of_landing_ctrl.reduction_factor_elc * prediction;
864 istate = 0.025 * of_landing_ctrl.reduction_factor_elc * prediction;
865 dstate = 0.0f;
866 }
867 pused = pstate;
868
869 // use the divergence for control:
871
872 // when to make the final landing:
873 if (pstate < of_landing_ctrl.p_land_threshold) {
874 elc_phase = 2;
875 }
876 } else {
878 }
879 } else if (of_landing_ctrl.CONTROL_METHOD == 5) {
880
881 // SSL raw, no landing procedure, ideal for hovering:
882
884 // printf("prediction = %f\n", pstate);
885 of_landing_ctrl.pgain = of_landing_ctrl.lp_factor_prediction * of_landing_ctrl.pgain +
886 (1.0f - of_landing_ctrl.lp_factor_prediction) * of_landing_ctrl.reduction_factor_elc * pstate;
887 pused = of_landing_ctrl.pgain;
888 // make sure pused does not become too small, nor grows too fast:
890 // have the i and d gain depend on the p gain:
891 istate = 0.025 * of_landing_ctrl.pgain;
892 dstate = 0.0f;
893 printf("of_landing_ctrl.pgain = %f\n", of_landing_ctrl.pgain);
894
895 // use the divergence for control:
897
898 if (pstate < of_landing_ctrl.p_land_threshold) {
900 }
901 }
902
903 if (in_flight) {
904 Bound(thrust_set, 0.25 * of_landing_ctrl.nominal_thrust * MAX_PPRZ, MAX_PPRZ);
905 }
906 }
907
908 /*********************/
909 // Horizontal control:
910 /*********************/
911
913
914 // negative command is flying forward, positive back.
915 lp_flow_x = of_landing_ctrl.lp_factor_prediction * lp_flow_x + (1.0f - of_landing_ctrl.lp_factor_prediction) *
917 lp_flow_y = of_landing_ctrl.lp_factor_prediction * lp_flow_y + (1.0f - of_landing_ctrl.lp_factor_prediction) *
919 lp_divergence_front = of_landing_ctrl.lp_factor_prediction * lp_divergence_front + (1.0f - of_landing_ctrl.lp_factor_prediction) *
921
922
923
924 if(lp_divergence_front > of_landing_ctrl.front_div_threshold) {
925 // Stop moving in the longitudinal direction:
926 of_landing_ctrl.omega_FB = 0.0f;
927 }
928
929 if (of_landing_ctrl.active_motion == 1) {
930 // Active motion through varying ventral flow commands
931 float period_factor = 0.2;
932 float sine_threshold = 0.75;
933 float omega_set_point = 20;
934
935 if (sinf(period_factor * module_active_time_sec) > sine_threshold) {
936 of_landing_ctrl.omega_LR = omega_set_point;
937 } else if (sinf(period_factor * module_active_time_sec) < -sine_threshold) {
938 of_landing_ctrl.omega_LR = -omega_set_point;
939 } else {
940 of_landing_ctrl.omega_LR = 0.0f;
941 }
942 } else if (of_landing_ctrl.active_motion == 2) {
943 // Active motion through varying roll trim commands
944 float period_factor = 0.2;
945 float sine_threshold = 0.9;
946 float trim_set_point = 2.0f;
947
948 if (sinf(period_factor * module_active_time_sec) > sine_threshold) {
949 of_landing_ctrl.roll_trim = trim_set_point;
950 } else if (sinf(period_factor * module_active_time_sec) < -sine_threshold) {
951 of_landing_ctrl.roll_trim = -trim_set_point;
952 } else {
953 of_landing_ctrl.roll_trim = 0.0f;
954 }
955 }
956
957 float error_pitch = new_flow_y - of_landing_ctrl.omega_FB;
958 float error_roll = new_flow_x - of_landing_ctrl.omega_LR;
959 sum_pitch_error += error_pitch;
960 sum_roll_error += error_roll;
961 float P_hor = of_landing_ctrl.pgain_horizontal_factor * of_landing_ctrl.pgain;
962 float I_hor = of_landing_ctrl.igain_horizontal_factor * of_landing_ctrl.igain;
963 float pitch_cmd;
964 float roll_cmd;
965 float add_active_sine = 0.0f;
966
967 if (ACTIVE_RATES) {
968 float sine_period = 0.05;
969 float sine_amplitude = RadOfDeg(1.0);
970 float sine_time = get_sys_time_float();
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);
973 }
975 // normal operation:
976 pitch_cmd = RadOfDeg(of_landing_ctrl.pitch_trim + error_pitch * P_hor + I_hor * sum_pitch_error);
977 // add_active_sine = 0.0f in normal operation:
978 roll_cmd = RadOfDeg(of_landing_ctrl.roll_trim + error_roll * P_hor + I_hor * sum_roll_error) + add_active_sine;
979 }
980 BoundAbs(pitch_cmd, RadOfDeg(10.0));
981 BoundAbs(roll_cmd, RadOfDeg(10.0));
982 float psi_cmd =
983 attitude->psi; // control of psi in simulation is still a bit enthusiastic! Increase the associated control effectiveness.
984 struct Int32Eulers rpy = { .phi = (int32_t)ANGLE_BFP_OF_REAL(roll_cmd),
985 .theta = (int32_t)ANGLE_BFP_OF_REAL(pitch_cmd), .psi = (int32_t)ANGLE_BFP_OF_REAL(psi_cmd)
986 };
987
988 // set the desired roll pitch and yaw:
991 // execute attitude stabilization:
992 stabilization_attitude_run(in_flight, &sp, &th, stabilization.cmd);
993}
994
999{
1000 // land with 85% nominal thrust:
1001 uint32_t nominal_throttle = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
1002 uint32_t thrust = 0.85 * nominal_throttle;
1003 Bound(thrust, 0.6 * nominal_throttle, 0.9 * MAX_PPRZ);
1004 landing = true;
1005
1006 return thrust;
1007}
1008
1015{
1016 // histories and cov detection:
1018
1019 normalized_thrust = (float)(thrust / (MAX_PPRZ / 100));
1021
1022 int ind_past = ind_hist - of_landing_ctrl.delay_steps;
1023 while (ind_past < 0) { ind_past += of_landing_ctrl.window_size; }
1025
1026 // determine the covariance for landing detection:
1027 // only take covariance into account if there are enough samples in the histories:
1028 if (of_landing_ctrl.COV_METHOD == 0 && cov_array_filled > 0) {
1029 // TODO: step in landing set point causes an incorrectly perceived covariance
1031 } else if (of_landing_ctrl.COV_METHOD == 1 && cov_array_filled > 1) {
1032 // todo: delay steps should be invariant to the run frequency
1034 }
1035
1036 if (cov_array_filled < 2 && ind_hist + 1 == of_landing_ctrl.window_size) {
1038 }
1039 ind_hist = (ind_hist + 1) % of_landing_ctrl.window_size;
1040}
1041
1051int32_t PID_divergence_control(float setpoint, float P, float I, float D, float dt)
1052{
1053
1054 if (previous_divergence_setpoint != setpoint) {
1055 ramp = 1;
1059 }
1060
1061 // determine the error, possibly using a ramp:
1062 float err;
1063 if (!ramp) {
1064 err = setpoint - of_landing_ctrl.divergence;
1065 } else {
1066 // rt is in [0,1] and used to make the ramp:
1067 float ramp_time = get_sys_time_float();
1068 float rt = (ramp_time - ramp_start_time) / (of_landing_ctrl.ramp_duration);
1069
1070 if (rt > 1.0f) {
1071 ramp = 0; // end of the ramp
1072 err = setpoint - of_landing_ctrl.divergence;
1073 // Send the setpoint back via the divergence message, so that we can see that this procedure works
1074 divergence_vision_dt = setpoint;
1075 } else {
1076 float ds = start_setpoint_ramp + rt * delta_setpoint;
1077 // Send the setpoint back via the divergence message, so that we can see that this procedure works
1079 // determine the error:
1080 err = ds - of_landing_ctrl.divergence;
1081 }
1082 }
1083
1084 // update the controller errors:
1085 update_errors(err, dt);
1086
1087 // PID control:
1088 int32_t thrust = (of_landing_ctrl.nominal_thrust
1089 + P * err
1090 + I * of_landing_ctrl.sum_err
1091 + D * of_landing_ctrl.d_err) * MAX_PPRZ;
1092
1093 // bound thrust:
1094 Bound(thrust, 0.25 * of_landing_ctrl.nominal_thrust * MAX_PPRZ, MAX_PPRZ);
1095
1096 // update covariance
1097 set_cov_div(thrust);
1098
1100
1101 return thrust;
1102}
1103
1109void update_errors(float err, float dt)
1110{
1111 float lp_factor = dt / of_landing_ctrl.lp_const;
1112 Bound(lp_factor, 0.f, 1.f);
1113
1114 // maintain the controller errors:
1115 of_landing_ctrl.sum_err += err;
1116 of_landing_ctrl.d_err += (((err - of_landing_ctrl.previous_err) / dt) - of_landing_ctrl.d_err) * lp_factor;
1117 of_landing_ctrl.previous_err = err;
1118}
1119
1120// Reading from "sensors":
1121void vertical_ctrl_agl_cb(uint8_t sender_id UNUSED, __attribute__((unused)) uint32_t stamp, float distance)
1122{
1123 of_landing_ctrl.agl = distance;
1124}
1125
1127 int32_t flow_x UNUSED, int32_t flow_y UNUSED,
1128 int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED,
1129 float quality UNUSED, float size_divergence)
1130{
1131
1132 if (sender_id == FLOW_OPTICFLOW_ID + 0) { // 0 = bottom 1 = front
1133 optical_flow_x = ((float)flow_der_x) / 10.0;
1134 optical_flow_y = ((float)flow_der_y) / 10.0;
1135 divergence_vision = size_divergence;
1136 //printf("Reading %f, %f, %f\n", optical_flow_x, optical_flow_y, divergence_vision);
1137 vision_time = ((float)stamp) / 1e6;
1138
1139
1140 // checking fps and newness of images:
1141 /*float new_flow_time = get_sys_time_float();
1142 float dt = (new_flow_time - old_flow_time);
1143 if (dt > 0) {
1144 float fps_flow = 1.0f / dt;
1145 printf("FPS flow bottom cam in OF landing = %f, optical_flow_x = %f\n", fps_flow, optical_flow_x);
1146 }
1147 old_flow_time = new_flow_time; */
1148 }
1149 else {
1150
1151 float new_flow_time = get_sys_time_float();
1152 float dt_flow_front = new_flow_time - old_flow_time;
1153 if (dt_flow_front > 0) {
1154 //float fps_flow = 1.0f / dt_flow_front;
1155 //printf("FPS flow front cam in OF landing = %f\n", fps_flow);
1156 old_flow_time = new_flow_time;
1157
1158 divergence_front = size_divergence / dt_flow_front;
1159 }
1160 }
1161}
1162
1164// Call our controller
1165
1170{
1172
1173 // adaptive estimation - assume hover condition when entering the module
1174 of_landing_ctrl.nominal_thrust = (float) stabilization.cmd[COMMAND_THRUST] / MAX_PPRZ;
1175 thrust_set = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
1176}
1177
1178void guidance_module_run(bool in_flight)
1179{
1180 vertical_ctrl_module_run(in_flight);
1181
1182 // FIXME horizontal guidance ?
1183}
1184
1185// SSL:
1187{
1188 printf("Logging textons!\n");
1189 int i;
1190
1191 // If not the same, append the target values (heights, gains) and texton values to a .dat file:
1192 char filename[512];
1193 sprintf(filename, "%s/Training_set_%05d.dat", STRINGIFY(TEXTON_DISTRIBUTION_PATH), 0);
1194 distribution_logger = fopen(filename, "a");
1195 if (distribution_logger == NULL) {
1196 perror(filename);
1197 //perror("Error while opening the file.\n");
1198 } else {
1199 printf("Logging at height %f, gain %f, cov_div %f\n", of_landing_ctrl.agl, pstate, cov_div);
1200
1201 // save the information in a single row:
1202 fprintf(distribution_logger, "%f ", of_landing_ctrl.agl); // sonar measurement
1203 fprintf(distribution_logger, "%f ", pstate); // gain measurement
1204 fprintf(distribution_logger, "%f ", cov_div); // cov div measurement
1205 for (i = 0; i < n_textons - 1; i++) {
1206 fprintf(distribution_logger, "%f ", texton_distribution[i]);
1207 }
1208 fprintf(distribution_logger, "%f\n", texton_distribution[n_textons - 1]);
1209 fclose(distribution_logger);
1210 }
1211}
1212
1214{
1215 int i, j, read_result;
1216 char filename[512];
1217
1218 sprintf(filename, "%s/Training_set_%05d.dat", STRINGIFY(TEXTON_DISTRIBUTION_PATH), 0);
1219 printf("Loading textons from %s\n", filename);
1220
1221 distribution_logger = fopen(filename, "r");
1222 if (distribution_logger) {
1223 // Load the dictionary:
1224 n_read_samples = 0;
1225 // For now we read the samples sequentially:
1226 for (i = 0; i < MAX_SAMPLES_LEARNING; i++)
1227 //for(i = 0; i < 30; i++)
1228 {
1229 read_result = fscanf(distribution_logger, "%f ", &sonar_OF[n_read_samples]);
1230 if (read_result == EOF) { break; }
1231 read_result = fscanf(distribution_logger, "%f ", &gains[n_read_samples]);
1232 if (read_result == EOF) { break; }
1233 read_result = fscanf(distribution_logger, "%f ", &cov_divs_log[n_read_samples]);
1234 if (read_result == EOF) { break; }
1235
1236 text_dists[n_read_samples] = (float *) calloc(n_textons, sizeof(float));
1237 for (j = 0; j < n_textons - 1; j++) {
1238 read_result = fscanf(distribution_logger, "%f ", &text_dists[n_read_samples][j]);
1239 if (read_result == EOF) { break; }
1240 }
1241 read_result = fscanf(distribution_logger, "%f\n", &text_dists[n_read_samples][n_textons - 1]);
1242 if (read_result == EOF) { break; }
1244 }
1245 fclose(distribution_logger);
1246 } else {
1247 printf("There was an error opening %s\n", filename);
1248 }
1249}
1250
1252{
1253 int i;
1254 float fit_error;
1255
1256 // first load the texton distributions:
1257 printf("Loading distribution\n");
1259
1260 // then learn from it:
1261 printf("Learning!\n");
1262 if (!RECURSIVE_LEARNING) {
1264 // fit_linear_model_OF(sonar_OF, text_dists, n_textons, n_read_samples, weights, &fit_error);
1265 } else {
1267 }
1268
1269 printf("Saving!\n");
1270 // save the weights to a file:
1271 save_weights();
1272
1273 printf("Learned! Fit error = %f\n", fit_error);
1274
1275 // free learning distributions:
1276 for (i = 0; i < (int) n_read_samples; i++) {
1277 free(text_dists[i]);
1278 }
1279}
1280
1290void recursive_least_squares_batch(float *targets, float **samples, uint8_t D, uint16_t count, float *params,
1291 float *fit_error)
1292{
1293 // TODO: potentially randomizing the sequence of the samples, as not to get a bias towards the later samples
1294 // local vars for iterating, random numbers:
1295 int sam, d;
1296 uint8_t D_1 = D + 1;
1297 float augmented_sample[D_1];
1298 // augmented sample is used if the bias is used:
1299 augmented_sample[D] = 1.0f;
1300
1301 // Initialize the weights with 0.0f:
1302 for (d = 0; d < D_1; d++) {
1303 weights[d] = 0.0f;
1304 }
1305
1306 // Reset the P matrix to an identity matrix:
1307 int i, j;
1308 for (i = 0; i < n_textons + 1; i++) {
1309 for (j = 0; j < n_textons + 1; j++) {
1310 if (i == j) {
1311 P_RLS[i][j] = 1.0f;
1312 } else {
1313 P_RLS[i][j] = 0.0f;
1314 }
1315 }
1316 }
1317
1318 // give the samples one by one to the recursive procedure:
1319 for (sam = 0; sam < count; sam++) {
1320 if (!of_landing_ctrl.use_bias) {
1321 recursive_least_squares(targets[sam], samples[sam], D, params);
1322 } else {
1323 for (d = 0; d < D; d++) {
1324 augmented_sample[d] = samples[sam][d];
1325 }
1326 recursive_least_squares(targets[sam], augmented_sample, D_1, params);
1327 }
1328 }
1329
1330 // give the samples one by one to the recursive procedure to determine the fit on the training set:
1331 float prediction;
1332 float sum_abs_err = 0;
1333 for (sam = 0; sam < count; sam++) {
1334 prediction = predict_gain(samples[sam]);
1335 sum_abs_err += fabs(prediction - targets[sam]);
1336 }
1337 (*fit_error) = sum_abs_err / count;
1338}
1339
1340void recursive_least_squares(float target, float *sample, uint8_t length_sample, UNUSED float *params)
1341{
1342 // MATLAB procedure:
1343 /*
1344 u = features(i,:);
1345 phi = u * P ;
1346 k(:, i) = phi' / (lamda + phi * u' );
1347 y(i)= weights(:,i)' * u';
1348 e(i) = targets(i) - y(i) ;
1349 weights(:,i+1) = weights(:,i) + k(:, i) * e(i) ;
1350 P = ( P - k(:, i) * phi ) / lamda;
1351 */
1352
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];
1361 float scalar;
1362 float _O[length_sample][length_sample];
1363 int i;
1364
1365 // u = features(i,:);
1366 for (i = 0; i < length_sample; i++) {
1367 _u[0][i] = sample[i];
1368 _uT[i][0] = sample[i];
1369 }
1370 MAKE_MATRIX_PTR(u, _u, 1);
1371 MAKE_MATRIX_PTR(uT, _uT, length_sample); // transpose
1372 MAKE_MATRIX_PTR(phi, _phi, 1);
1373 MAKE_MATRIX_PTR(phiT, _phiT, length_sample);
1374 MAKE_MATRIX_PTR(u_P_uT, _u_P_uT, 1); // actually a scalar
1375 MAKE_MATRIX_PTR(k, _k, length_sample);
1376 MAKE_MATRIX_PTR(ke, _ke, length_sample);
1377
1378 MAKE_MATRIX_PTR(P, P_RLS, length_sample);
1379 MAKE_MATRIX_PTR(O, _O, length_sample);
1380 // phi = u * P ;
1381 // result of multiplication goes into phi
1382 MAT_MUL(1, length_sample, length_sample, phi, u, P);
1383 // k(:, i) = phi' / (lamda + phi * u' );
1384 for (i = 0; i < length_sample; i++) {
1385 phiT[i][0] = phi[0][i];
1386 }
1387 // scalar:
1388 MAT_MUL(1, length_sample, 1, u_P_uT, phi, uT);
1389 scalar = u_P_uT[0][0];
1390 float_mat_div_scalar(k, phiT, lambda + scalar, length_sample, 1);
1391 // y(i)= weights(:,i)' * u';
1392 prediction = predict_gain(sample);
1393 // e(i) = targets(i) - y(i) ;
1394 error = target - prediction;
1395 // weights(:,i+1) = weights(:,i) + k(:, i) * e(i) ;
1396 float_mat_mul_scalar(ke, k, error, length_sample, 1);
1397 for (i = 0; i < length_sample; i++) {
1398 weights[i] += ke[i][0];
1399 }
1400 // P = ( P - k(:, i) * phi ) / lamda;
1401 MAT_MUL(length_sample, 1, length_sample, O, k, phi);
1402 MAT_SUB(length_sample, length_sample, P, P, O);
1403 float_mat_div_scalar(P, P, lambda, length_sample, length_sample);
1404}
1405
1415void fit_linear_model_OF(float *targets, float **samples, uint8_t D, uint16_t count, float *params, float *fit_error)
1416{
1417
1418 // We will solve systems of the form A x = b,
1419 // where A = [nx(D+1)] matrix with entries [s1, ..., sD, 1] for each sample (1 is the bias)
1420 // and b = [nx1] vector with the target values.
1421 // x in the system are the parameters for the linear regression function.
1422
1423 // local vars for iterating, random numbers:
1424 int sam, d;
1425 uint16_t n_samples = count;
1426 uint8_t D_1 = D + 1;
1427 // ensure that n_samples is high enough to ensure a result for a single fit:
1428 n_samples = (n_samples < D_1) ? D_1 : n_samples;
1429 // n_samples should not be higher than count:
1430 n_samples = (n_samples < count) ? n_samples : count;
1431
1432 // initialize matrices and vectors for the full point set problem:
1433 // this is used for determining inliers
1434 float _AA[count][D_1];
1435 MAKE_MATRIX_PTR(AA, _AA, count);
1436 float _targets_all[count][1];
1437 MAKE_MATRIX_PTR(targets_all, _targets_all, count);
1438
1439 for (sam = 0; sam < count; sam++) {
1440 for (d = 0; d < D; d++) {
1441 AA[sam][d] = samples[sam][d];
1442 }
1443 if (of_landing_ctrl.use_bias) {
1444 AA[sam][D] = 1.0f;
1445 } else {
1446 AA[sam][D] = 0.0f;
1447 }
1448 targets_all[sam][0] = targets[sam];
1449 }
1450
1451
1452 // decompose A in u, w, v with singular value decomposition A = u * w * vT.
1453 // u replaces A as output:
1454 float _parameters[D_1][1];
1455 MAKE_MATRIX_PTR(parameters, _parameters, D_1);
1456 float w[n_samples], _v[D_1][D_1];
1457 MAKE_MATRIX_PTR(v, _v, D_1);
1458
1459 pprz_svd_float(AA, w, v, count, D_1);
1460 pprz_svd_solve_float(parameters, AA, w, v, targets_all, count, D_1, 1);
1461
1462 // used to determine the error of a set of parameters on the whole set:
1463 float _bb[count][1];
1464 MAKE_MATRIX_PTR(bb, _bb, count);
1465 float _C[count][1];
1466 MAKE_MATRIX_PTR(C, _C, count);
1467
1468 // error is determined on the entire set
1469 // bb = AA * parameters:
1470 MAT_MUL(count, D_1, 1, bb, AA, parameters);
1471 // subtract bu_all: C = 0 in case of perfect fit:
1472 MAT_SUB(count, 1, C, bb, targets_all);
1473 *fit_error = 0;
1474 for (sam = 0; sam < count; sam++) {
1475 *fit_error += fabs(C[sam][0]);
1476 }
1477 *fit_error /= count;
1478
1479 for (d = 0; d < D_1; d++) {
1480 params[d] = parameters[d][0];
1481 }
1482
1483}
1484
1485
1486float predict_gain(float *distribution)
1487{
1488 //printf("Start prediction!\n");
1489 int i;
1490 float sum;
1491
1492 sum = 0.0f;
1493 for (i = 0; i < n_textons; i++) {
1494 sum += weights[i] * distribution[i];
1495 }
1496 if (of_landing_ctrl.use_bias) {
1497 sum += weights[n_textons];
1498 }
1499 // printf("Prediction = %f\n", sum);
1500 return sum;
1501}
1502
1504{
1505 // save the weights to a file:
1506 int i;
1507 char filename[512];
1508 sprintf(filename, "%s/Weights_%05d.dat", STRINGIFY(TEXTON_DISTRIBUTION_PATH), 0);
1509 weights_file = fopen(filename, "w");
1510 if (weights_file == NULL) {
1511 perror(filename);
1512 } else {
1513 // save the information in a single row:
1514 for (i = 0; i <= n_textons; i++) {
1515 fprintf(weights_file, "%f ", weights[i]);
1516 }
1517 fclose(weights_file);
1518 }
1519}
1520
1522{
1523 int i, read_result;
1524 char filename[512];
1525 sprintf(filename, "%s/Weights_%05d.dat", STRINGIFY(TEXTON_DISTRIBUTION_PATH), 0);
1526 weights_file = fopen(filename, "r");
1527 if (weights_file == NULL) {
1528 printf("No weights file!\n");
1529 perror(filename);
1530 } else {
1531 // load the weights, stored in a single row:
1532 for (i = 0; i <= n_textons; i++) {
1533 read_result = fscanf(weights_file, "%f ", &weights[i]);
1534 if (read_result == EOF) { break; }
1535 }
1536 fclose(weights_file);
1537 }
1538}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define FLOW_OPTICFLOW_ID
Core autopilot interface common to all firmwares.
#define attitude
#define UNUSED(x)
Common flight_plan functions shared between fixedwing and rotorcraft.
int n_samples
Definition detect_gate.c:85
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.
euler angles
#define ANGLE_BFP_OF_REAL(_af)
euler angles
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1069
Adaptation block of the vertical guidance.
float lp_factor
Definition ins_flow.c:182
float parameters[22]
Definition ins_flow.c:249
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static abi_event optical_flow_ev
float divergence_vision
float vision_time
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
float module_enter_time
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_COV_METHOD
#define OFL_IGAIN
#define OFL_IGAIN_HORIZONTAL_FACTOR
bool landing
#define n_ts
float elc_d_gain_start
float previous_cov_err
float * weights
float normalized_thrust
float optical_flow_y
float * text_dists[MAX_SAMPLES_LEARNING]
#define OFL_ROLL_TRIM
float lp_divergence_front
float ** P_RLS
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_OMEGA_LR
#define OFL_COV_SETPOINT
float ramp_start_time
int32_t thrust_set
#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 lp_flow_x
uint32_t ind_hist
float optical_flow_x
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_DGAIN
float elc_i_gain_start
#define OFL_OPTICAL_FLOW_ID
#define INCREASE_GAIN_PER_SECOND
float istate
#define OFL_CLOSE_TO_EDGE
uint8_t cov_array_filled
void optical_flow_landing_init(void)
Initialize the optical flow landing module.
#define OFL_CONTROL_METHOD
int32_t elc_phase
#define OFL_VISION_METHOD
float dstate
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)
float prev_vision_time
float pused
float lp_cov_div
void guidance_module_run(bool in_flight)
float elc_p_gain_start
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 lp_flow_y
float delta_setpoint
float past_divergence_history[OFL_COV_WINDOW_SIZE]
float sum_roll_error
#define OFL_LP_CONST
float pstate
#define OFL_OMEGA_FB
#define OFL_DGAIN_ADAPTIVE
#define OFL_COV_WINDOW_SIZE
float divergence_front
#define OFL_PGAIN
#define MINIMUM_GAIN
unsigned int n_read_samples
float cov_divs_log[MAX_SAMPLES_LEARNING]
float lambda
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]
float old_flow_time
struct OpticalFlowLanding of_landing_ctrl
#define OFL_PITCH_TRIM
void load_texton_distribution(void)
void load_weights(void)
float divergence_history[OFL_COV_WINDOW_SIZE]
#define OFL_COV_LANDING_LIMIT
void save_weights(void)
int32_t count_covdiv
float gains[MAX_SAMPLES_LEARNING]
#define OFL_AGL_ID
float sum_pitch_error
#define OFL_ACTIVE_MOTION
float previous_divergence_setpoint
float module_active_time_sec
uint32_t elc_time_start
float cov_div
#define OFL_IGAIN_ADAPTIVE
#define ACTIVE_RATES
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
#define MAX_PPRZ
Definition paparazzi.h:8
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
float z
in meters
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 ...
Definition pprz_stat.c:152
Statistics functions.
struct VerticalGuidance guidance_v
Definition guidance_v.c:60
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.
#define THRUST_AXIS_Z
void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
Stabilization setpoint.
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.
Definition sys_time.h:241
struct target_t target
Definition target_pos.c:65
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
float * texton_distribution
Definition textons.c:42
uint8_t n_textons
Definition textons.c:122
Takes an image and represents the texture and colors in the image with a texton histogram.
static float D
static float P[3][3]
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.