diff --git a/conf/modules/dw1000_arduino.xml b/conf/modules/dw1000_arduino.xml index 7d036ee34b..4e5893bfc6 100644 --- a/conf/modules/dw1000_arduino.xml +++ b/conf/modules/dw1000_arduino.xml @@ -8,6 +8,8 @@ Decawave DW1000 modules (http://www.decawave.com/products/dwm1000-module) are Ultra-Wide-Band devices that can be used for communication and ranging. Especially, using 3 modules as anchors can provide data for a localization system based on trilateration. The DW1000 is using a SPI connection, but an arduino-compatible board can be used with the library https://github.com/thotro/arduino-dw1000 to hyde the low level drivers and provide direct ranging informations. + + See https://hal-enac.archives-ouvertes.fr/hal-01936955 for more information on the EKF filtering. diff --git a/conf/modules/shift_tracking.xml b/conf/modules/shift_tracking.xml index 45b2effffd..dd362fce5e 100644 --- a/conf/modules/shift_tracking.xml +++ b/conf/modules/shift_tracking.xml @@ -10,6 +10,8 @@ with precision or in a net. Data are coming from POSITION_ESTIMATE ABI message. + + See https://hal-enac.archives-ouvertes.fr/hal-01936955 for more information on UAV landing in a net.
diff --git a/sw/airborne/filters/pid.h b/sw/airborne/filters/pid.h index 0e97529ac8..3d72f28633 100644 --- a/sw/airborne/filters/pid.h +++ b/sw/airborne/filters/pid.h @@ -117,6 +117,27 @@ static inline void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float pid->g[2] = Ki; } +/** Set integral part, can be used to reset. + * The new sum of errors is calculated from current gains and bounds. + * + * @param pid pointer to PID structure + * @param value integral part of the PID control, 0. will reset it + */ +static inline void set_integral_pid_f(struct PID_f *pid, float value) +{ + float integral = value; + if (integral < -pid->max_sum) { + integral = -pid->max_sum; + } else if (integral > pid->max_sum) { + integral = pid->max_sum; + } + if (fabsf(pid->g[2]) < 1e-6) { + pid->sum = 0.f; // integral gain is too low, prevent division by zero, just reset sum + } else { + pid->sum = integral / pid->g[2]; + } +} + /** Distcrete time PID structure.