fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 19:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix format

Browse Source
This commit is contained in:
JaeyoungLim
2023-11-15 10:24:54 +01:00
parent 39cc1f7893
commit 1b39d5795a
6 changed files with 1840 additions and 1716 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+1197 -1120
View File
File diff suppressed because it is too large Load Diff
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+47 -34
View File
@@ -1,5 +1,5 @@
/**
* Implementation of a generic incremental position controller
* Implementation of a generic incremental position controller
* using incremental nonlinear dynamic inversion and differential flatness
* for a fixed wing aircraft during dynamic soaring cycles.
* The controller directly outputs actuator deflections for ailerons, elevator and rudder.
@@ -115,23 +115,23 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
// Subscriptions
// Subscriptions
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // vehicle status
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed
uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)}; // angle of attack
uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)}; // angle of sideslip
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed
uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)}; // angle of attack
uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)}; // angle of sideslip
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; // linear acceleration
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // local NED position
uORB::Subscription _home_position_sub{ORB_ID(home_position)}; // home position
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // local NED position
uORB::Subscription _home_position_sub{ORB_ID(home_position)}; // home position
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel
uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)}; // vehicle status flags
uORB::Subscription _soaring_estimator_shear_sub{ORB_ID(soaring_estimator_shear)}; // shear params for trajectory selection
uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)};
// Publishers
// Publishers
uORB::Publication<actuator_controls_s> _actuators_0_pub;
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_rates_setpoint_s> _angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)};
@@ -144,7 +144,7 @@ private:
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
uORB::Publication<debug_value_s> _debug_value_pub{ORB_ID(debug_value)};
// Message structs
// Message structs
vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
actuator_controls_s _actuators {}; // actuator commands
manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data
@@ -164,7 +164,7 @@ private:
soaring_controller_status_s _soaring_controller_status {}; ///< soaring controller status
soaring_controller_heartbeat_s _soaring_controller_heartbeat{}; ///< soaring controller hrt
soaring_controller_position_setpoint_s _soaring_controller_position_setpoint{}; ///< soaring controller pos setpoint
soaring_controller_position_s _soaring_controller_position{}; ///< soaring controller pos
soaring_controller_position_s _soaring_controller_position{}; ///< soaring controller pos
soaring_controller_wind_s _soaring_controller_wind{}; ///< soaring controller wind
soaring_estimator_shear_s _soaring_estimator_shear{}; ///< soaring estimator shear
debug_value_s _debug_value{}; // slip angle
@@ -245,7 +245,7 @@ private:
uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position
uint8_t _alt_reset_counter{0}; ///< captures the number of times the estimator has reset the altitude state
// Update our local parameter cache.
int parameters_update();
@@ -261,7 +261,7 @@ private:
void vehicle_angular_velocity_poll();
void vehicle_angular_acceleration_poll();
void actuator_controls_poll();
void control_update();
void manual_control_setpoint_poll();
void rc_channels_poll();
@@ -281,30 +281,42 @@ private:
void _select_loiter_trajectory(); // select the correct loiter trajectory based on available energy
void _select_soaring_trajectory(); // select the correct loiter trajectory based on available energy
void _read_trajectory_coeffs_csv(char *filename); // read in the correct coefficients of the appropriate trajectory
float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position
float _get_closest_t(Vector3f
pos); // get the normalized time, at which the reference path is closest to the current position
Vector3f _compute_wind_estimate(); // compute a wind estimate to be used only inside the controller
Vector3f _compute_wind_estimate_EKF(); // compute a wind estimate to be used only as a measurement for the shear estimator
void _set_wind_estimate(Vector3f wind);
void _set_wind_estimate_EKF(Vector3f wind);
Vector<float, _num_basis_funs> _get_basis_funs(float t=0); // compute the vector of basis functions at normalized time t in [0,1]
Vector<float, _num_basis_funs> _get_d_dt_basis_funs(float t=0); // compute the vector of basis function gradients at normalized time t in [0,1]
Vector<float, _num_basis_funs> _get_d2_dt2_basis_funs(float t=0); // compute the vector of basis function curvatures at normalized time t in [0,1]
Vector<float, _num_basis_funs> _get_basis_funs(float t =
0); // compute the vector of basis functions at normalized time t in [0,1]
Vector<float, _num_basis_funs> _get_d_dt_basis_funs(float t =
0); // compute the vector of basis function gradients at normalized time t in [0,1]
Vector<float, _num_basis_funs> _get_d2_dt2_basis_funs(float t =
0); // compute the vector of basis function curvatures at normalized time t in [0,1]
void _load_basis_coefficients(); // load the coefficients of the current path approximation
Vector3f _get_position_ref(float t=0); // get the reference position on the current path, at normalized time t in [0,1]
Vector3f _get_velocity_ref(float t=0, float T=1); // get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
Vector3f _get_acceleration_ref(float t=0, float T=1); // get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
Quatf _get_attitude_ref(float t=0, float T=1); // get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T
Vector3f _get_angular_velocity_ref(float t=0, float T=1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
Vector3f _get_angular_acceleration_ref(float t=0, float T=1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel
Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref);
Vector3f _get_position_ref(float t =
0); // get the reference position on the current path, at normalized time t in [0,1]
Vector3f _get_velocity_ref(float t = 0,
float T = 1); // get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
Vector3f _get_acceleration_ref(float t = 0,
float T = 1); // get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
Quatf _get_attitude_ref(float t = 0,
float T = 1); // get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T
Vector3f _get_angular_velocity_ref(float t = 0,
float T = 1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
Vector3f _get_angular_acceleration_ref(float t = 0,
float T = 1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
Quatf _get_attitude(Vector3f vel, Vector3f
f); // get the attitude to produce force f while flying with velocity vel
Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref,
Vector3f alpha_ref);
Vector3f _compute_INDI_stage_2(Vector3f ctrl);
Vector3f _compute_actuator_deflections(Vector3f ctrl);
// helper methods
void _reverse(char* str, int len); // reverse a string of length 'len'
void _reverse(char *str, int len); // reverse a string of length 'len'
int _int_to_str(int x, char str[], int d); // convert an integer x into a string of length d
void _float_to_str(float n, char* res, int afterpoint); // convert float to string
void _float_to_str(float n, char *res, int afterpoint); // convert float to string
// control variables
@@ -326,9 +338,9 @@ private:
Vector3f _omega; // angular rate vector
Vector3f _alpha; // angular acceleration vector
float _k_ail;
float _k_ele;
float _k_d_roll;
float _k_d_pitch;
float _k_ele;
float _k_d_roll;
float _k_d_pitch;
hrt_abstime _last_run{0};
// controller frequency
@@ -339,9 +351,10 @@ private:
math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // force command
math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // body rates
// smoothing filter to reject HF noise in control output
const float _cutoff_frequency_smoothing = 20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz)
const float _cutoff_frequency_smoothing =
20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz)
math::LowPassFilter2p _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}}; // force command stage 1
math::LowPassFilter2p _lp_filter_rud {_sample_frequency, 10}; // rudder command
math::LowPassFilter2p _lp_filter_rud {_sample_frequency, 10}; // rudder command
// Low-Pass filters stage 2
const float _cutoff_frequency_2 = 20.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF"
math::LowPassFilter2p _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // filter to match acceleration processing delay
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+91 -91
View File
@@ -12,9 +12,9 @@
/**
* inertia around body x-axis
*
*
* This is the inertia of the aircraft, used for the INDI
*
*
* @unit kg
* @min 0.01
* @max 10
@@ -26,9 +26,9 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_ROLL, 0.197563f);
/**
* inertia around body y-axis
*
*
* This is the inertia of the aircraft, used for the INDI
*
*
* @unit kg
* @min 0.01
* @max 10
@@ -40,9 +40,9 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_PITCH, 0.1458929f);
/**
* inertia around body z-axis
*
*
* This is the inertia of the aircraft, used for the INDI
*
*
* @unit kg
* @min 0.01
* @max 10
@@ -54,9 +54,9 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_YAW, 0.1477f);
/**
* inertia tensor term in body xz-axis (roll-yaw coupling)
*
*
* This is the inertia of the aircraft, used for the INDI
*
*
* @unit kg
* @min -0.5
* @max 0
@@ -68,9 +68,9 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_RP, -0.0f);
/**
* total takeoff mass
*
*
* This is the mass of the aircraft, used for the INDI
*
*
* @unit kg
* @min 1.0
* @max 2.0
@@ -82,7 +82,7 @@ PARAM_DEFINE_FLOAT(DS_MASS, 1.4f);
/**
* total wing area used for lift and drag computation
*
*
* @unit m^2
* @min 0.1
* @max 1.0
@@ -95,8 +95,8 @@ PARAM_DEFINE_FLOAT(DS_WING_AREA, 0.4f);
/**
* air density used for lift and drag computation
*
* @unit
*
* @unit
* @min 0.5
* @max 1.225
* @decimal 3
@@ -107,11 +107,11 @@ PARAM_DEFINE_FLOAT(DS_RHO, 1.223f);
/**
* estimated lift coefficients used for lift and drag computation
*
*
* Used as L = C_l0 + C_l1*alpha,
* where alpha is the angle of attack.
*
* @unit
*
* @unit
* @min -100
* @max 100
* @decimal 3
@@ -122,11 +122,11 @@ PARAM_DEFINE_FLOAT(DS_C_L0, 0.356f);
/**
* estimated lift coefficients used for lift and drag computation
*
*
* Used as L = C_l0 + C_l1*alpha,
* where alpha is the angle of attack.
*
* @unit
*
* @unit
* @min -100
* @max 100
* @decimal 3
@@ -138,11 +138,11 @@ PARAM_DEFINE_FLOAT(DS_C_L1, 2.354f);
/**
* estimated drag coefficients used for lift and drag computation
*
*
* Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
* where alpha is the angle of attack.
*
* @unit
*
* @unit
* @min -100
* @max 100
* @decimal 4
@@ -153,11 +153,11 @@ PARAM_DEFINE_FLOAT(DS_C_D0, 0.0288f);
/**
* estimated drag coefficients used for lift and drag computation
*
*
* Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
* where alpha is the angle of attack.
*
* @unit
*
* @unit
* @min -100
* @max 100
* @decimal 4
@@ -168,11 +168,11 @@ PARAM_DEFINE_FLOAT(DS_C_D1, 0.3783f);
/**
* estimated drag coefficients used for lift and drag computation
*
*
* Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
* where alpha is the angle of attack.
*
* @unit
*
* @unit
* @min -100
* @max 100
* @decimal 4
@@ -183,11 +183,11 @@ PARAM_DEFINE_FLOAT(DS_C_D2, 1.984f);
/**
* estimated sideslip sensitivity coefficients used for wind estimation
*
*
* Used as F_y = 0.5 * DS_RHO * ASPD^2 * DS_C_B1,
* where alpha is the angle of attack.
*
* @unit
*
* @unit
* @min -100
* @max -0.01
* @decimal 4
@@ -198,9 +198,9 @@ PARAM_DEFINE_FLOAT(DS_C_B1, -3.32f);
/**
* offset angle between body frame (Pixhawk) and the wing chord
*
*
* Used to compute the AoA
*
*
* @unit rad
* @min 0
* @max 0.1
@@ -212,7 +212,7 @@ PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.07f);
/**
* stall speed of the aircraft
*
*
* @unit m/s
* @min 5
* @max 10
@@ -228,8 +228,8 @@ PARAM_DEFINE_FLOAT(DS_STALL_SPEED, 9.0f);
// ========================================================
/**
* control gain of position PD-controller (body x-direction)
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 1
@@ -240,8 +240,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_K_X, 1.0f);
/**
* control gain of position PD-controller (body y-direction)
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 1
@@ -252,8 +252,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_K_Y, 1.0f);
/**
* control gain of position PD-controller (body z-direction)
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 1
@@ -264,8 +264,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_K_Z, 1.0f);
/**
* normalized damping coefficient of position PD-controller (body x-direction)
*
* @unit
*
* @unit
* @min 0
* @max 2
* @decimal 2
@@ -276,8 +276,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_C_X, 1.0f);
/**
* normalized damping coefficient of position PD-controller (body y-direction)
*
* @unit
*
* @unit
* @min 0
* @max 2
* @decimal 2
@@ -288,8 +288,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_C_Y, 1.0f);
/**
* normalized damping coefficient of position PD-controller (body z-direction)
*
* @unit
*
* @unit
* @min 0
* @max 2
* @decimal 2
@@ -300,8 +300,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_C_Z, 1.0f);
/**
* acceleration feedback gain of position PD-controller (body x-direction)
*
* @unit
*
* @unit
* @min 0
* @max 10
* @decimal 1
@@ -312,8 +312,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_FF_X, 0.5f);
/**
* acceleration feedback gain of position PD-controller (body y-direction)
*
* @unit
*
* @unit
* @min 0
* @max 10
* @decimal 1
@@ -324,8 +324,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_FF_Y, 0.5f);
/**
* acceleration feedback gain of position PD-controller (body z-direction)
*
* @unit
*
* @unit
* @min 0
* @max 10
* @decimal 1
@@ -336,8 +336,8 @@ PARAM_DEFINE_FLOAT(DS_LIN_FF_Z, 0.5f);
/**
* control gain of attitude PD-controller (body roll-direction)
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 1
@@ -348,8 +348,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 30.0f);
/**
* control gain of attitude PD-controller (body pitch-direction)
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 1
@@ -360,8 +360,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 30.0f);
/**
* rudder turn coordination FF-gain
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 1
@@ -372,8 +372,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_FF_YAW, 1.0f);
/**
* normalized damping coefficient of attitude PD-controller (body roll-direction)
*
* @unit
*
* @unit
* @min 0
* @max 2
* @decimal 2
@@ -384,8 +384,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 1.0f);
/**
* normalized damping coefficient of attitude PD-controller (body pitch-direction)
*
* @unit
*
* @unit
* @min 0
* @max 2
* @decimal 2
@@ -396,8 +396,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 1.0f);
/**
* rudder turn coordination P-gain
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 2
@@ -413,8 +413,8 @@ PARAM_DEFINE_FLOAT(DS_ROT_P_YAW, 1.0f);
/**
* roll gain of K_ACT (actuator deflection gain)
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 3
@@ -425,8 +425,8 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.25f);
/**
* pitch gain of K_ACT (actuator deflection gain)
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 2
@@ -438,8 +438,8 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.05f);
/**
* roll gain of K_ACT_DAMPING (actuator damping gain)
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 3
@@ -450,8 +450,8 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.04f);
/**
* pitch gain of K_ACT_DAMPING (actuator damping gain)
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 3
@@ -467,8 +467,8 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.02f);
/**
* latitude of trajectory start point (WGS84)
*
* @unit
*
* @unit
* @min -90
* @max 90
* @decimal 7
@@ -479,8 +479,8 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3130000f);
/**
* longitude of trajectory start point (WGS84)
*
* @unit
*
* @unit
* @min -180
* @max 180
* @decimal 7
@@ -491,8 +491,8 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8100000f);
/**
* altitude of trajectory start point (WGS84)
*
* @unit
*
* @unit
* @min 0
* @max 2000
* @decimal 1
@@ -507,8 +507,8 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f);
/**
* integer in {0,1,2,3,4,5} defining the loiter trajectory
*
* @unit
*
* @unit
* @min 0
* @max 7
* @decimal 1
@@ -523,7 +523,7 @@ PARAM_DEFINE_INT32(DS_LOITER, 0);
/**
* integer defining wind heading
*
*
* @unit deg
* @min -180
* @max 180
@@ -539,8 +539,8 @@ PARAM_DEFINE_INT32(DS_W_HEADING, 0);
/**
* float defining wind shear vertical postition in soaring frame
*
* @unit
*
* @unit
* @min 0
* @max 100
* @decimal 1
@@ -554,8 +554,8 @@ PARAM_DEFINE_FLOAT(DS_W_HEIGHT, 100.f);
// ==============================================
/**
* float in [0,1] corresponding to the engine thrust
*
* @unit
*
* @unit
* @min 0
* @max 1
* @decimal 2
@@ -570,8 +570,8 @@ PARAM_DEFINE_FLOAT(DS_THRUST, 0);
/**
* integer in {0,1} defining if the commanded force has an upper bound (saturates), 0=no saturation, 1=saturation
*
* @unit
*
* @unit
* @min 0
* @max 1
* @decimal 1
@@ -587,7 +587,7 @@ PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1);
/**
* integer in {0,1} defining if the trajectory origin is taken from hardcoded params or shear estimate, 1=params, 0=estimate
* @unit
* @unit
* @min 0
* @max 1
* @decimal 1
@@ -602,7 +602,7 @@ PARAM_DEFINE_INT32(DS_SWITCH_ORI_HC, 1);
/**
* integer in {0,1} defining if the loiter circle defined by param DS_LOITER shall be used, 0=soaring, 1=loiter
* @unit
* @unit
* @min 0
* @max 1
* @decimal 1
@@ -617,7 +617,7 @@ PARAM_DEFINE_INT32(DS_SWITCH_LOITER, 1);
/**
* integer in {0,1} defining if we are using manual feedthrough, only used in SITL mode
* @unit
* @unit
* @min 0
* @max 1
* @decimal 1
@@ -632,7 +632,7 @@ PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 1);
/**
* integer in {0,1} defining if the shear parameters are changed by the estimator while soaring (closed loop). 0=fixed shear, 1=changing shear
* @unit
* @unit
* @min 0
* @max 1
* @decimal 1
src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
+436 -402
View File
@@ -48,7 +48,7 @@ using matrix::geninv;
FixedwingShearEstimator::FixedwingShearEstimator() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::test1),
_soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear)),
_soaring_estimator_shear_pub(ORB_ID(soaring_estimator_shear)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
// limit to 10 Hz
@@ -70,124 +70,132 @@ FixedwingShearEstimator::init()
PX4_ERR("vehicle position callback registration failed!");
return false;
}
PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR");
// normalization variables
_unit_v = 1.f;
_unit_h = 1.f;
_unit_a = 1.f;
PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR");
parameters_update();
// normalization variables
_unit_v = 1.f;
_unit_h = 1.f;
_unit_a = 1.f;
// init time
_last_run = hrt_absolute_time();
parameters_update();
// init reset counter
_reset_counter = -1;
reset_filter();
// init time
_last_run = hrt_absolute_time();
//
_v_max_lock = 8.f;
_alpha_lock = 0.5f;
// init reset counter
_reset_counter = -1;
reset_filter();
// ==============================================================================
// fill the min airspeed matrix with the correct entries for trajectory selection
// ==============================================================================
bool error = false;
//char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
strcat(home_dir, "robust/min_aspd_matrix.csv");
FILE* fp = fopen(home_dir, "r");
//
_v_max_lock = 8.f;
_alpha_lock = 0.5f;
if (fp == nullptr) {
PX4_ERR("Can't open MIN_ASPD_MATRIX");
error = true;
}
else {
// Here we have taken size of
// array 200 you can modify it
const uint buffersize = 600;
char buffer[buffersize];
// ==============================================================================
// fill the min airspeed matrix with the correct entries for trajectory selection
// ==============================================================================
int row = 0;
int column = 0;
// loop over rows
while (fgets(buffer,
buffersize, fp)) {
column = 0;
// Splitting the data
char* value = strtok(buffer, ",");
// loop over columns
while (value) {
if (*value=='\0'||*value==' ') {
// simply skip extra characters
continue;
}
if (row<(int)_num_v_max && column<(int)_num_alpha) {
_MIN_ASPD_MATRIX(row,column) = (float)atof(value);
_MAX_ASPD_MATRIX(row,column) = (float)atof(value) + 4.f;
}
else {
PX4_ERR("Encountered invalid MIN_ASPD_MATRIX");
}
//PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value));
value = strtok(NULL, ",");
column++;
}
row++;
}
int failure = fclose(fp);
if (failure==-1) {
PX4_ERR("Can't close MIN_ASPD_MATRIX");
}
}
// =======================================================================
bool error = false;
if (error) {
/*
// column 0: wind speed = 8
_MIN_ASPD_MATRIX(0,0) = 28;
_MIN_ASPD_MATRIX(1,0) = 24;
_MIN_ASPD_MATRIX(2,0) = 22;
_MIN_ASPD_MATRIX(3,0) = 20;
_MIN_ASPD_MATRIX(4,0) = 20;
_MIN_ASPD_MATRIX(5,0) = 20;
_MIN_ASPD_MATRIX(6,0) = 20;
_MIN_ASPD_MATRIX(7,0) = 20;
_MIN_ASPD_MATRIX(8,0) = 20;
//char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
strcat(home_dir, "robust/min_aspd_matrix.csv");
FILE *fp = fopen(home_dir, "r");
// column 0: wind speed = 9
_MIN_ASPD_MATRIX(0,0) = 26;
_MIN_ASPD_MATRIX(1,0) = 22;
_MIN_ASPD_MATRIX(2,0) = 20;
_MIN_ASPD_MATRIX(3,0) = 20;
_MIN_ASPD_MATRIX(4,0) = 20;
_MIN_ASPD_MATRIX(5,0) = 20;
_MIN_ASPD_MATRIX(6,0) = 20;
_MIN_ASPD_MATRIX(7,0) = 18;
_MIN_ASPD_MATRIX(8,0) = 18;
if (fp == nullptr) {
PX4_ERR("Can't open MIN_ASPD_MATRIX");
error = true;
// column 0: wind speed = 10
_MIN_ASPD_MATRIX(0,0) = 24;
_MIN_ASPD_MATRIX(1,0) = 22;
_MIN_ASPD_MATRIX(2,0) = 20;
_MIN_ASPD_MATRIX(3,0) = 20;
_MIN_ASPD_MATRIX(4,0) = 20;
_MIN_ASPD_MATRIX(5,0) = 20;
_MIN_ASPD_MATRIX(6,0) = 18;
_MIN_ASPD_MATRIX(7,0) = 18;
_MIN_ASPD_MATRIX(8,0) = 18;
*/
} else {
// Here we have taken size of
// array 200 you can modify it
const uint buffersize = 600;
char buffer[buffersize];
}
int row = 0;
int column = 0;
// loop over rows
while (fgets(buffer,
buffersize, fp)) {
column = 0;
// Splitting the data
char *value = strtok(buffer, ",");
// loop over columns
while (value) {
if (*value == '\0' || *value == ' ') {
// simply skip extra characters
continue;
}
if (row < (int)_num_v_max && column < (int)_num_alpha) {
_MIN_ASPD_MATRIX(row, column) = (float)atof(value);
_MAX_ASPD_MATRIX(row, column) = (float)atof(value) + 4.f;
} else {
PX4_ERR("Encountered invalid MIN_ASPD_MATRIX");
}
//PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value));
value = strtok(NULL, ",");
column++;
}
row++;
}
int failure = fclose(fp);
if (failure == -1) {
PX4_ERR("Can't close MIN_ASPD_MATRIX");
}
}
// =======================================================================
if (error) {
/*
// column 0: wind speed = 8
_MIN_ASPD_MATRIX(0,0) = 28;
_MIN_ASPD_MATRIX(1,0) = 24;
_MIN_ASPD_MATRIX(2,0) = 22;
_MIN_ASPD_MATRIX(3,0) = 20;
_MIN_ASPD_MATRIX(4,0) = 20;
_MIN_ASPD_MATRIX(5,0) = 20;
_MIN_ASPD_MATRIX(6,0) = 20;
_MIN_ASPD_MATRIX(7,0) = 20;
_MIN_ASPD_MATRIX(8,0) = 20;
// column 0: wind speed = 9
_MIN_ASPD_MATRIX(0,0) = 26;
_MIN_ASPD_MATRIX(1,0) = 22;
_MIN_ASPD_MATRIX(2,0) = 20;
_MIN_ASPD_MATRIX(3,0) = 20;
_MIN_ASPD_MATRIX(4,0) = 20;
_MIN_ASPD_MATRIX(5,0) = 20;
_MIN_ASPD_MATRIX(6,0) = 20;
_MIN_ASPD_MATRIX(7,0) = 18;
_MIN_ASPD_MATRIX(8,0) = 18;
// column 0: wind speed = 10
_MIN_ASPD_MATRIX(0,0) = 24;
_MIN_ASPD_MATRIX(1,0) = 22;
_MIN_ASPD_MATRIX(2,0) = 20;
_MIN_ASPD_MATRIX(3,0) = 20;
_MIN_ASPD_MATRIX(4,0) = 20;
_MIN_ASPD_MATRIX(5,0) = 20;
_MIN_ASPD_MATRIX(6,0) = 18;
_MIN_ASPD_MATRIX(7,0) = 18;
_MIN_ASPD_MATRIX(8,0) = 18;
*/
}
return true;
return true;
}
int
@@ -196,25 +204,25 @@ FixedwingShearEstimator::parameters_update()
updateParams();
// update params...
_Q_horizontal.setZero();
_Q_horizontal(0,0) = powf(_param_sigma_q_vel.get()/_unit_v,2);
_Q_horizontal(1,1) = powf(_param_sigma_q_vel.get()/_unit_v,2);
_Q_horizontal(2,2) = powf(_param_sigma_q_vel.get()/_unit_v,2);
_Q_horizontal(3,3) = powf(_param_sigma_q_vel.get()/_unit_v,2);
_Q_horizontal(4,4) = powf(_param_sigma_q_h.get()/_unit_h,2);
_Q_horizontal(5,5) = powf(_param_sigma_q_a.get()/_unit_a,2);
_Q_horizontal.setZero();
_Q_horizontal(0, 0) = powf(_param_sigma_q_vel.get() / _unit_v, 2);
_Q_horizontal(1, 1) = powf(_param_sigma_q_vel.get() / _unit_v, 2);
_Q_horizontal(2, 2) = powf(_param_sigma_q_vel.get() / _unit_v, 2);
_Q_horizontal(3, 3) = powf(_param_sigma_q_vel.get() / _unit_v, 2);
_Q_horizontal(4, 4) = powf(_param_sigma_q_h.get() / _unit_h, 2);
_Q_horizontal(5, 5) = powf(_param_sigma_q_a.get() / _unit_a, 2);
_R_horizontal.setZero();
_R_horizontal(0,0) = powf(_param_sigma_r_vel.get()/_unit_v,2);
_R_horizontal(1,1) = powf(_param_sigma_r_vel.get()/_unit_v,2);
_R_horizontal.setZero();
_R_horizontal(0, 0) = powf(_param_sigma_r_vel.get() / _unit_v, 2);
_R_horizontal(1, 1) = powf(_param_sigma_r_vel.get() / _unit_v, 2);
for (int i=0;i<(int)_dim_vertical;i++){
_Q_vertical(i,i) = powf(_param_sigma_q_vel.get(),2);
}
for (int i = 0; i < (int)_dim_vertical; i++) {
_Q_vertical(i, i) = powf(_param_sigma_q_vel.get(), 2);
}
_R_vertical(0,0) = powf(_param_sigma_r_vel.get(),2);
_R_vertical(0, 0) = powf(_param_sigma_r_vel.get(), 2);
_init_height = _param_init_h.get()/_unit_h;
_init_height = _param_init_h.get() / _unit_h;
return PX4_OK;
}
@@ -222,143 +230,151 @@ FixedwingShearEstimator::parameters_update()
void
FixedwingShearEstimator::reset_filter()
{
// reset all states of the filter to some initial guess.
// reset all states of the filter to some initial guess.
// reset horizontal wind state
for (uint i=0;i<6;i++){
_X_prior_horizontal(i) = 0.0f;
_X_posterior_horizontal(i) = 0.0f;
}
_P_prior_horizontal = 10.f*_Q_horizontal;
_P_posterior_horizontal = 10.f*_Q_horizontal;
// reset horizontal wind state
for (uint i = 0; i < 6; i++) {
_X_prior_horizontal(i) = 0.0f;
_X_posterior_horizontal(i) = 0.0f;
}
// reset vertical wind state
for (uint i=0;i<_dim_vertical;i++){
_X_prior_vertical(i) = 0.0f;
_X_posterior_vertical(i) = 0.0f;
}
_P_prior_vertical = 10.f*_Q_vertical;
_P_posterior_vertical = 10.f*_Q_vertical;
_P_prior_horizontal = 10.f * _Q_horizontal;
_P_posterior_horizontal = 10.f * _Q_horizontal;
// set height to enable convergence
_X_prior_horizontal(4) = _init_height;
_X_posterior_horizontal(4) = _init_height;
_X_prior_horizontal(5) = 0.5f/_unit_a;
_X_posterior_horizontal(5) = 0.5f/_unit_a;
// reset vertical wind state
for (uint i = 0; i < _dim_vertical; i++) {
_X_prior_vertical(i) = 0.0f;
_X_posterior_vertical(i) = 0.0f;
}
// increment counter
_reset_counter += 1;
_P_prior_vertical = 10.f * _Q_vertical;
_P_posterior_vertical = 10.f * _Q_vertical;
// set height to enable convergence
_X_prior_horizontal(4) = _init_height;
_X_posterior_horizontal(4) = _init_height;
_X_prior_horizontal(5) = 0.5f / _unit_a;
_X_posterior_horizontal(5) = 0.5f / _unit_a;
// increment counter
_reset_counter += 1;
}
void
FixedwingShearEstimator::perform_prior_update()
{
// get time since last run
float dt = (float)(hrt_absolute_time() - _last_run)/1000000.f;
_last_run = hrt_absolute_time();
if (dt>1.f) {
dt = 1.f;
}
// get time since last run
float dt = (float)(hrt_absolute_time() - _last_run) / 1000000.f;
_last_run = hrt_absolute_time();
// perform prior update assuming trivial dynamics of the wind field (mean field stays the same)
_X_prior_horizontal = _X_posterior_horizontal;
_X_prior_vertical = _X_posterior_vertical;
_P_prior_horizontal = _P_posterior_horizontal + dt*_Q_horizontal;
_P_prior_vertical = _P_posterior_vertical + dt*_Q_vertical;
if (dt > 1.f) {
dt = 1.f;
}
// perform prior update assuming trivial dynamics of the wind field (mean field stays the same)
_X_prior_horizontal = _X_posterior_horizontal;
_X_prior_vertical = _X_posterior_vertical;
_P_prior_horizontal = _P_posterior_horizontal + dt * _Q_horizontal;
_P_prior_vertical = _P_posterior_vertical + dt * _Q_vertical;
}
void
FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
{
//
bool error = false;
//
bool error = false;
// first fill the horizontal observation matrix:
float vx = _X_prior_horizontal(0);
float vy = _X_prior_horizontal(1);
float h = _X_prior_horizontal(4);
float a = _X_prior_horizontal(5);
// first fill the horizontal observation matrix:
float vx = _X_prior_horizontal(0);
float vy = _X_prior_horizontal(1);
float h = _X_prior_horizontal(4);
float a = _X_prior_horizontal(5);
_H_horizontal.setZero();
_H_horizontal(0,0) = 1.f/(1.f + expf(-(height-h)*a));
_H_horizontal(1,1) = 1.f/(1.f + expf(-(height-h)*a));
_H_horizontal(0,2) = 1.f;
_H_horizontal(1,3) = 1.f;
_H_horizontal(0,4) = -((a*vx*expf(-(height-h)*a)))/powf((1.f + expf(-(height-h)*a)),2.f);
_H_horizontal(1,4) = -((a*vy*expf(-(height-h)*a)))/powf((1.f + expf(-(height-h)*a)),2.f);
_H_horizontal(0,5) = -((vx*(h-height))*expf(-(height-h)*a))/powf((1.f + expf(-(height-h)*a)),2.f);
_H_horizontal(1,5) = -((vy*(h-height))*expf(-(height-h)*a))/powf((1.f + expf(-(height-h)*a)),2.f);
_H_horizontal.setZero();
_H_horizontal(0, 0) = 1.f / (1.f + expf(-(height - h) * a));
_H_horizontal(1, 1) = 1.f / (1.f + expf(-(height - h) * a));
_H_horizontal(0, 2) = 1.f;
_H_horizontal(1, 3) = 1.f;
_H_horizontal(0, 4) = -((a * vx * expf(-(height - h) * a))) / powf((1.f + expf(-(height - h) * a)), 2.f);
_H_horizontal(1, 4) = -((a * vy * expf(-(height - h) * a))) / powf((1.f + expf(-(height - h) * a)), 2.f);
_H_horizontal(0, 5) = -((vx * (h - height)) * expf(-(height - h) * a)) / powf((1.f + expf(-(height - h) * a)), 2.f);
_H_horizontal(1, 5) = -((vy * (h - height)) * expf(-(height - h) * a)) / powf((1.f + expf(-(height - h) * a)), 2.f);
// then fill the vertical observation matrix
for (uint i=0;i<_dim_vertical;i++){
_H_vertical(0,i) = powf(height-h,i);
}
// then fill the vertical observation matrix
for (uint i = 0; i < _dim_vertical; i++) {
_H_vertical(0, i) = powf(height - h, i);
}
// compute Kalman gain matrix for horizontal wind states
Matrix<float, 6, 2> tmp1_horizontal = _P_prior_horizontal*_H_horizontal.T();
Matrix<float, 2, 2> tmp2_horizontal = _H_horizontal*(_P_prior_horizontal*_H_horizontal.T()) + _R_horizontal;
float determinant_horizontal = tmp2_horizontal(0,0)*tmp2_horizontal(1,1) - tmp2_horizontal(1,0)*tmp2_horizontal(0,1);
if ((float)fabs(determinant_horizontal)>=0.000001f){
_K_horizontal = tmp1_horizontal*geninv(tmp2_horizontal);
}
else{
//PX4_WARN("singular horizontal matrix, resetting filter");
error = true;
}
// compute Kalman gain matrix for horizontal wind states
Matrix<float, 6, 2> tmp1_horizontal = _P_prior_horizontal * _H_horizontal.T();
Matrix<float, 2, 2> tmp2_horizontal = _H_horizontal * (_P_prior_horizontal * _H_horizontal.T()) + _R_horizontal;
float determinant_horizontal = tmp2_horizontal(0, 0) * tmp2_horizontal(1, 1) - tmp2_horizontal(1,
0) * tmp2_horizontal(0, 1);
// then fill the vertical observation matrix
for (uint i=0;i<_dim_vertical;i++){
_H_vertical(0,i) = powf(height-h,i);
}
if ((float)fabs(determinant_horizontal) >= 0.000001f) {
_K_horizontal = tmp1_horizontal * geninv(tmp2_horizontal);
// compute Kalman gain matrix for vertical wind states
Matrix<float, _dim_vertical, 1> tmp1_vertical = _P_prior_vertical*_H_vertical.T();
Matrix<float, 1, 1> tmp2_vertical = _H_vertical*_P_prior_vertical*_H_vertical.T() + _R_vertical;
Matrix<float, 1, 1> inv_vertical;
float determinant_vertical = tmp2_vertical(0,0);
if ((float)fabs(determinant_vertical)>0.000001f){
inv_vertical(0,0) = 1.f / determinant_vertical;
_K_vertical = tmp1_vertical*inv_vertical;
}
else{
//PX4_WARN("singular vertical matrix, resetting filter");
error = true;
}
} else {
//PX4_WARN("singular horizontal matrix, resetting filter");
error = true;
}
if (error) {
reset_filter();
}
else {
// perform horizontal update
Vector<float, 2> z_expected_horizontal;
Vector<float, 2> wind_horizontal;
Matrix<float, 6, 6> identity_1;
z_expected_horizontal(0) = _X_prior_horizontal(0)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(2);
z_expected_horizontal(1) = _X_prior_horizontal(1)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(3);
wind_horizontal(0) = _current_wind(0);
wind_horizontal(1) = _current_wind(1);
identity_1.setIdentity();
_X_posterior_horizontal = _X_prior_horizontal + _K_horizontal*(wind_horizontal - z_expected_horizontal);
_P_posterior_horizontal = (identity_1 - _K_horizontal*_H_horizontal)*_P_prior_horizontal;
// then fill the vertical observation matrix
for (uint i = 0; i < _dim_vertical; i++) {
_H_vertical(0, i) = powf(height - h, i);
}
// perform vertical update
Vector<float, 1> z_expected_vertical = (Vector<float, 1>) (_H_vertical*_X_prior_vertical);
Vector<float, 1> wind_vertical;
Matrix<float, _dim_vertical, _dim_vertical> identity_2;
wind_vertical(0) = wind(2);
identity_2.setIdentity();
_X_posterior_vertical = _X_prior_vertical + _K_vertical*(wind_vertical - z_expected_vertical);
_P_posterior_vertical = (identity_2 - _K_vertical*_H_vertical)*_P_prior_vertical;
}
// compute Kalman gain matrix for vertical wind states
Matrix<float, _dim_vertical, 1> tmp1_vertical = _P_prior_vertical * _H_vertical.T();
Matrix<float, 1, 1> tmp2_vertical = _H_vertical * _P_prior_vertical * _H_vertical.T() + _R_vertical;
Matrix<float, 1, 1> inv_vertical;
float determinant_vertical = tmp2_vertical(0, 0);
// find the correct sign of params (parametrization is not unique)
if (_X_posterior_horizontal(5)<0.f) {
_X_posterior_horizontal(0) *= -1.f;
_X_posterior_horizontal(1) *= -1.f;
_X_posterior_horizontal(2) = _X_posterior_horizontal(2) - _X_posterior_horizontal(0);
_X_posterior_horizontal(3) = _X_posterior_horizontal(3) - _X_posterior_horizontal(1);
_X_posterior_horizontal(5) *= -1.f;
}
if ((float)fabs(determinant_vertical) > 0.000001f) {
inv_vertical(0, 0) = 1.f / determinant_vertical;
_K_vertical = tmp1_vertical * inv_vertical;
} else {
//PX4_WARN("singular vertical matrix, resetting filter");
error = true;
}
if (error) {
reset_filter();
} else {
// perform horizontal update
Vector<float, 2> z_expected_horizontal;
Vector<float, 2> wind_horizontal;
Matrix<float, 6, 6> identity_1;
z_expected_horizontal(0) = _X_prior_horizontal(0) / (1.f + expf(-_X_prior_horizontal(5) *
(_current_height - _X_prior_horizontal(4)))) + _X_prior_horizontal(2);
z_expected_horizontal(1) = _X_prior_horizontal(1) / (1.f + expf(-_X_prior_horizontal(5) *
(_current_height - _X_prior_horizontal(4)))) + _X_prior_horizontal(3);
wind_horizontal(0) = _current_wind(0);
wind_horizontal(1) = _current_wind(1);
identity_1.setIdentity();
_X_posterior_horizontal = _X_prior_horizontal + _K_horizontal * (wind_horizontal - z_expected_horizontal);
_P_posterior_horizontal = (identity_1 - _K_horizontal * _H_horizontal) * _P_prior_horizontal;
// perform vertical update
Vector<float, 1> z_expected_vertical = (Vector<float, 1>)(_H_vertical * _X_prior_vertical);
Vector<float, 1> wind_vertical;
Matrix<float, _dim_vertical, _dim_vertical> identity_2;
wind_vertical(0) = wind(2);
identity_2.setIdentity();
_X_posterior_vertical = _X_prior_vertical + _K_vertical * (wind_vertical - z_expected_vertical);
_P_posterior_vertical = (identity_2 - _K_vertical * _H_vertical) * _P_prior_vertical;
}
// find the correct sign of params (parametrization is not unique)
if (_X_posterior_horizontal(5) < 0.f) {
_X_posterior_horizontal(0) *= -1.f;
_X_posterior_horizontal(1) *= -1.f;
_X_posterior_horizontal(2) = _X_posterior_horizontal(2) - _X_posterior_horizontal(0);
_X_posterior_horizontal(3) = _X_posterior_horizontal(3) - _X_posterior_horizontal(1);
_X_posterior_horizontal(5) *= -1.f;
}
}
@@ -366,19 +382,19 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
void
FixedwingShearEstimator::Run()
{
if (should_exit()) {
if (should_exit()) {
_soaring_controller_wind_sub.unregisterCallback();
exit_and_cleanup();
return;
}
perf_begin(_loop_perf);
perf_begin(_loop_perf);
// only run controller if wind info changed
soaring_controller_wind_s soaring_controller_wind;
if (_soaring_controller_wind_sub.update(&soaring_controller_wind))
{
// only update parameters if they changed
// only run controller if wind info changed
soaring_controller_wind_s soaring_controller_wind;
if (_soaring_controller_wind_sub.update(&soaring_controller_wind)) {
// only update parameters if they changed
bool params_updated = _parameter_update_sub.updated();
// check for parameter updates
@@ -391,194 +407,211 @@ FixedwingShearEstimator::Run()
updateParams();
parameters_update();
}
// get current measurement
_current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered)/_unit_v;
_current_height = Vector3f(soaring_controller_wind.position)(2)/_unit_h;
_current_airspeed = soaring_controller_wind.airspeed;
_lock_params = soaring_controller_wind.lock_params;
//PX4_INFO("estimating shear, shear lock is: \t%d", _lock_params);
// only update shear estimae if aircraft is close to the soaring frame origin (shear is a local phenomenon)
if (soaring_controller_wind.valid) {
// prior update
perform_prior_update();
// get current measurement
_current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered) / _unit_v;
_current_height = Vector3f(soaring_controller_wind.position)(2) / _unit_h;
_current_airspeed = soaring_controller_wind.airspeed;
_lock_params = soaring_controller_wind.lock_params;
//PX4_INFO("estimating shear, shear lock is: \t%d", _lock_params);
// posterior update
perform_posterior_update(_current_height, _current_wind);
// only update shear estimae if aircraft is close to the soaring frame origin (shear is a local phenomenon)
if (soaring_controller_wind.valid) {
// prior update
perform_prior_update();
// check if filter diverges
// maybe reset filters...
if (sqrtf(_P_posterior_horizontal(4,4))>=5.f||sqrtf(_P_posterior_horizontal(5,5))>=0.3f) {
PX4_WARN("large height uncertainty, resetting filter");
reset_filter();
}
// posterior update
perform_posterior_update(_current_height, _current_wind);
}
// check if filter diverges
// maybe reset filters...
if (sqrtf(_P_posterior_horizontal(4, 4)) >= 5.f || sqrtf(_P_posterior_horizontal(5, 5)) >= 0.3f) {
PX4_WARN("large height uncertainty, resetting filter");
reset_filter();
}
// =====================================================
// get the correct shear params for trajectory selection
// =====================================================
float shear = sqrtf(powf(_X_posterior_horizontal(0)*_unit_v, 2) +
powf(_X_posterior_horizontal(1)*_unit_v, 2));
float v = _findClosest(_v_max_arr, 5, shear); // wind velocity
float a = _findClosest(_alpha_arr, 9, _X_posterior_horizontal(5)*_unit_a); // shear strength
float heading = atan2f(_X_posterior_horizontal(0),
_X_posterior_horizontal(1));
}
// =====================================================
// get the correct shear params for trajectory selection
// =====================================================
float shear = sqrtf(powf(_X_posterior_horizontal(0) * _unit_v, 2) +
powf(_X_posterior_horizontal(1) * _unit_v, 2));
float v = _findClosest(_v_max_arr, 5, shear); // wind velocity
float a = _findClosest(_alpha_arr, 9, _X_posterior_horizontal(5) * _unit_a); // shear strength
float heading = atan2f(_X_posterior_horizontal(0),
_X_posterior_horizontal(1));
// only update all params for trajectory selection if required to do so
if (!_lock_params) {
// get the appropriate airspeed for trajectory selection
float min_aspd = _MIN_ASPD_MATRIX(round(v-8.f),round(10.f*(a-0.2f)));
float max_aspd = _MAX_ASPD_MATRIX(round(v-8.f),round(10.f*(a-0.2f)));
uint num = (uint)round((max_aspd-min_aspd)/2.f + 1.f);
float aspd_arr[num] = {};
for (uint i=0; i<num; i++) {
aspd_arr[i] = min_aspd + 2.f;
}
float aspd = _findClosest(aspd_arr, num, _current_airspeed);
// update all shear params for trajectory selection
_soaring_estimator_shear.v_max = v;
_soaring_estimator_shear.alpha = a;
_soaring_estimator_shear.psi = heading;
_soaring_estimator_shear.h_ref = _X_posterior_horizontal(4)*_unit_h;
_soaring_estimator_shear.aspd = aspd;
// update the locked params for the future
_v_max_lock = v;
_alpha_lock = a;
}
// if not, we still need to find an appropriate airspeed
else {
float min_aspd = _MIN_ASPD_MATRIX(round(_v_max_lock-8.f),round(10.f*(_alpha_lock-0.2f)));
float max_aspd = _MAX_ASPD_MATRIX(round(_v_max_lock-8.f),round(10.f*(_alpha_lock-0.2f)));
uint num = (uint)round((max_aspd-min_aspd)/2.f + 1.f);
float aspd_arr[num] = {};
for (uint i=0; i<num; i++) {
aspd_arr[i] = min_aspd + 2.f;
}
float aspd = _findClosest(aspd_arr, num, _current_airspeed);
// only update airspeed for trajectory selection
_soaring_estimator_shear.aspd = aspd;
}
//PX4_INFO("Unlocked shear estimator, current airspeed \t%.1f", (double)_current_airspeed);
// only update all params for trajectory selection if required to do so
if (!_lock_params) {
// get the appropriate airspeed for trajectory selection
float min_aspd = _MIN_ASPD_MATRIX(round(v - 8.f), round(10.f * (a - 0.2f)));
float max_aspd = _MAX_ASPD_MATRIX(round(v - 8.f), round(10.f * (a - 0.2f)));
uint num = (uint)round((max_aspd - min_aspd) / 2.f + 1.f);
float aspd_arr[num] = {};
// publish shear params
// ========================================
// publish controller position in ENU frame
// ========================================
_soaring_estimator_shear.timestamp = hrt_absolute_time();
_soaring_estimator_shear.vx = _X_posterior_horizontal(0)*_unit_v;
_soaring_estimator_shear.vy = _X_posterior_horizontal(1)*_unit_v;
_soaring_estimator_shear.bx = _X_posterior_horizontal(2)*_unit_v;
_soaring_estimator_shear.by = _X_posterior_horizontal(3)*_unit_v;
_soaring_estimator_shear.h = _X_posterior_horizontal(4)*_unit_h;
_soaring_estimator_shear.a = _X_posterior_horizontal(5)*_unit_a;
_soaring_estimator_shear.sigma_vx = sqrtf(_P_posterior_horizontal(0,0))*_unit_v;
_soaring_estimator_shear.sigma_vy = sqrtf(_P_posterior_horizontal(1,1))*_unit_v;
_soaring_estimator_shear.sigma_bx = sqrtf(_P_posterior_horizontal(2,2))*_unit_v;
_soaring_estimator_shear.sigma_by = sqrtf(_P_posterior_horizontal(3,3))*_unit_v;
_soaring_estimator_shear.sigma_h = sqrtf(_P_posterior_horizontal(4,4))*_unit_h;
_soaring_estimator_shear.sigma_a = sqrtf(_P_posterior_horizontal(5,5))*_unit_a;
_soaring_estimator_shear.coeff_0 = _X_posterior_vertical(0);
_soaring_estimator_shear.soaring_feasible = check_feasibility();
_soaring_estimator_shear.reset_counter = _reset_counter;
_soaring_estimator_shear_pub.publish(_soaring_estimator_shear);
}
for (uint i = 0; i < num; i++) {
aspd_arr[i] = min_aspd + 2.f;
}
float aspd = _findClosest(aspd_arr, num, _current_airspeed);
// update all shear params for trajectory selection
_soaring_estimator_shear.v_max = v;
_soaring_estimator_shear.alpha = a;
_soaring_estimator_shear.psi = heading;
_soaring_estimator_shear.h_ref = _X_posterior_horizontal(4) * _unit_h;
_soaring_estimator_shear.aspd = aspd;
// update the locked params for the future
_v_max_lock = v;
_alpha_lock = a;
}
// if not, we still need to find an appropriate airspeed
else {
float min_aspd = _MIN_ASPD_MATRIX(round(_v_max_lock - 8.f), round(10.f * (_alpha_lock - 0.2f)));
float max_aspd = _MAX_ASPD_MATRIX(round(_v_max_lock - 8.f), round(10.f * (_alpha_lock - 0.2f)));
uint num = (uint)round((max_aspd - min_aspd) / 2.f + 1.f);
float aspd_arr[num] = {};
for (uint i = 0; i < num; i++) {
aspd_arr[i] = min_aspd + 2.f;
}
float aspd = _findClosest(aspd_arr, num, _current_airspeed);
// only update airspeed for trajectory selection
_soaring_estimator_shear.aspd = aspd;
}
//PX4_INFO("Unlocked shear estimator, current airspeed \t%.1f", (double)_current_airspeed);
// publish shear params
// ========================================
// publish controller position in ENU frame
// ========================================
_soaring_estimator_shear.timestamp = hrt_absolute_time();
_soaring_estimator_shear.vx = _X_posterior_horizontal(0) * _unit_v;
_soaring_estimator_shear.vy = _X_posterior_horizontal(1) * _unit_v;
_soaring_estimator_shear.bx = _X_posterior_horizontal(2) * _unit_v;
_soaring_estimator_shear.by = _X_posterior_horizontal(3) * _unit_v;
_soaring_estimator_shear.h = _X_posterior_horizontal(4) * _unit_h;
_soaring_estimator_shear.a = _X_posterior_horizontal(5) * _unit_a;
_soaring_estimator_shear.sigma_vx = sqrtf(_P_posterior_horizontal(0, 0)) * _unit_v;
_soaring_estimator_shear.sigma_vy = sqrtf(_P_posterior_horizontal(1, 1)) * _unit_v;
_soaring_estimator_shear.sigma_bx = sqrtf(_P_posterior_horizontal(2, 2)) * _unit_v;
_soaring_estimator_shear.sigma_by = sqrtf(_P_posterior_horizontal(3, 3)) * _unit_v;
_soaring_estimator_shear.sigma_h = sqrtf(_P_posterior_horizontal(4, 4)) * _unit_h;
_soaring_estimator_shear.sigma_a = sqrtf(_P_posterior_horizontal(5, 5)) * _unit_a;
_soaring_estimator_shear.coeff_0 = _X_posterior_vertical(0);
_soaring_estimator_shear.soaring_feasible = check_feasibility();
_soaring_estimator_shear.reset_counter = _reset_counter;
_soaring_estimator_shear_pub.publish(_soaring_estimator_shear);
}
}
bool FixedwingShearEstimator::check_feasibility()
{
// ================================================================
// simple check, if dynamic soaring is feasible in these conditions
// ================================================================
// ================================================================
// simple check, if dynamic soaring is feasible in these conditions
// ================================================================
float shear_x = _X_posterior_horizontal(0) - _X_posterior_horizontal(2);
float shear_y = _X_posterior_horizontal(1) - _X_posterior_horizontal(3);
float shear_strength = _X_posterior_horizontal(5);
float shear_x = _X_posterior_horizontal(0) - _X_posterior_horizontal(2);
float shear_y = _X_posterior_horizontal(1) - _X_posterior_horizontal(3);
float shear_strength = _X_posterior_horizontal(5);
// require shear strength above 8 m/s
float shear = sqrtf(powf(shear_x,2) + powf(shear_y,2));
if (shear<8.f) {
return false;
}
// require shear strength above 8 m/s
float shear = sqrtf(powf(shear_x, 2) + powf(shear_y, 2));
// require shear strength above 0.2 1/s
if (shear_strength<0.2f) {
return false;
}
if (shear < 8.f) {
return false;
}
// require covariance certainty below 2 m/s (use propagation of variance)
float shear_stdev = sqrtf(powf(shear_x/shear,2)*_P_posterior_horizontal(0,0) +
powf(shear_x/shear,2)*_P_posterior_horizontal(2,2) +
powf(shear_y/shear,2)*_P_posterior_horizontal(1,1) +
powf(shear_y/shear,2)*_P_posterior_horizontal(3,3));
if (shear_stdev>3.f) {
return false;
}
// require shear strength above 0.2 1/s
if (shear_strength < 0.2f) {
return false;
}
return true;
// require covariance certainty below 2 m/s (use propagation of variance)
float shear_stdev = sqrtf(powf(shear_x / shear, 2) * _P_posterior_horizontal(0, 0) +
powf(shear_x / shear, 2) * _P_posterior_horizontal(2, 2) +
powf(shear_y / shear, 2) * _P_posterior_horizontal(1, 1) +
powf(shear_y / shear, 2) * _P_posterior_horizontal(3, 3));
if (shear_stdev > 3.f) {
return false;
}
return true;
}
float
FixedwingShearEstimator::_getClosest(float val1, float val2, float target)
{
// =============================================================================
// METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/"
// =============================================================================
if (target - val1 >= val2 - target)
return val2;
else
return val1;
// =============================================================================
// METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/"
// =============================================================================
if (target - val1 >= val2 - target) {
return val2;
} else {
return val1;
}
}
float
FixedwingShearEstimator::_findClosest(float arr[], int n, float target)
{
// =============================================================================
// METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/"
// =============================================================================
// Corner cases
if (target <= arr[0])
return arr[0];
if (target >= arr[n - 1])
return arr[n - 1];
// Doing binary search
int i = 0, j = n, mid = 0;
while (i < j) {
mid = (i + j) / 2;
if ((float)fabs(arr[mid]-target) < (float)0.0001f)
return arr[mid];
/* If target is less than array element,
then search in left */
if (target < arr[mid]) {
// If target is greater than previous
// to mid, return closest of two
if (mid > 0 && target > arr[mid - 1])
return _getClosest(arr[mid - 1],
arr[mid], target);
/* Repeat for left half */
j = mid;
}
// If target is greater than mid
else {
if (mid < n - 1 && target < arr[mid + 1])
return _getClosest(arr[mid],
arr[mid + 1], target);
// update i
i = mid + 1;
}
}
// Only single element left after search
return arr[mid];
// =============================================================================
// METHOD COPIED FROM "https://www.geeksforgeeks.org/find-closest-number-array/"
// =============================================================================
// Corner cases
if (target <= arr[0]) {
return arr[0];
}
if (target >= arr[n - 1]) {
return arr[n - 1];
}
// Doing binary search
int i = 0, j = n, mid = 0;
while (i < j) {
mid = (i + j) / 2;
if ((float)fabs(arr[mid] - target) < (float)0.0001f) {
return arr[mid];
}
/* If target is less than array element,
then search in left */
if (target < arr[mid]) {
// If target is greater than previous
// to mid, return closest of two
if (mid > 0 && target > arr[mid - 1])
return _getClosest(arr[mid - 1],
arr[mid], target);
/* Repeat for left half */
j = mid;
}
// If target is greater than mid
else {
if (mid < n - 1 && target < arr[mid + 1])
return _getClosest(arr[mid],
arr[mid + 1], target);
// update i
i = mid + 1;
}
}
// Only single element left after search
return arr[mid];
}
@@ -608,10 +641,11 @@ int FixedwingShearEstimator::task_spawn(int argc, char *argv[])
int FixedwingShearEstimator::custom_command(int argc, char *argv[])
{
if (!strcmp(argv[0], "reset")) {
get_instance()->reset_filter();
if (!strcmp(argv[0], "reset")) {
get_instance()->reset_filter();
return 0;
}
return print_usage("unknown command");
}
@@ -630,7 +664,7 @@ int FixedwingShearEstimator::print_usage(const char *reason)
PRINT_MODULE_USAGE_NAME("fw_dyn_soar_estimator", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
PRINT_MODULE_USAGE_COMMAND("reset");
PRINT_MODULE_USAGE_COMMAND("reset");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp
+59 -59
View File
@@ -66,90 +66,90 @@ private:
uORB::SubscriptionCallbackWorkItem _soaring_controller_wind_sub{this, ORB_ID(soaring_controller_wind)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
// Publishers
// Publishers
uORB::Publication<soaring_estimator_shear_s> _soaring_estimator_shear_pub{ORB_ID(soaring_estimator_shear)};
// Message structs
soaring_estimator_shear_s _soaring_estimator_shear{}; ///< soaring controller pos
// Message structs
soaring_estimator_shear_s _soaring_estimator_shear{}; ///< soaring controller pos
soaring_controller_wind_s _soaring_controller_wind{}; ///< soaring controller wind
// parameter struct
DEFINE_PARAMETERS(
// aircraft params
(ParamFloat<px4::params::DS_SIGMA_Q_V>) _param_sigma_q_vel,
(ParamFloat<px4::params::DS_SIGMA_Q_H>) _param_sigma_q_h,
(ParamFloat<px4::params::DS_SIGMA_Q_A>) _param_sigma_q_a,
(ParamFloat<px4::params::DS_SIGMA_R_V>) _param_sigma_r_vel,
(ParamFloat<px4::params::DS_INIT_H>) _param_init_h
(ParamFloat<px4::params::DS_SIGMA_Q_H>) _param_sigma_q_h,
(ParamFloat<px4::params::DS_SIGMA_Q_A>) _param_sigma_q_a,
(ParamFloat<px4::params::DS_SIGMA_R_V>) _param_sigma_r_vel,
(ParamFloat<px4::params::DS_INIT_H>) _param_init_h
)
perf_counter_t _loop_perf; ///< loop performance counter
// Update our local parameter cache.
int parameters_update();
void reset_filter();
void perform_prior_update();
void perform_posterior_update(float height, Vector3f wind);
bool check_feasibility();
void publish_estimate();
void status_publish();
void reset_filter();
void perform_prior_update();
void perform_posterior_update(float height, Vector3f wind);
bool check_feasibility();
void publish_estimate();
void status_publish();
// helper methods
// helper methods
float _getClosest(float val1, float val2, float taget); // get float closest to target
float _findClosest(float arr[], int n, float target); // return element in arr closest to n
// control variables
hrt_abstime _last_run{0};
uint _reset_counter = {};
const static size_t _dim_vertical = 1; // order of vertical approximation function for vertical wind
// control variables
hrt_abstime _last_run{0};
uint _reset_counter = {};
const static size_t _dim_vertical = 1; // order of vertical approximation function for vertical wind
Vector<float, 6> _X_prior_horizontal= {};
Matrix<float, 6, 6> _P_prior_horizontal = {};
Vector<float, 6> _X_posterior_horizontal= {};
Matrix<float, 6, 6> _P_posterior_horizontal = {};
Matrix<float, 6, 6> _Q_horizontal = {};
Matrix<float, 2, 2> _R_horizontal = {};
Matrix<float, 2, 6> _H_horizontal = {};
Matrix<float, 6, 6> _A_horizontal = {};
Matrix<float, 6, 2> _K_horizontal = {};
Vector<float, 6> _X_prior_horizontal = {};
Matrix<float, 6, 6> _P_prior_horizontal = {};
Vector<float, 6> _X_posterior_horizontal = {};
Matrix<float, 6, 6> _P_posterior_horizontal = {};
Matrix<float, 6, 6> _Q_horizontal = {};
Matrix<float, 2, 2> _R_horizontal = {};
Matrix<float, 2, 6> _H_horizontal = {};
Matrix<float, 6, 6> _A_horizontal = {};
Matrix<float, 6, 2> _K_horizontal = {};
Vector<float, _dim_vertical> _X_prior_vertical= {};
Matrix<float, _dim_vertical, _dim_vertical> _P_prior_vertical = {};
Vector<float, _dim_vertical> _X_posterior_vertical= {};
Matrix<float, _dim_vertical, _dim_vertical> _P_posterior_vertical = {};
Vector<float, _dim_vertical> _X_vertical = {}; // params of vertical wind
Matrix<float, _dim_vertical, _dim_vertical> _Q_vertical = {};
Matrix<float, 1, 1> _R_vertical = {};
Matrix<float, 1, _dim_vertical> _H_vertical = {};
Matrix<float, _dim_vertical, _dim_vertical> _A_vertical = {};
Matrix<float, _dim_vertical, 1> _K_vertical = {};
Vector<float, _dim_vertical> _X_prior_vertical = {};
Matrix<float, _dim_vertical, _dim_vertical> _P_prior_vertical = {};
Vector<float, _dim_vertical> _X_posterior_vertical = {};
Matrix<float, _dim_vertical, _dim_vertical> _P_posterior_vertical = {};
Vector<float, _dim_vertical> _X_vertical = {}; // params of vertical wind
Matrix<float, _dim_vertical, _dim_vertical> _Q_vertical = {};
Matrix<float, 1, 1> _R_vertical = {};
Matrix<float, 1, _dim_vertical> _H_vertical = {};
Matrix<float, _dim_vertical, _dim_vertical> _A_vertical = {};
Matrix<float, _dim_vertical, 1> _K_vertical = {};
// measurement variables
Vector3f _current_wind = {};
float _current_height = {};
float _current_airspeed = {};
bool _lock_params = {};
float _v_max_lock = {};
float _alpha_lock = {};
// measurement variables
Vector3f _current_wind = {};
float _current_height = {};
float _current_airspeed = {};
bool _lock_params = {};
float _v_max_lock = {};
float _alpha_lock = {};
float _unit_v;
float _unit_h;
float _unit_a;
float _unit_v;
float _unit_h;
float _unit_a;
// helper variables
float _init_height = {};
// helper variables
float _init_height = {};
// trajectory selection variables
// vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength
float _v_max_arr[5] = {8.f,9.f,10.f,11.f,12.f};
float _alpha_arr[9] = {0.2f,0.3f,0.4f,0.5f,0.6f,0.7f,0.8f,0.9f,1.0f};
const static size_t _num_v_max = 5;
const static size_t _num_alpha = 9;
Matrix<int, _num_v_max, _num_alpha> _MIN_ASPD_MATRIX = {};
Matrix<int, _num_v_max, _num_alpha> _MAX_ASPD_MATRIX = {};
// trajectory selection variables
// vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength
float _v_max_arr[5] = {8.f, 9.f, 10.f, 11.f, 12.f};
float _alpha_arr[9] = {0.2f, 0.3f, 0.4f, 0.5f, 0.6f, 0.7f, 0.8f, 0.9f, 1.0f};
const static size_t _num_v_max = 5;
const static size_t _num_alpha = 9;
Matrix<int, _num_v_max, _num_alpha> _MIN_ASPD_MATRIX = {};
Matrix<int, _num_v_max, _num_alpha> _MAX_ASPD_MATRIX = {};
};
src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
+10 -10
View File
@@ -12,9 +12,9 @@
/**
* Standard deviation of velicity state in shear model
*
*
* This is the std dev of the wind velocity in each direction
*
*
* @unit m
* @min 0.0
* @max 10
@@ -26,9 +26,9 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.3f);
/**
* Standard deviation of vertical shear position
*
*
* This is the std dev of the shear vertical position
*
*
* @unit m
* @min 0.0
* @max 10
@@ -40,9 +40,9 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.3f);
/**
* Standard deviation of velicity state in shear model
*
*
* This is the std dev of the shear strenght param
*
*
* @unit m
* @min 0.0
* @max 10
@@ -54,9 +54,9 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.01f);
/**
* Standard deviation of velicity measurement (wind)
*
*
* This is the std dev of the wind pseudomeasurement passed to the EKF
*
*
* @unit m
* @min 0.0
* @max 10
@@ -68,9 +68,9 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 3.f);
/**
* Initial guess of the vertical shear position
*
*
* This is the filter initial value of the shear vertical position
*
*
* @unit m
* @min -200
* @max 200