mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-12 03:00:04 +08:00
Compare commits
25 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 717ee531cb | |||
| b405d75553 | |||
| 4e3bd4f196 | |||
| 0cc4b41a51 | |||
| f602228048 | |||
| 9b122adae4 | |||
| 1ec0ba4736 | |||
| 8da8b88a54 | |||
| be08c57a0a | |||
| a436a8f3b8 | |||
| 5ad0e68d8e | |||
| f07eeaa776 | |||
| 506c60c471 | |||
| 643d3e3bf3 | |||
| 8243b4f474 | |||
| 22b957696d | |||
| c338891677 | |||
| c4c41c49e5 | |||
| 021dd0d0af | |||
| c221da27a7 | |||
| 51fe4351c6 | |||
| 8a75733511 | |||
| 1032dd3470 | |||
| 424c3cd2cb | |||
| 68100650da |
@@ -0,0 +1,80 @@
|
||||
#!/bin/sh
|
||||
# @name Gazebo lawnmower
|
||||
# @type Rover
|
||||
# @class Rover
|
||||
|
||||
. ${R}etc/init.d/rc.rover_differential_defaults
|
||||
|
||||
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
|
||||
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
|
||||
PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}
|
||||
|
||||
param set-default SIM_GZ_EN 1 # Gazebo bridge
|
||||
|
||||
# Simulated sensors
|
||||
param set-default SENS_EN_GPSSIM 1
|
||||
param set-default SENS_EN_BAROSIM 0
|
||||
param set-default SENS_EN_MAGSIM 1
|
||||
param set-default SENS_EN_ARSPDSIM 1
|
||||
# We can arm and drive in manual mode when it slides and GPS check fails:
|
||||
param set-default COM_ARM_WO_GPS 1
|
||||
|
||||
# Set Differential Drive Kinematics Library parameters:
|
||||
param set RDD_WHEEL_BASE 0.9
|
||||
param set RDD_WHEEL_RADIUS 0.22
|
||||
param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h
|
||||
|
||||
# Actuator mapping - set SITL motors/servos output parameters:
|
||||
|
||||
# "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
|
||||
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
|
||||
#param set-default SIM_GZ_WH_MIN1 0
|
||||
#param set-default SIM_GZ_WH_MAX1 200
|
||||
#param set-default SIM_GZ_WH_DIS1 100
|
||||
#param set-default SIM_GZ_WH_FAIL1 100
|
||||
|
||||
param set-default SIM_GZ_WH_FUNC2 102 # left wheel
|
||||
#param set-default SIM_GZ_WH_MIN2 0
|
||||
#param set-default SIM_GZ_WH_MAX2 200
|
||||
#aram set-default SIM_GZ_WH_DIS2 100
|
||||
#param set-default SIM_GZ_WH_FAIL2 100
|
||||
|
||||
param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
|
||||
|
||||
# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation.
|
||||
# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate
|
||||
# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator
|
||||
# controls in practical scenarios.
|
||||
|
||||
# Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
|
||||
param set-default SIM_GZ_SV_FUNC3 203
|
||||
param set-default SIM_GZ_SV_MIN3 0
|
||||
param set-default SIM_GZ_SV_MAX3 1000
|
||||
param set-default SIM_GZ_SV_DIS3 500
|
||||
param set-default SIM_GZ_SV_FAIL3 500
|
||||
|
||||
# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
|
||||
# - on minimum when disarmed or failed:
|
||||
param set-default SIM_GZ_SV_FUNC4 204
|
||||
param set-default SIM_GZ_SV_MIN4 0
|
||||
param set-default SIM_GZ_SV_MAX4 1000
|
||||
param set-default SIM_GZ_SV_DIS4 500
|
||||
param set-default SIM_GZ_SV_FAIL4 500
|
||||
|
||||
# Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:
|
||||
|
||||
# Strobes, PCA9685 servo channel 5, "Servo 5" (205) - flashing indicates Mission mode:
|
||||
#param set-default SIM_GZ_SV_FUNC5 205
|
||||
#param set-default SIM_GZ_SV_MIN5 1000
|
||||
#param set-default SIM_GZ_SV_MAX5 2000
|
||||
#param set-default SIM_GZ_SV_DIS5 1000
|
||||
#param set-default SIM_GZ_SV_FAIL5 1000
|
||||
|
||||
# Horn, PCA9685 servo channel 6, "Servo 6" (206) - for alarms like GPS failure:
|
||||
#param set-default SIM_GZ_SV_FUNC6 206
|
||||
|
||||
# Spare PCA9685 servo channel 7 on "RC AUX2" (408) - right knob, or "Servo 7" (207):
|
||||
#param set-default SIM_GZ_SV_FUNC7 207
|
||||
|
||||
# Spare PCA9685 servo channel 8 - "Servo 8" (208):
|
||||
#param set-default SIM_GZ_SV_FUNC8 208
|
||||
@@ -82,6 +82,7 @@ px4_add_romfs_files(
|
||||
4008_gz_advanced_plane
|
||||
4009_gz_r1_rover
|
||||
4010_gz_x500_mono_cam
|
||||
4011_gz_lawnmower
|
||||
|
||||
6011_gazebo-classic_typhoon_h480
|
||||
6011_gazebo-classic_typhoon_h480.post
|
||||
|
||||
@@ -164,6 +164,10 @@ param set-default COM_RC_IN_MODE 1
|
||||
# Speedup SITL startup
|
||||
param set-default EKF2_REQ_GPS_H 0.5
|
||||
|
||||
# Multi-EKF
|
||||
param set-default EKF2_MULTI_IMU 3
|
||||
param set-default SENS_IMU_MODE 0
|
||||
|
||||
param set-default IMU_GYRO_FFT_EN 1
|
||||
param set-default MAV_PROTO_VER 2 # Ensures QGC does not drop the first few packets after a SITL restart due to MAVLINK 1 packets
|
||||
|
||||
|
||||
+1
-1
Submodule Tools/simulation/gz updated: f1c461fffb...2228336568
@@ -10,7 +10,6 @@ CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
|
||||
CONFIG_DRIVERS_QSHELL_QURT=y
|
||||
CONFIG_DRIVERS_VOXL2_IO=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
@@ -28,4 +27,5 @@ CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
|
||||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_ORB_COMMUNICATOR=y
|
||||
|
||||
@@ -49,6 +49,6 @@ add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)
|
||||
|
||||
@@ -989,10 +989,10 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
||||
|
||||
gps.device_id = device_id.devid;
|
||||
|
||||
gps.lat = hil_gps.lat;
|
||||
gps.lon = hil_gps.lon;
|
||||
gps.alt = hil_gps.alt;
|
||||
gps.alt_ellipsoid = hil_gps.alt;
|
||||
gps.latitude_deg = hil_gps.lat;
|
||||
gps.longitude_deg = hil_gps.lon;
|
||||
gps.altitude_msl_m = hil_gps.alt;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt;
|
||||
|
||||
gps.s_variance_m_s = 0.25f;
|
||||
gps.c_variance_rad = 0.5f;
|
||||
|
||||
@@ -173,6 +173,7 @@ set(msg_files
|
||||
RegisterExtComponentReply.msg
|
||||
RegisterExtComponentRequest.msg
|
||||
Rpm.msg
|
||||
RtlStatus.msg
|
||||
RtlTimeEstimate.msg
|
||||
SatelliteInfo.msg
|
||||
SensorAccel.msg
|
||||
|
||||
@@ -55,15 +55,9 @@ bool fs_bad_airspeed # 5 - true if fusion of the airspeed has encounte
|
||||
bool fs_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
|
||||
bool fs_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error
|
||||
bool fs_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error
|
||||
bool fs_bad_vel_n # 9 - true if fusion of the North velocity has encountered a numerical error
|
||||
bool fs_bad_vel_e # 10 - true if fusion of the East velocity has encountered a numerical error
|
||||
bool fs_bad_vel_d # 11 - true if fusion of the Down velocity has encountered a numerical error
|
||||
bool fs_bad_pos_n # 12 - true if fusion of the North position has encountered a numerical error
|
||||
bool fs_bad_pos_e # 13 - true if fusion of the East position has encountered a numerical error
|
||||
bool fs_bad_pos_d # 14 - true if fusion of the Down position has encountered a numerical error
|
||||
bool fs_bad_acc_bias # 15 - true if bad delta velocity bias estimates have been detected
|
||||
bool fs_bad_acc_vertical # 16 - true if bad vertical accelerometer data has been detected
|
||||
bool fs_bad_acc_clipping # 17 - true if delta velocity data contains clipping (asymmetric railing)
|
||||
bool fs_bad_acc_bias # 9 - true if bad delta velocity bias estimates have been detected
|
||||
bool fs_bad_acc_vertical # 10 - true if bad vertical accelerometer data has been detected
|
||||
bool fs_bad_acc_clipping # 11 - true if delta velocity data contains clipping (asymmetric railing)
|
||||
|
||||
|
||||
# innovation test failures
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint32 safe_points_id # unique ID of active set of safe_point_items
|
||||
bool is_evaluation_pending # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading).
|
||||
|
||||
bool has_vtol_approach # flag if approaches are defined for current RTL_TYPE parameter setting
|
||||
|
||||
uint8 rtl_type # Type of RTL chosen
|
||||
uint8 safe_point_index # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode
|
||||
|
||||
uint8 RTL_STATUS_TYPE_NONE=0 # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
|
||||
uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position
|
||||
uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing
|
||||
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # RTL type is following the mission from closest point to mission landing
|
||||
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position
|
||||
@@ -48,6 +48,8 @@
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdbool.h>
|
||||
#include <syslog.h>
|
||||
@@ -57,7 +59,6 @@
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
****************************************************************************/
|
||||
#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
|
||||
|
||||
typedef struct {
|
||||
hw_base_id_t hw_base_id; /* The ID of the Base */
|
||||
|
||||
@@ -40,15 +40,34 @@
|
||||
#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
#if defined(CONFIG_MODULES_MUORB_APPS)
|
||||
extern "C" { int muorb_init(); }
|
||||
#endif
|
||||
|
||||
int px4_platform_init(void)
|
||||
{
|
||||
hrt_init();
|
||||
|
||||
px4::WorkQueueManagerStart();
|
||||
|
||||
// MUORB has slightly different startup requirements
|
||||
#if defined(CONFIG_MODULES_MUORB_APPS)
|
||||
//Put sleeper in here to allow wq to finish initializing before param_init is called
|
||||
usleep(10000);
|
||||
|
||||
uorb_start();
|
||||
|
||||
muorb_init();
|
||||
|
||||
// Give muorb some time to setup the DSP
|
||||
usleep(100000);
|
||||
|
||||
param_init();
|
||||
#else
|
||||
param_init();
|
||||
|
||||
uorb_start();
|
||||
#endif
|
||||
|
||||
px4_log_initialize();
|
||||
|
||||
|
||||
@@ -45,6 +45,7 @@ px4_add_module(
|
||||
voxl2_io.cpp
|
||||
voxl2_io.hpp
|
||||
DEPENDS
|
||||
rc
|
||||
px4_work_queue
|
||||
mixer_module
|
||||
MODULE_CONFIG
|
||||
|
||||
@@ -127,7 +127,8 @@ list(APPEND EKF_SRCS
|
||||
EKF/height_control.cpp
|
||||
EKF/imu_down_sampler.cpp
|
||||
EKF/output_predictor.cpp
|
||||
EKF/vel_pos_fusion.cpp
|
||||
EKF/velocity_fusion.cpp
|
||||
EKF/position_fusion.cpp
|
||||
EKF/yaw_fusion.cpp
|
||||
EKF/zero_innovation_heading_update.cpp
|
||||
|
||||
|
||||
@@ -44,7 +44,8 @@ list(APPEND EKF_SRCS
|
||||
height_control.cpp
|
||||
imu_down_sampler.cpp
|
||||
output_predictor.cpp
|
||||
vel_pos_fusion.cpp
|
||||
velocity_fusion.cpp
|
||||
position_fusion.cpp
|
||||
yaw_fusion.cpp
|
||||
zero_innovation_heading_update.cpp
|
||||
|
||||
|
||||
@@ -66,7 +66,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
const float innovation = ekf.state().vel(i) - vel_obs(i);
|
||||
ekf.fuseVelPosHeight(innovation, innov_var(i), State::vel.idx + i);
|
||||
ekf.fuseDirectStateMeasurement(innovation, innov_var(i), State::vel.idx + i);
|
||||
}
|
||||
|
||||
_time_last_zero_velocity_fuse = imu_delayed.time_us;
|
||||
|
||||
@@ -42,10 +42,10 @@ void Ekf::controlAuxVelFusion()
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_aux_vel);
|
||||
|
||||
updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
|
||||
updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
|
||||
|
||||
if (isHorizontalAidingActive()) {
|
||||
fuseVelocity(_aid_src_aux_vel);
|
||||
fuseHorizontalVelocity(_aid_src_aux_vel);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -509,15 +509,9 @@ union fault_status_u {
|
||||
bool bad_sideslip : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
|
||||
bool bad_optflow_X : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
|
||||
bool bad_optflow_Y : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
|
||||
bool bad_vel_N : 1; ///< 9 - true if fusion of the North velocity has encountered a numerical error
|
||||
bool bad_vel_E : 1; ///< 10 - true if fusion of the East velocity has encountered a numerical error
|
||||
bool bad_vel_D : 1; ///< 11 - true if fusion of the Down velocity has encountered a numerical error
|
||||
bool bad_pos_N : 1; ///< 12 - true if fusion of the North position has encountered a numerical error
|
||||
bool bad_pos_E : 1; ///< 13 - true if fusion of the East position has encountered a numerical error
|
||||
bool bad_pos_D : 1; ///< 14 - true if fusion of the Down position has encountered a numerical error
|
||||
bool bad_acc_bias : 1; ///< 15 - true if bad delta velocity bias estimates have been detected
|
||||
bool bad_acc_vertical : 1; ///< 16 - true if bad vertical accelerometer data has been detected
|
||||
bool bad_acc_clipping : 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing)
|
||||
bool bad_acc_bias : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
|
||||
bool bad_acc_vertical : 1; ///< 10 - true if bad vertical accelerometer data has been detected
|
||||
bool bad_acc_clipping : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing)
|
||||
} flags;
|
||||
uint32_t value;
|
||||
};
|
||||
|
||||
@@ -327,8 +327,8 @@ public:
|
||||
#endif
|
||||
}
|
||||
|
||||
// fuse single velocity and position measurement
|
||||
bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index);
|
||||
// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
|
||||
bool fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index);
|
||||
|
||||
// gyro bias
|
||||
const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
|
||||
@@ -477,7 +477,7 @@ public:
|
||||
|
||||
for (unsigned row = 0; row < State::size; row++) {
|
||||
for (unsigned col = 0; col < State::size; col++) {
|
||||
// Instad of literally computing KHP, use an equvalent
|
||||
// Instead of literally computing KHP, use an equivalent
|
||||
// equation involving less mathematical operations
|
||||
KHP(row, col) = KS(row) * K(col);
|
||||
}
|
||||
@@ -828,7 +828,7 @@ private:
|
||||
void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;
|
||||
|
||||
// 2d & 3d velocity aid source
|
||||
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
|
||||
void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
|
||||
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const;
|
||||
|
||||
// horizontal and vertical position fusion
|
||||
@@ -836,7 +836,7 @@ private:
|
||||
void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
|
||||
|
||||
// 2d & 3d velocity fusion
|
||||
void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
|
||||
void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src);
|
||||
void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
@@ -1137,8 +1137,6 @@ private:
|
||||
void resetFakePosFusion();
|
||||
void stopFakePosFusion();
|
||||
|
||||
void setVelPosStatus(const int state_index, const bool healthy);
|
||||
|
||||
// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
|
||||
// yaw : Euler yaw angle (rad)
|
||||
// yaw_variance : yaw error variance (rad^2)
|
||||
|
||||
@@ -45,121 +45,6 @@
|
||||
#include <lib/world_magnetic_model/geo_mag_declination.h>
|
||||
#include <cstdlib>
|
||||
|
||||
void Ekf::resetHorizontalVelocityToZero()
|
||||
{
|
||||
_information_events.flags.reset_vel_to_zero = true;
|
||||
ECL_INFO("reset velocity to zero");
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f);
|
||||
}
|
||||
|
||||
void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
|
||||
{
|
||||
resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
|
||||
resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)
|
||||
{
|
||||
const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
|
||||
_state.vel.xy() = new_horz_vel;
|
||||
|
||||
if (PX4_ISFINITE(new_horz_vel_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(new_horz_vel_var(1))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
|
||||
}
|
||||
|
||||
_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
|
||||
_state_reset_status.velNE_change = delta_horz_vel;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.velNE_change += delta_horz_vel;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.velNE++;
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_vel_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
|
||||
{
|
||||
const float delta_vert_vel = new_vert_vel - _state.vel(2);
|
||||
_state.vel(2) = new_vert_vel;
|
||||
|
||||
if (PX4_ISFINITE(new_vert_vel_var)) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
|
||||
}
|
||||
|
||||
_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
|
||||
_state_reset_status.velD_change = delta_vert_vel;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.velD_change += delta_vert_vel;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.velD++;
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_ver_vel_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToLastKnown()
|
||||
{
|
||||
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
|
||||
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
|
||||
{
|
||||
const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
|
||||
_state.pos.xy() = new_horz_pos;
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(1))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
|
||||
}
|
||||
|
||||
_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
|
||||
_state_reset_status.posNE_change = delta_horz_pos;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posNE_change += delta_horz_pos;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posNE++;
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
bool Ekf::isHeightResetRequired() const
|
||||
{
|
||||
// check if height is continuously failing because of accel errors
|
||||
@@ -171,68 +56,6 @@ bool Ekf::isHeightResetRequired() const
|
||||
return (continuous_bad_accel_hgt || hgt_fusion_timeout);
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) {
|
||||
_information_events.flags.reset_pos_to_ext_obs = true;
|
||||
ECL_INFO("reset position to external observation");
|
||||
resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
|
||||
{
|
||||
const float old_vert_pos = _state.pos(2);
|
||||
_state.pos(2) = new_vert_pos;
|
||||
|
||||
if (PX4_ISFINITE(new_vert_pos_var)) {
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
|
||||
}
|
||||
|
||||
const float delta_z = new_vert_pos - old_vert_pos;
|
||||
|
||||
// apply the change in height / height rate to our newest height / height rate estimate
|
||||
// which have already been taken out from the output buffer
|
||||
_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
|
||||
_state_reset_status.posD_change = delta_z;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posD_change += delta_z;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posD++;
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
terrainHandleVerticalPositionReset(delta_z);
|
||||
#endif
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityToZero()
|
||||
{
|
||||
// we don't know what the vertical velocity is, so set it to zero
|
||||
// Set the variance to a value large enough to allow the state to converge quickly
|
||||
// that does not destabilise the filter
|
||||
resetVerticalVelocityTo(0.0f, 10.f);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
|
||||
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
|
||||
{
|
||||
@@ -684,7 +507,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
{
|
||||
ekf_solution_status_u soln_status{};
|
||||
// TODO: Is this accurate enough?
|
||||
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
|
||||
soln_status.flags.attitude = attitude_valid();
|
||||
soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
|
||||
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
|
||||
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
|
||||
@@ -1015,3 +838,39 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
|
||||
_accel_bias_inhibit[index] = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index)
|
||||
{
|
||||
VectorState Kfusion; // Kalman gain vector for any single observation - sequential fusion is used.
|
||||
|
||||
// calculate kalman gain K = PHS, where S = 1/innovation variance
|
||||
for (int row = 0; row < State::size; row++) {
|
||||
Kfusion(row) = P(row, state_index) / innov_var;
|
||||
}
|
||||
|
||||
clearInhibitedStateKalmanGains(Kfusion);
|
||||
|
||||
SquareMatrixState KHP;
|
||||
|
||||
for (unsigned row = 0; row < State::size; row++) {
|
||||
for (unsigned column = 0; column < State::size; column++) {
|
||||
KHP(row, column) = Kfusion(row) * P(state_index, column);
|
||||
}
|
||||
}
|
||||
|
||||
const bool healthy = checkAndFixCovarianceUpdate(KHP);
|
||||
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
P -= KHP;
|
||||
|
||||
constrainStateVariances();
|
||||
|
||||
// apply the state corrections
|
||||
fuse(Kfusion, innov);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -41,6 +41,7 @@
|
||||
void Ekf::controlExternalVisionFusion()
|
||||
{
|
||||
_ev_pos_b_est.predict(_dt_ekf_avg);
|
||||
_ev_hgt_b_est.predict(_dt_ekf_avg);
|
||||
|
||||
// Check for new external vision data
|
||||
extVisionSample ev_sample;
|
||||
|
||||
@@ -45,7 +45,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
|
||||
|
||||
HeightBiasEstimator &bias_est = _ev_hgt_b_est;
|
||||
|
||||
bias_est.predict(_dt_ekf_avg);
|
||||
// bias_est.predict(_dt_ekf_avg) called by controlExternalVisionFusion()
|
||||
|
||||
// correct position for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
||||
|
||||
@@ -227,35 +227,52 @@ bool Ekf::fuseDeclination(float decl_sigma)
|
||||
return false;
|
||||
}
|
||||
|
||||
// observation variance (rad**2)
|
||||
const float R_DECL = sq(decl_sigma);
|
||||
float decl_measurement = NAN;
|
||||
|
||||
VectorState H;
|
||||
float decl_pred;
|
||||
float innovation_variance;
|
||||
if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
|
||||
&& PX4_ISFINITE(_mag_declination_gps)
|
||||
) {
|
||||
decl_measurement = _mag_declination_gps;
|
||||
|
||||
// TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter?
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
|
||||
|
||||
const float innovation = wrap_pi(decl_pred - getMagDeclination());
|
||||
|
||||
if (innovation_variance < R_DECL) {
|
||||
// variance calculation is badly conditioned
|
||||
return false;
|
||||
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
|
||||
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
|
||||
) {
|
||||
decl_measurement = math::radians(_params.mag_declination_deg);
|
||||
}
|
||||
|
||||
// Calculate the Kalman gains
|
||||
VectorState Kfusion = P * H / innovation_variance;
|
||||
if (PX4_ISFINITE(decl_measurement)) {
|
||||
|
||||
const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);
|
||||
// observation variance (rad**2)
|
||||
const float R_DECL = sq(decl_sigma);
|
||||
|
||||
_fault_status.flags.bad_mag_decl = !is_fused;
|
||||
VectorState H;
|
||||
float decl_pred;
|
||||
float innovation_variance;
|
||||
|
||||
if (is_fused) {
|
||||
limitDeclination();
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
|
||||
|
||||
const float innovation = wrap_pi(decl_pred - decl_measurement);
|
||||
|
||||
if (innovation_variance < R_DECL) {
|
||||
// variance calculation is badly conditioned
|
||||
return false;
|
||||
}
|
||||
|
||||
// Calculate the Kalman gains
|
||||
VectorState Kfusion = P * H / innovation_variance;
|
||||
|
||||
const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);
|
||||
|
||||
_fault_status.flags.bad_mag_decl = !is_fused;
|
||||
|
||||
if (is_fused) {
|
||||
limitDeclination();
|
||||
}
|
||||
|
||||
return is_fused;
|
||||
}
|
||||
|
||||
return is_fused;
|
||||
return false;
|
||||
}
|
||||
|
||||
void Ekf::limitDeclination()
|
||||
@@ -274,7 +291,9 @@ void Ekf::limitDeclination()
|
||||
// set to 50% of the horizontal strength from geo tables if location is known
|
||||
h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
|
||||
|
||||
} else if (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) {
|
||||
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
|
||||
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
|
||||
) {
|
||||
// use parameter value if GPS isn't available
|
||||
decl_reference = math::radians(_params.mag_declination_deg);
|
||||
}
|
||||
|
||||
@@ -32,7 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vel_pos_fusion.cpp
|
||||
* @file optflow_fusion.cpp
|
||||
* Function for fusing gps and baro measurements/
|
||||
* equations generated using EKF/python/ekf_derivation/main.py
|
||||
*
|
||||
|
||||
@@ -0,0 +1,210 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ekf.h"
|
||||
|
||||
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
|
||||
const float innov_gate, estimator_aid_source2d_s &aid_src) const
|
||||
{
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
aid_src.observation[i] = obs(i);
|
||||
aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
|
||||
|
||||
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
|
||||
const int state_index = State::pos.idx + i;
|
||||
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
|
||||
const float innov_gate, estimator_aid_source1d_s &aid_src) const
|
||||
{
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
aid_src.observation = obs;
|
||||
aid_src.innovation = _state.pos(2) - aid_src.observation;
|
||||
|
||||
aid_src.observation_variance = math::max(sq(0.01f), obs_var);
|
||||
aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance;
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
// z special case if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
||||
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
|
||||
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
|
||||
aid_src.innovation_rejected = false;
|
||||
}
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
// x & y
|
||||
if (!aid_src.innovation_rejected
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx + 0)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
|
||||
{
|
||||
// z
|
||||
if (!aid_src.innovation_rejected
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
|
||||
{
|
||||
const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
|
||||
_state.pos.xy() = new_horz_pos;
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(1))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
|
||||
}
|
||||
|
||||
_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
|
||||
_state_reset_status.posNE_change = delta_horz_pos;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posNE_change += delta_horz_pos;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posNE++;
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
|
||||
{
|
||||
const float old_vert_pos = _state.pos(2);
|
||||
_state.pos(2) = new_vert_pos;
|
||||
|
||||
if (PX4_ISFINITE(new_vert_pos_var)) {
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
|
||||
}
|
||||
|
||||
const float delta_z = new_vert_pos - old_vert_pos;
|
||||
|
||||
// apply the change in height / height rate to our newest height / height rate estimate
|
||||
// which have already been taken out from the output buffer
|
||||
_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
|
||||
_state_reset_status.posD_change = delta_z;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posD_change += delta_z;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posD++;
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
terrainHandleVerticalPositionReset(delta_z);
|
||||
#endif
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToLastKnown()
|
||||
{
|
||||
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy)
|
||||
{
|
||||
ECL_INFO("reset position to external observation");
|
||||
_information_events.flags.reset_pos_to_ext_obs = true;
|
||||
|
||||
resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
|
||||
}
|
||||
@@ -1,303 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vel_pos_fusion.cpp
|
||||
*
|
||||
* @author Roman Bast <bapstroman@gmail.com>
|
||||
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
|
||||
* @author Paul Riseborough <p_riseborough@live.com.au>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include "ekf.h"
|
||||
|
||||
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
|
||||
const float innov_gate, estimator_aid_source2d_s &aid_src) const
|
||||
{
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
aid_src.observation[i] = obs(i);
|
||||
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
|
||||
|
||||
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
|
||||
const int state_index = State::vel.idx + i;
|
||||
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var,
|
||||
const float innov_gate, estimator_aid_source3d_s &aid_src) const
|
||||
{
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
aid_src.observation[i] = obs(i);
|
||||
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
|
||||
|
||||
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
|
||||
const int state_index = State::vel.idx + i;
|
||||
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
// vz special case if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
||||
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]);
|
||||
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
|
||||
aid_src.innovation_rejected = false;
|
||||
}
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
|
||||
const float innov_gate, estimator_aid_source1d_s &aid_src) const
|
||||
{
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
aid_src.observation = obs;
|
||||
aid_src.innovation = _state.pos(2) - aid_src.observation;
|
||||
|
||||
aid_src.observation_variance = math::max(sq(0.01f), obs_var);
|
||||
aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance;
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
// z special case if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
||||
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
|
||||
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
|
||||
aid_src.innovation_rejected = false;
|
||||
}
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
|
||||
const float innov_gate, estimator_aid_source2d_s &aid_src) const
|
||||
{
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
aid_src.observation[i] = obs(i);
|
||||
aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
|
||||
|
||||
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
|
||||
const int state_index = State::pos.idx + i;
|
||||
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
if (!aid_src.innovation_rejected) {
|
||||
// vx, vy
|
||||
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
|
||||
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
|
||||
{
|
||||
if (!aid_src.innovation_rejected) {
|
||||
// vx, vy, vz
|
||||
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
|
||||
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
|
||||
&& fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
// x & y
|
||||
if (!aid_src.innovation_rejected) {
|
||||
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
|
||||
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
|
||||
{
|
||||
// z
|
||||
if (!aid_src.innovation_rejected) {
|
||||
if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Helper function that fuses a single velocity or position measurement
|
||||
bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index)
|
||||
{
|
||||
VectorState Kfusion; // Kalman gain vector for any single observation - sequential fusion is used.
|
||||
|
||||
// calculate kalman gain K = PHS, where S = 1/innovation variance
|
||||
for (int row = 0; row < State::size; row++) {
|
||||
Kfusion(row) = P(row, state_index) / innov_var;
|
||||
}
|
||||
|
||||
clearInhibitedStateKalmanGains(Kfusion);
|
||||
|
||||
SquareMatrixState KHP;
|
||||
|
||||
for (unsigned row = 0; row < State::size; row++) {
|
||||
for (unsigned column = 0; column < State::size; column++) {
|
||||
KHP(row, column) = Kfusion(row) * P(state_index, column);
|
||||
}
|
||||
}
|
||||
|
||||
const bool healthy = checkAndFixCovarianceUpdate(KHP);
|
||||
|
||||
setVelPosStatus(state_index, healthy);
|
||||
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
P -= KHP;
|
||||
|
||||
constrainStateVariances();
|
||||
|
||||
// apply the state corrections
|
||||
fuse(Kfusion, innov);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void Ekf::setVelPosStatus(const int state_index, const bool healthy)
|
||||
{
|
||||
switch (state_index) {
|
||||
case State::vel.idx:
|
||||
if (healthy) {
|
||||
_fault_status.flags.bad_vel_N = false;
|
||||
_time_last_hor_vel_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
_fault_status.flags.bad_vel_N = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case State::vel.idx + 1:
|
||||
if (healthy) {
|
||||
_fault_status.flags.bad_vel_E = false;
|
||||
_time_last_hor_vel_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
_fault_status.flags.bad_vel_E = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case State::vel.idx + 2:
|
||||
if (healthy) {
|
||||
_fault_status.flags.bad_vel_D = false;
|
||||
_time_last_ver_vel_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
_fault_status.flags.bad_vel_D = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case State::pos.idx:
|
||||
if (healthy) {
|
||||
_fault_status.flags.bad_pos_N = false;
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
_fault_status.flags.bad_pos_N = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case State::pos.idx + 1:
|
||||
if (healthy) {
|
||||
_fault_status.flags.bad_pos_E = false;
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
_fault_status.flags.bad_pos_E = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case State::pos.idx + 2:
|
||||
if (healthy) {
|
||||
_fault_status.flags.bad_pos_D = false;
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
_fault_status.flags.bad_pos_D = true;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,195 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ekf.h"
|
||||
|
||||
void Ekf::updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
|
||||
const float innov_gate, estimator_aid_source2d_s &aid_src) const
|
||||
{
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
aid_src.observation[i] = obs(i);
|
||||
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
|
||||
|
||||
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
|
||||
const int state_index = State::vel.idx + i;
|
||||
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var,
|
||||
const float innov_gate, estimator_aid_source3d_s &aid_src) const
|
||||
{
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
aid_src.observation[i] = obs(i);
|
||||
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
|
||||
|
||||
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
|
||||
const int state_index = State::vel.idx + i;
|
||||
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
// vz special case if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
||||
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]);
|
||||
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
|
||||
aid_src.innovation_rejected = false;
|
||||
}
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
// vx, vy
|
||||
if (!aid_src.innovation_rejected
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
_time_last_hor_vel_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
|
||||
{
|
||||
// vx, vy, vz
|
||||
if (!aid_src.innovation_rejected
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
|
||||
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
_time_last_hor_vel_fuse = _time_delayed_us;
|
||||
_time_last_ver_vel_fuse = _time_delayed_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)
|
||||
{
|
||||
const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
|
||||
_state.vel.xy() = new_horz_vel;
|
||||
|
||||
if (PX4_ISFINITE(new_horz_vel_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(new_horz_vel_var(1))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
|
||||
}
|
||||
|
||||
_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
|
||||
_state_reset_status.velNE_change = delta_horz_vel;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.velNE_change += delta_horz_vel;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.velNE++;
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_vel_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
|
||||
{
|
||||
const float delta_vert_vel = new_vert_vel - _state.vel(2);
|
||||
_state.vel(2) = new_vert_vel;
|
||||
|
||||
if (PX4_ISFINITE(new_vert_vel_var)) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
|
||||
}
|
||||
|
||||
_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
|
||||
_state_reset_status.velD_change = delta_vert_vel;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.velD_change += delta_vert_vel;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.velD++;
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_ver_vel_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityToZero()
|
||||
{
|
||||
ECL_INFO("reset velocity to zero");
|
||||
_information_events.flags.reset_vel_to_zero = true;
|
||||
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f);
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityToZero()
|
||||
{
|
||||
// we don't know what the vertical velocity is, so set it to zero
|
||||
// Set the variance to a value large enough to allow the state to converge quickly
|
||||
// that does not destabilise the filter
|
||||
resetVerticalVelocityTo(0.0f, 10.f);
|
||||
}
|
||||
|
||||
void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
|
||||
{
|
||||
resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
|
||||
resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
|
||||
}
|
||||
@@ -1896,12 +1896,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp)
|
||||
status_flags.fs_bad_sideslip = _ekf.fault_status_flags().bad_sideslip;
|
||||
status_flags.fs_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X;
|
||||
status_flags.fs_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y;
|
||||
status_flags.fs_bad_vel_n = _ekf.fault_status_flags().bad_vel_N;
|
||||
status_flags.fs_bad_vel_e = _ekf.fault_status_flags().bad_vel_E;
|
||||
status_flags.fs_bad_vel_d = _ekf.fault_status_flags().bad_vel_D;
|
||||
status_flags.fs_bad_pos_n = _ekf.fault_status_flags().bad_pos_N;
|
||||
status_flags.fs_bad_pos_e = _ekf.fault_status_flags().bad_pos_E;
|
||||
status_flags.fs_bad_pos_d = _ekf.fault_status_flags().bad_pos_D;
|
||||
status_flags.fs_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias;
|
||||
status_flags.fs_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical;
|
||||
status_flags.fs_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping;
|
||||
|
||||
@@ -270,122 +270,122 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
|
||||
26790000,0.78,0.074,-0.028,-0.62,1.1,0.83,-1.3,0.54,0.53,-3.7e+02,-0.00075,-0.0058,1e-05,0.0093,-0.02,-0.13,-0.017,-0.0038,0.57,0,0,0,0,0,0.00041,0.00081,0.019,0.074,0.09,0.0061,0.39,0.4,0.033,5.8e-07,5.8e-07,5e-06,0.031,0.031,0.00015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1
|
||||
26890000,0.78,0.096,-0.035,-0.62,1.2,0.92,-1.3,0.65,0.61,-3.7e+02,-0.00075,-0.0058,1e-05,0.0093,-0.02,-0.13,-0.017,-0.0038,0.57,0,0,0,0,0,0.00045,0.0011,0.019,0.08,0.1,0.0061,0.42,0.43,0.034,5.8e-07,5.8e-07,5e-06,0.031,0.031,0.00015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1
|
||||
26990000,0.77,0.12,-0.039,-0.62,1.4,1,-1.3,0.79,0.71,-3.7e+02,-0.00074,-0.0058,1e-05,0.0093,-0.02,-0.13,-0.017,-0.0038,0.57,0,0,0,0,0,0.00052,0.0014,0.018,0.088,0.11,0.0061,0.45,0.47,0.033,5.8e-07,5.9e-07,5e-06,0.031,0.031,0.00015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1
|
||||
27090000,0.77,0.12,-0.04,-0.62,1.6,1.2,-1.3,0.94,0.82,-3.7e+02,-0.00074,-0.0058,1e-05,0.0092,-0.02,-0.13,-0.1,-0.023,0.51,0.0048,-0.1,-0.04,0,0,0.00053,0.0015,0.018,0.097,0.13,0.0062,0.49,0.51,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00015,0.0013,0.00033,0.0013,0.00053,0.0013,0.0013,1,1
|
||||
27190000,0.77,0.11,-0.037,-0.63,1.8,1.3,-1.2,1.1,0.94,-3.7e+02,-0.00074,-0.0058,1e-05,0.0092,-0.02,-0.13,-0.11,-0.024,0.5,0.0049,-0.1,-0.041,0,0,0.0005,0.0013,0.019,0.11,0.14,0.0062,0.53,0.55,0.034,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00015,0.0012,0.00022,0.0013,0.00033,0.0013,0.0013,1,1
|
||||
27290000,0.77,0.097,-0.033,-0.63,1.9,1.4,-1.2,1.3,1.1,-3.7e+02,-0.00074,-0.0058,1e-05,0.0092,-0.019,-0.13,-0.11,-0.024,0.5,0.0051,-0.1,-0.042,0,0,0.00047,0.0011,0.019,0.11,0.16,0.0062,0.57,0.6,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0012,0.00017,0.0013,0.00023,0.0013,0.0013,1,1
|
||||
27390000,0.77,0.08,-0.029,-0.63,2,1.5,-1.2,1.5,1.2,-3.7e+02,-0.00074,-0.0058,1e-05,0.0091,-0.019,-0.13,-0.11,-0.024,0.5,0.005,-0.1,-0.043,0,0,0.00044,0.00087,0.019,0.12,0.17,0.0063,0.61,0.65,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0012,0.00014,0.0013,0.00018,0.0013,0.0012,1,1
|
||||
27490000,0.78,0.065,-0.025,-0.63,2.1,1.5,-1.2,1.7,1.4,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.009,-0.019,-0.13,-0.11,-0.024,0.5,0.0045,-0.1,-0.043,0,0,0.00041,0.00071,0.019,0.13,0.18,0.0063,0.66,0.71,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0012,0.00013,0.0012,0.00014,0.0012,0.0012,1,1
|
||||
27590000,0.78,0.052,-0.021,-0.63,2.1,1.6,-1.2,1.9,1.5,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.0089,-0.019,-0.13,-0.11,-0.025,0.5,0.004,-0.099,-0.044,0,0,0.0004,0.0006,0.02,0.14,0.19,0.0063,0.71,0.77,0.034,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0012,0.00012,0.0012,0.00012,0.0012,0.0012,1,1
|
||||
27690000,0.78,0.05,-0.021,-0.63,2.2,1.6,-1.2,2.1,1.7,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.0088,-0.018,-0.13,-0.11,-0.025,0.5,0.0034,-0.096,-0.044,0,0,0.0004,0.00059,0.02,0.14,0.2,0.0063,0.77,0.84,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0011,0.00011,0.0012,0.00011,0.0012,0.0012,1,1
|
||||
27790000,0.78,0.052,-0.021,-0.63,2.2,1.6,-1.2,2.3,1.8,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.0087,-0.018,-0.13,-0.12,-0.026,0.49,0.0028,-0.094,-0.044,0,0,0.0004,0.0006,0.02,0.15,0.21,0.0064,0.83,0.92,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0011,0.0001,0.0011,9.5e-05,0.0012,0.0011,1,1
|
||||
27890000,0.78,0.05,-0.021,-0.63,2.2,1.6,-1.2,2.6,2,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.0085,-0.018,-0.13,-0.12,-0.026,0.49,0.0028,-0.092,-0.044,0,0,0.0004,0.00059,0.02,0.15,0.21,0.0064,0.9,1,0.034,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0011,9.5e-05,0.0011,8.6e-05,0.0011,0.0011,1,1
|
||||
27990000,0.78,0.046,-0.02,-0.63,2.3,1.6,-1.2,2.8,2.2,-3.7e+02,-0.00073,-0.0058,1.1e-05,0.0083,-0.017,-0.13,-0.12,-0.026,0.49,0.0026,-0.091,-0.044,0,0,0.0004,0.00056,0.02,0.16,0.22,0.0064,0.96,1.1,0.034,5.9e-07,6e-07,5e-06,0.031,0.031,0.00014,0.0011,9.1e-05,0.0011,7.8e-05,0.0011,0.0011,1,1
|
||||
28090000,0.78,0.06,-0.024,-0.63,2.3,1.7,-1.2,3,2.3,-3.7e+02,-0.00073,-0.0058,1.1e-05,0.0082,-0.017,-0.12,-0.12,-0.026,0.49,0.0023,-0.09,-0.044,0,0,0.00041,0.00068,0.02,0.16,0.23,0.0065,1,1.2,0.033,5.9e-07,6e-07,5e-06,0.031,0.031,0.00014,0.0011,8.8e-05,0.0011,7.1e-05,0.0011,0.0011,1,1
|
||||
28190000,0.78,0.073,-0.028,-0.63,2.3,1.7,-0.94,3.2,2.5,-3.7e+02,-0.00073,-0.0058,1.1e-05,0.0079,-0.016,-0.12,-0.12,-0.026,0.49,0.0023,-0.09,-0.044,0,0,0.00043,0.00081,0.019,0.17,0.23,0.0066,1.1,1.3,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.0011,8.5e-05,0.0011,6.7e-05,0.0011,0.0011,1,1
|
||||
28290000,0.78,0.056,-0.022,-0.63,2.3,1.7,-0.078,3.5,2.7,-3.7e+02,-0.00073,-0.0058,1.1e-05,0.0077,-0.016,-0.12,-0.12,-0.026,0.49,0.0022,-0.09,-0.044,0,0,0.00041,0.00064,0.02,0.17,0.24,0.0066,1.2,1.4,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.0011,8.3e-05,0.0011,6.2e-05,0.0011,0.0011,1,1
|
||||
28390000,0.78,0.023,-0.0091,-0.63,2.3,1.7,0.78,3.7,2.8,-3.7e+02,-0.00073,-0.0058,1.2e-05,0.0073,-0.015,-0.12,-0.12,-0.027,0.49,0.002,-0.089,-0.043,0,0,0.00039,0.00043,0.02,0.17,0.24,0.0067,1.3,1.5,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.0011,8e-05,0.0011,5.8e-05,0.0011,0.0011,1,1
|
||||
28490000,0.78,0.0035,-0.0023,-0.63,2.3,1.7,1.1,3.9,3,-3.7e+02,-0.00072,-0.0058,1.2e-05,0.0069,-0.014,-0.12,-0.12,-0.027,0.49,0.0018,-0.086,-0.041,0,0,0.00039,0.00039,0.02,0.17,0.23,0.0067,1.4,1.6,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.001,7.6e-05,0.001,5.4e-05,0.0011,0.001,1,1
|
||||
28590000,0.78,-0.00016,-0.00091,-0.63,2.2,1.7,0.97,4.1,3.2,-3.7e+02,-0.00072,-0.0058,1.2e-05,0.0064,-0.013,-0.12,-0.13,-0.028,0.49,0.0017,-0.081,-0.04,0,0,0.00039,0.0004,0.02,0.18,0.23,0.0068,1.5,1.7,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.00096,7.2e-05,0.00098,5.1e-05,0.001,0.00098,1,1
|
||||
28690000,0.78,-0.0011,-0.0003,-0.63,2.1,1.6,0.98,4.4,3.3,-3.7e+02,-0.00072,-0.0058,1.3e-05,0.0059,-0.011,-0.12,-0.13,-0.029,0.48,0.0014,-0.076,-0.038,0,0,0.00039,0.0004,0.02,0.18,0.23,0.0068,1.6,1.8,0.034,6e-07,6e-07,5.1e-06,0.031,0.031,0.00013,0.00091,6.9e-05,0.00093,4.8e-05,0.00095,0.00093,1,1
|
||||
28790000,0.78,-0.0015,-2.3e-05,-0.63,2.1,1.6,0.98,4.6,3.5,-3.7e+02,-0.00071,-0.0058,1.3e-05,0.0053,-0.0096,-0.12,-0.13,-0.03,0.48,0.0012,-0.073,-0.037,0,0,0.0004,0.0004,0.02,0.18,0.23,0.0068,1.7,1.9,0.034,6e-07,6e-07,5.1e-06,0.031,0.031,0.00013,0.00088,6.6e-05,0.0009,4.6e-05,0.00092,0.00089,1,1
|
||||
28890000,0.78,-0.0013,2.6e-05,-0.63,2,1.5,0.97,4.8,3.6,-3.7e+02,-0.00071,-0.0058,1.3e-05,0.0047,-0.0081,-0.12,-0.13,-0.03,0.48,0.0011,-0.071,-0.036,0,0,0.0004,0.0004,0.02,0.18,0.23,0.0069,1.8,2.1,0.034,6e-07,6e-07,5.1e-06,0.031,0.031,0.00013,0.00085,6.4e-05,0.00087,4.4e-05,0.00089,0.00087,1,1
|
||||
28990000,0.78,-0.00088,-5.7e-05,-0.63,1.9,1.5,0.97,5,3.8,-3.7e+02,-0.00071,-0.0058,1.4e-05,0.0039,-0.0062,-0.12,-0.14,-0.03,0.48,0.00083,-0.069,-0.034,0,0,0.0004,0.0004,0.02,0.19,0.23,0.0069,1.9,2.2,0.034,6e-07,6.1e-07,5.1e-06,0.031,0.031,0.00013,0.00082,6.2e-05,0.00084,4.2e-05,0.00086,0.00084,1,1
|
||||
29090000,0.78,-0.00046,-0.00016,-0.63,1.9,1.5,0.96,5.1,3.9,-3.7e+02,-0.0007,-0.0058,1.4e-05,0.0033,-0.0045,-0.12,-0.14,-0.031,0.48,0.0007,-0.067,-0.034,0,0,0.0004,0.0004,0.02,0.19,0.23,0.007,2,2.4,0.034,6e-07,6.1e-07,5.1e-06,0.031,0.031,0.00012,0.0008,6e-05,0.00082,4e-05,0.00084,0.00082,1,1
|
||||
29190000,0.78,-0.00012,-0.00026,-0.63,1.8,1.5,0.95,5.3,4.1,-3.7e+02,-0.0007,-0.0058,1.5e-05,0.0025,-0.0026,-0.12,-0.14,-0.031,0.48,0.00071,-0.065,-0.034,0,0,0.0004,0.0004,0.02,0.19,0.23,0.007,2.1,2.5,0.034,6e-07,6.1e-07,5.1e-06,0.031,0.031,0.00012,0.00078,5.8e-05,0.0008,3.9e-05,0.00082,0.0008,1,1
|
||||
29290000,0.78,0.00077,-0.00051,-0.63,1.8,1.4,0.98,5.5,4.2,-3.7e+02,-0.0007,-0.0058,1.5e-05,0.0017,-0.00039,-0.12,-0.14,-0.032,0.47,0.00059,-0.063,-0.033,0,0,0.0004,0.00041,0.02,0.2,0.23,0.0071,2.2,2.6,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00077,5.7e-05,0.00078,3.7e-05,0.0008,0.00078,1,1
|
||||
29390000,0.78,0.0022,-0.00079,-0.63,1.7,1.4,0.99,5.7,4.3,-3.7e+02,-0.00069,-0.0058,1.6e-05,0.00075,0.002,-0.12,-0.14,-0.032,0.47,0.00039,-0.062,-0.033,0,0,0.0004,0.00041,0.02,0.2,0.23,0.0071,2.4,2.8,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00076,5.6e-05,0.00077,3.6e-05,0.00079,0.00077,1,1
|
||||
29490000,0.78,0.0034,-0.0012,-0.63,1.6,1.4,0.99,5.8,4.5,-3.7e+02,-0.00069,-0.0058,1.6e-05,5.5e-05,0.0038,-0.12,-0.14,-0.032,0.47,0.00037,-0.061,-0.032,0,0,0.0004,0.00041,0.02,0.21,0.24,0.0072,2.5,3,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00075,5.5e-05,0.00076,3.5e-05,0.00078,0.00076,1,1
|
||||
29590000,0.78,0.0044,-0.0014,-0.63,1.6,1.4,0.98,6,4.6,-3.7e+02,-0.00069,-0.0058,1.7e-05,-0.001,0.0066,-0.12,-0.14,-0.032,0.47,0.00033,-0.06,-0.032,0,0,0.0004,0.00041,0.02,0.21,0.24,0.0072,2.6,3.1,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00074,5.4e-05,0.00075,3.3e-05,0.00077,0.00075,1,1
|
||||
29690000,0.78,0.0051,-0.0016,-0.63,1.5,1.3,0.98,6.1,4.7,-3.7e+02,-0.00069,-0.0058,1.7e-05,-0.0019,0.0087,-0.12,-0.14,-0.032,0.47,0.00028,-0.059,-0.032,0,0,0.00041,0.00041,0.02,0.22,0.24,0.0072,2.8,3.3,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00073,5.3e-05,0.00074,3.2e-05,0.00076,0.00074,1,1
|
||||
29790000,0.78,0.0056,-0.0017,-0.63,1.5,1.3,0.97,6.3,4.9,-3.7e+02,-0.00068,-0.0058,1.8e-05,-0.0028,0.011,-0.12,-0.14,-0.032,0.47,0.0003,-0.059,-0.031,0,0,0.00041,0.00041,0.02,0.22,0.25,0.0073,2.9,3.5,0.035,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00072,5.2e-05,0.00073,3.2e-05,0.00076,0.00073,1,1
|
||||
29890000,0.78,0.0058,-0.0018,-0.63,1.5,1.3,0.95,6.4,5,-3.7e+02,-0.00068,-0.0058,1.9e-05,-0.0041,0.015,-0.12,-0.14,-0.032,0.47,0.00022,-0.058,-0.031,0,0,0.00041,0.00041,0.02,0.23,0.25,0.0073,3.1,3.6,0.035,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00071,5.1e-05,0.00073,3.1e-05,0.00075,0.00073,1,1
|
||||
29990000,0.78,0.006,-0.0018,-0.63,1.4,1.3,0.94,6.6,5.1,-3.7e+02,-0.00068,-0.0058,1.9e-05,-0.0052,0.017,-0.12,-0.14,-0.033,0.47,0.0002,-0.058,-0.031,0,0,0.00041,0.00042,0.02,0.24,0.26,0.0073,3.3,3.8,0.035,6.1e-07,6.2e-07,5.1e-06,0.031,0.03,0.00012,0.00071,5.1e-05,0.00072,3e-05,0.00074,0.00072,1,1
|
||||
30090000,0.78,0.0059,-0.0018,-0.63,1.4,1.3,0.93,6.7,5.2,-3.7e+02,-0.00068,-0.0058,2e-05,-0.0063,0.02,-0.12,-0.15,-0.033,0.47,0.00014,-0.057,-0.031,0,0,0.00041,0.00042,0.02,0.24,0.26,0.0073,3.4,4,0.035,6.1e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.0007,5e-05,0.00072,2.9e-05,0.00074,0.00072,1,1
|
||||
30190000,0.78,0.0057,-0.0017,-0.63,1.3,1.2,0.91,6.8,5.4,-3.7e+02,-0.00068,-0.0058,2e-05,-0.0071,0.022,-0.12,-0.15,-0.033,0.47,0.00017,-0.056,-0.031,0,0,0.00041,0.00042,0.02,0.25,0.27,0.0074,3.6,4.2,0.035,6.1e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.0007,4.9e-05,0.00071,2.8e-05,0.00073,0.00071,1,1
|
||||
30290000,0.78,0.0056,-0.0017,-0.63,1.3,1.2,0.9,7,5.5,-3.7e+02,-0.00068,-0.0058,2.1e-05,-0.0079,0.024,-0.12,-0.15,-0.033,0.47,0.00016,-0.055,-0.032,0,0,0.00041,0.00042,0.02,0.26,0.27,0.0074,3.8,4.4,0.035,6.1e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.00069,4.9e-05,0.00071,2.7e-05,0.00073,0.00071,1,1
|
||||
30390000,0.78,0.0054,-0.0016,-0.63,1.3,1.2,0.89,7.1,5.6,-3.7e+02,-0.00067,-0.0058,2.1e-05,-0.0091,0.027,-0.12,-0.15,-0.033,0.47,0.00015,-0.055,-0.031,0,0,0.00041,0.00042,0.02,0.26,0.28,0.0074,4,4.6,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.00069,4.8e-05,0.0007,2.7e-05,0.00072,0.0007,1,1
|
||||
30490000,0.78,0.0051,-0.0015,-0.63,1.2,1.2,0.87,7.2,5.7,-3.7e+02,-0.00067,-0.0058,2.2e-05,-0.0098,0.029,-0.12,-0.15,-0.033,0.47,0.00017,-0.055,-0.031,0,0,0.00041,0.00042,0.02,0.27,0.28,0.0074,4.2,4.9,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.00069,4.8e-05,0.0007,2.6e-05,0.00072,0.0007,1,1
|
||||
30590000,0.78,0.0048,-0.0014,-0.63,1.2,1.2,0.83,7.3,5.8,-3.7e+02,-0.00067,-0.0058,2.2e-05,-0.011,0.032,-0.12,-0.15,-0.033,0.47,0.00022,-0.055,-0.031,0,0,0.00041,0.00042,0.02,0.28,0.29,0.0074,4.4,5.1,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.029,0.00011,0.00068,4.7e-05,0.00069,2.6e-05,0.00071,0.00069,1,1
|
||||
30690000,0.78,0.0045,-0.0013,-0.63,1.1,1.2,0.83,7.5,5.9,-3.7e+02,-0.00067,-0.0058,2.3e-05,-0.012,0.035,-0.12,-0.15,-0.033,0.47,0.00023,-0.054,-0.031,0,0,0.00041,0.00043,0.02,0.29,0.3,0.0074,4.6,5.3,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.029,0.00011,0.00068,4.7e-05,0.00069,2.5e-05,0.00071,0.00069,1,1
|
||||
30790000,0.78,0.0042,-0.0012,-0.63,1.1,1.1,0.82,7.6,6.1,-3.7e+02,-0.00067,-0.0058,2.3e-05,-0.013,0.037,-0.12,-0.15,-0.033,0.47,0.00019,-0.054,-0.031,0,0,0.00041,0.00043,0.02,0.29,0.3,0.0074,4.8,5.6,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.029,0.00011,0.00067,4.7e-05,0.00069,2.5e-05,0.00071,0.00069,1,1
|
||||
30890000,0.78,0.0038,-0.0011,-0.63,1.1,1.1,0.81,7.7,6.2,-3.7e+02,-0.00067,-0.0058,2.4e-05,-0.014,0.039,-0.12,-0.15,-0.033,0.47,0.0002,-0.053,-0.031,0,0,0.00041,0.00043,0.02,0.3,0.31,0.0074,5.1,5.8,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.029,0.00011,0.00067,4.6e-05,0.00068,2.4e-05,0.0007,0.00068,1,1
|
||||
30990000,0.78,0.0032,-0.00096,-0.63,1,1.1,0.8,7.8,6.3,-3.7e+02,-0.00067,-0.0058,2.4e-05,-0.015,0.042,-0.12,-0.15,-0.034,0.47,0.00021,-0.053,-0.031,0,0,0.00041,0.00043,0.021,0.31,0.32,0.0074,5.3,6.1,0.035,6.2e-07,6.3e-07,5.1e-06,0.031,0.029,0.00011,0.00067,4.6e-05,0.00068,2.3e-05,0.0007,0.00068,1,1
|
||||
31090000,0.78,0.0028,-0.00081,-0.63,0.99,1.1,0.79,7.9,6.4,-3.7e+02,-0.00067,-0.0058,2.5e-05,-0.016,0.045,-0.12,-0.15,-0.034,0.47,0.00019,-0.053,-0.031,0,0,0.00041,0.00043,0.021,0.32,0.32,0.0074,5.6,6.3,0.035,6.2e-07,6.3e-07,5.1e-06,0.031,0.029,0.00011,0.00066,4.5e-05,0.00068,2.3e-05,0.0007,0.00068,1,1
|
||||
31190000,0.78,0.0026,-0.00071,-0.63,0.95,1.1,0.78,8,6.5,-3.7e+02,-0.00067,-0.0058,2.6e-05,-0.018,0.049,-0.12,-0.15,-0.034,0.47,0.0002,-0.052,-0.031,0,0,0.00041,0.00043,0.021,0.33,0.33,0.0074,5.8,6.6,0.035,6.2e-07,6.3e-07,5.1e-06,0.03,0.029,0.0001,0.00066,4.5e-05,0.00067,2.3e-05,0.00069,0.00067,1,1
|
||||
31290000,0.78,0.0021,-0.00056,-0.63,0.91,1.1,0.78,8.1,6.6,-3.7e+02,-0.00068,-0.0058,2.7e-05,-0.019,0.053,-0.12,-0.15,-0.034,0.47,0.00022,-0.052,-0.032,0,0,0.00041,0.00043,0.021,0.34,0.34,0.0074,6.1,6.9,0.035,6.2e-07,6.3e-07,5.1e-06,0.03,0.029,0.0001,0.00066,4.5e-05,0.00067,2.2e-05,0.00069,0.00067,1,1
|
||||
31390000,0.78,0.0015,-0.00039,-0.63,0.87,1,0.78,8.2,6.7,-3.7e+02,-0.00068,-0.0058,2.7e-05,-0.02,0.056,-0.12,-0.15,-0.034,0.47,0.00023,-0.052,-0.032,0,0,0.00041,0.00044,0.021,0.35,0.35,0.0074,6.4,7.2,0.035,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,0.0001,0.00065,4.4e-05,0.00067,2.2e-05,0.00069,0.00067,1,1
|
||||
31490000,0.78,0.00099,-0.00026,-0.63,0.83,1,0.78,8.3,6.8,-3.7e+02,-0.00068,-0.0058,2.8e-05,-0.022,0.059,-0.11,-0.15,-0.034,0.47,0.00019,-0.051,-0.032,0,0,0.00042,0.00044,0.021,0.36,0.36,0.0074,6.7,7.5,0.036,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,0.0001,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31590000,0.78,0.00066,-0.00015,-0.63,0.8,1,0.77,8.3,6.9,-3.7e+02,-0.00068,-0.0058,2.8e-05,-0.023,0.062,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00044,0.021,0.37,0.37,0.0073,7,7.8,0.035,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,0.0001,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31690000,0.78,6.1e-05,1.1e-05,-0.63,0.76,0.99,0.78,8.4,7,-3.7e+02,-0.00068,-0.0058,2.9e-05,-0.024,0.066,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00044,0.021,0.38,0.38,0.0073,7.3,8.1,0.035,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,0.0001,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31790000,0.78,-0.00061,0.00024,-0.63,0.72,0.97,0.78,8.5,7.1,-3.7e+02,-0.00068,-0.0058,3e-05,-0.026,0.069,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00044,0.021,0.39,0.39,0.0073,7.6,8.5,0.036,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,9.9e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31890000,0.78,-0.0012,0.00039,-0.63,0.68,0.95,0.77,8.6,7.2,-3.7e+02,-0.00068,-0.0058,3e-05,-0.027,0.073,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00045,0.021,0.4,0.4,0.0073,7.9,8.8,0.036,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,9.8e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31990000,0.78,-0.0016,0.00052,-0.63,0.64,0.93,0.77,8.6,7.3,-3.7e+02,-0.00069,-0.0058,3.1e-05,-0.029,0.076,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00045,0.021,0.41,0.41,0.0072,8.3,9.1,0.035,6.3e-07,6.4e-07,5.1e-06,0.03,0.028,9.8e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32090000,0.78,-0.0023,0.00071,-0.63,0.6,0.91,0.78,8.7,7.4,-3.7e+02,-0.00069,-0.0058,3.2e-05,-0.03,0.08,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00045,0.021,0.42,0.42,0.0072,8.6,9.5,0.036,6.3e-07,6.4e-07,5.1e-06,0.03,0.027,9.7e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32190000,0.78,-0.0031,0.0009,-0.63,0.56,0.9,0.78,8.8,7.5,-3.7e+02,-0.00069,-0.0058,3.3e-05,-0.032,0.085,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00046,0.021,0.43,0.43,0.0072,9,9.9,0.036,6.3e-07,6.4e-07,5.1e-06,0.03,0.027,9.6e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32290000,0.78,-0.0037,0.001,-0.63,0.53,0.88,0.77,8.8,7.6,-3.7e+02,-0.0007,-0.0058,3.3e-05,-0.034,0.089,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00046,0.021,0.44,0.44,0.0072,9.4,10,0.036,6.3e-07,6.4e-07,5.1e-06,0.03,0.027,9.5e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32390000,0.78,-0.0043,0.0012,-0.63,0.48,0.86,0.77,8.9,7.7,-3.7e+02,-0.0007,-0.0058,3.4e-05,-0.035,0.091,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00046,0.021,0.46,0.45,0.0072,9.8,11,0.036,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.5e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32490000,0.78,-0.0044,0.0012,-0.63,0.44,0.84,0.78,9,7.8,-3.7e+02,-0.0007,-0.0058,3.4e-05,-0.036,0.095,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00046,0.021,0.47,0.46,0.0071,10,11,0.036,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.4e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32590000,0.78,-0.0047,0.0013,-0.63,-1.6,-0.84,0.63,-1e+06,1.2e+04,-3.7e+02,-0.0007,-0.0058,3.5e-05,-0.037,0.096,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.25,0.25,0.56,0.25,0.25,0.038,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.3e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32690000,0.78,-0.0047,0.0013,-0.63,-1.6,-0.86,0.62,-1e+06,1.2e+04,-3.7e+02,-0.0007,-0.0058,3.5e-05,-0.038,0.098,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.25,0.25,0.55,0.26,0.26,0.049,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.3e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32790000,0.78,-0.0046,0.0013,-0.63,-1.6,-0.84,0.62,-1e+06,1.2e+04,-3.7e+02,-0.00071,-0.0058,3.4e-05,-0.038,0.1,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.13,0.13,0.27,0.26,0.26,0.049,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.2e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32890000,0.78,-0.0045,0.0012,-0.63,-1.6,-0.85,0.6,-1e+06,1.2e+04,-3.7e+02,-0.00071,-0.0058,3.5e-05,-0.039,0.1,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.13,0.13,0.26,0.27,0.27,0.059,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.2e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32990000,0.78,-0.0044,0.0011,-0.63,-1.6,-0.85,0.6,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,3.3e-05,-0.04,0.1,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.085,0.085,0.17,0.27,0.27,0.057,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.2e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
33090000,0.78,-0.0044,0.0011,-0.63,-1.6,-0.86,0.59,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,3.4e-05,-0.04,0.11,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.086,0.086,0.16,0.28,0.28,0.065,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.2e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
33190000,0.78,-0.0029,-0.0022,-0.62,-1.6,-0.85,0.53,-1e+06,1.2e+04,-3.7e+02,-0.00076,-0.0058,2.9e-05,-0.041,0.11,-0.11,-0.15,-0.034,0.47,0.00022,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.065,0.065,0.11,0.28,0.28,0.062,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00065,4.4e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
33290000,0.82,-0.00064,-0.014,-0.57,-1.6,-0.87,0.51,-1e+06,1.2e+04,-3.7e+02,-0.00076,-0.0058,2.9e-05,-0.041,0.11,-0.11,-0.15,-0.034,0.47,0.00013,-0.05,-0.032,0,0,0.00043,0.00047,0.021,0.066,0.067,0.11,0.29,0.29,0.067,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00064,4.3e-05,0.00066,2.1e-05,0.00067,0.00066,1,1
|
||||
33390000,0.89,-0.00074,-0.012,-0.46,-1.5,-0.86,0.71,-1e+06,1.2e+04,-3.7e+02,-0.00079,-0.0058,2.4e-05,-0.041,0.11,-0.11,-0.16,-0.035,0.47,-0.00011,-0.045,-0.031,0,0,0.00043,0.00048,0.021,0.053,0.054,0.083,0.29,0.29,0.065,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00057,3.9e-05,0.00066,2e-05,0.00059,0.00066,1,1
|
||||
33490000,0.95,0.0012,-0.0053,-0.31,-1.6,-0.88,0.73,-1e+06,1.2e+04,-3.7e+02,-0.00079,-0.0058,2.5e-05,-0.041,0.11,-0.11,-0.17,-0.039,0.47,-0.00054,-0.03,-0.032,0,0,0.00043,0.00048,0.021,0.055,0.056,0.075,0.3,0.3,0.068,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00038,3e-05,0.00065,2e-05,0.00039,0.00066,1,1
|
||||
33590000,0.99,-0.0012,4.4e-05,-0.15,-1.5,-0.85,0.69,-1e+06,1.2e+04,-3.7e+02,-0.00084,-0.0058,2.3e-05,-0.041,0.11,-0.11,-0.18,-0.042,0.47,-0.00071,-0.017,-0.032,0,0,0.00045,0.00047,0.021,0.047,0.048,0.061,0.3,0.3,0.065,6.3e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00021,2.1e-05,0.00065,1.9e-05,0.00022,0.00065,1,1
|
||||
33690000,1,-0.0045,0.0029,0.022,-1.6,-0.88,0.7,-1e+06,1.2e+04,-3.7e+02,-0.00084,-0.0058,2.3e-05,-0.041,0.11,-0.11,-0.19,-0.043,0.47,-0.0009,-0.011,-0.033,0,0,0.00049,0.00046,0.021,0.05,0.052,0.056,0.31,0.31,0.068,6.3e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00013,1.6e-05,0.00065,1.9e-05,0.00013,0.00065,1,1
|
||||
33790000,0.98,-0.0054,0.0036,0.19,-1.6,-0.87,0.69,-1e+06,1.2e+04,-3.7e+02,-0.00092,-0.0059,2.6e-05,-0.041,0.11,-0.11,-0.19,-0.044,0.47,-0.00094,-0.0069,-0.034,0,0,0.0005,0.00044,0.021,0.046,0.048,0.047,0.31,0.31,0.064,6.2e-07,6.3e-07,5.1e-06,0.03,0.026,9.1e-05,7.7e-05,1.3e-05,0.00065,1.8e-05,8e-05,0.00065,1,1
|
||||
33890000,0.94,-0.0063,0.0044,0.35,-1.6,-0.92,0.67,-1e+06,1.2e+04,-3.7e+02,-0.00092,-0.0059,2.6e-05,-0.041,0.11,-0.11,-0.2,-0.045,0.47,-0.00092,-0.0049,-0.034,0,0,0.00052,0.00044,0.021,0.052,0.055,0.043,0.32,0.32,0.065,6.3e-07,6.3e-07,5.1e-06,0.03,0.026,9.1e-05,5.2e-05,1.1e-05,0.00065,1.6e-05,5.3e-05,0.00065,1,1
|
||||
33990000,0.87,-0.0083,0.00062,0.49,-1.6,-0.91,0.65,-1e+06,1.2e+04,-3.7e+02,-0.001,-0.0059,3.8e-05,-0.041,0.11,-0.11,-0.2,-0.045,0.47,-0.0011,-0.0032,-0.034,0,0,0.00048,0.00043,0.02,0.05,0.054,0.036,0.32,0.32,0.062,6.1e-07,6.2e-07,5e-06,0.03,0.026,9.1e-05,3.7e-05,1e-05,0.00065,1.5e-05,3.8e-05,0.00065,1,1
|
||||
34090000,0.81,-0.0099,-0.00096,0.59,-1.6,-0.97,0.66,-1e+06,1.2e+04,-3.7e+02,-0.001,-0.0059,3.8e-05,-0.041,0.11,-0.11,-0.2,-0.045,0.47,-0.0013,-0.0022,-0.034,0,0,0.00047,0.00044,0.02,0.059,0.064,0.034,0.33,0.33,0.063,6.2e-07,6.2e-07,5e-06,0.03,0.026,9.1e-05,2.9e-05,9.2e-06,0.00065,1.4e-05,2.9e-05,0.00065,1,1
|
||||
34190000,0.75,-0.0074,-0.0036,0.66,-1.6,-0.95,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0011,-0.006,5.8e-05,-0.077,0.13,-0.11,-0.2,-0.046,0.47,-0.0014,-0.0012,-0.034,0,0,0.00042,0.00041,0.02,0.057,0.062,0.029,0.33,0.33,0.06,6e-07,6e-07,5e-06,0.028,0.025,9.1e-05,2.3e-05,8.4e-06,0.00065,1.3e-05,2.3e-05,0.00065,1,1
|
||||
34290000,0.72,-0.0049,-0.0025,0.7,-1.6,-0.99,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0011,-0.006,5.8e-05,-0.077,0.13,-0.11,-0.2,-0.046,0.47,-0.0015,-0.00051,-0.034,0,0,0.00041,0.00042,0.02,0.068,0.074,0.027,0.34,0.34,0.06,6e-07,6.1e-07,5e-06,0.028,0.025,9.1e-05,1.9e-05,7.8e-06,0.00065,1.2e-05,1.9e-05,0.00065,1,1
|
||||
34390000,0.69,-0.0011,-0.0052,0.72,-1.5,-0.93,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.006,8.9e-05,-0.12,0.16,-0.11,-0.2,-0.047,0.47,-0.0017,0.00017,-0.034,0,0,0.00036,0.00037,0.02,0.064,0.069,0.024,0.34,0.34,0.059,5.9e-07,5.9e-07,5e-06,0.026,0.023,9.1e-05,1.6e-05,7.4e-06,0.00065,1.1e-05,1.6e-05,0.00065,1,1
|
||||
34490000,0.68,0.00076,-0.0041,0.73,-1.5,-0.95,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.006,8.9e-05,-0.12,0.16,-0.11,-0.2,-0.047,0.47,-0.002,0.00057,-0.034,0,0,0.00035,0.00037,0.02,0.077,0.083,0.022,0.35,0.35,0.058,5.9e-07,5.9e-07,5e-06,0.026,0.023,9.1e-05,1.4e-05,6.9e-06,0.00065,1.1e-05,1.4e-05,0.00065,1,1
|
||||
34590000,0.67,0.0027,-0.0069,0.74,-1.5,-0.87,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0013,-0.0061,0.00012,-0.17,0.19,-0.11,-0.2,-0.047,0.47,-0.0023,0.001,-0.034,0,0,0.0003,0.00031,0.019,0.07,0.074,0.019,0.35,0.35,0.056,5.8e-07,5.8e-07,5e-06,0.022,0.02,9e-05,1.3e-05,6.5e-06,0.00065,1e-05,1.2e-05,0.00065,1,1
|
||||
34690000,0.66,0.0033,-0.0065,0.75,-1.5,-0.89,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0013,-0.0061,0.00012,-0.17,0.19,-0.11,-0.2,-0.047,0.47,-0.0025,0.0016,-0.034,0,0,0.0003,0.00031,0.019,0.083,0.088,0.018,0.36,0.36,0.056,5.8e-07,5.8e-07,5e-06,0.022,0.02,9e-05,1.2e-05,6.3e-06,0.00065,9.7e-06,1.1e-05,0.00065,1,1
|
||||
34790000,0.66,0.0041,-0.0099,0.75,-1.4,-0.8,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0061,0.00014,-0.2,0.23,-0.11,-0.2,-0.048,0.47,-0.0026,0.0021,-0.033,0,0,0.00025,0.00025,0.019,0.073,0.077,0.016,0.36,0.36,0.054,5.7e-07,5.8e-07,4.9e-06,0.019,0.018,9e-05,1.1e-05,6e-06,0.00065,9.2e-06,1e-05,0.00065,1,1
|
||||
34890000,0.66,0.0042,-0.0099,0.75,-1.4,-0.8,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0061,0.00014,-0.2,0.23,-0.11,-0.2,-0.048,0.47,-0.0029,0.0024,-0.033,0,0,0.00025,0.00025,0.019,0.085,0.09,0.015,0.37,0.37,0.053,5.7e-07,5.8e-07,5e-06,0.019,0.018,9e-05,9.7e-06,5.7e-06,0.00064,8.8e-06,9.6e-06,0.00064,1,1
|
||||
34990000,0.65,0.00073,-0.0051,0.76,-2.2,-1.5,-0.19,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.048,0.47,-0.0031,0.0027,-0.033,0,0,0.0002,0.00021,0.019,0.096,0.11,0.014,0.37,0.37,0.052,5.6e-07,5.7e-07,4.9e-06,0.017,0.016,8.9e-05,9.1e-06,5.5e-06,0.00064,8.5e-06,9e-06,0.00064,1,1
|
||||
35090000,0.65,0.00068,-0.0051,0.76,-2.3,-1.5,-0.24,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.048,0.47,-0.0033,0.003,-0.033,0,0,0.0002,0.00021,0.019,0.11,0.12,0.013,0.38,0.38,0.051,5.7e-07,5.7e-07,4.9e-06,0.017,0.016,8.9e-05,8.6e-06,5.3e-06,0.00064,8.2e-06,8.4e-06,0.00064,1,1
|
||||
35190000,0.65,0.00061,-0.0051,0.76,-2.3,-1.5,-0.24,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.0035,0.0033,-0.033,0,0,0.0002,0.00021,0.019,0.12,0.14,0.013,0.39,0.4,0.05,5.7e-07,5.7e-07,4.9e-06,0.017,0.016,8.9e-05,8.1e-06,5.1e-06,0.00064,7.9e-06,7.9e-06,0.00064,1,1
|
||||
35290000,0.65,0.00052,-0.0051,0.76,-2.2,-1.5,-0.23,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.0036,0.0035,-0.032,0,0,0.0002,0.00021,0.019,0.14,0.15,0.012,0.41,0.41,0.049,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,7.6e-06,5e-06,0.00064,7.6e-06,7.5e-06,0.00064,1,1
|
||||
35390000,0.65,0.00052,-0.005,0.76,-2.2,-1.5,-0.22,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.0037,0.0037,-0.032,0,0,0.0002,0.00021,0.019,0.15,0.17,0.011,0.43,0.43,0.049,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,7.3e-06,4.8e-06,0.00064,7.4e-06,7.2e-06,0.00064,1,1
|
||||
35490000,0.65,0.00048,-0.005,0.76,-2.2,-1.5,-0.21,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.0038,0.0038,-0.032,0,0,0.0002,0.00021,0.019,0.17,0.18,0.011,0.45,0.46,0.048,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,7e-06,4.7e-06,0.00064,7.1e-06,6.8e-06,0.00064,1,1
|
||||
35590000,0.65,0.00046,-0.005,0.76,-2.2,-1.5,-0.21,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.004,0.004,-0.032,0,0,0.0002,0.0002,0.019,0.18,0.2,0.01,0.48,0.49,0.047,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,6.7e-06,4.5e-06,0.00064,6.9e-06,6.6e-06,0.00064,1,1
|
||||
35690000,0.65,0.00047,-0.005,0.76,-2.2,-1.5,-0.2,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.0041,0.0041,-0.032,0,0,0.0002,0.0002,0.019,0.2,0.22,0.0096,0.51,0.52,0.047,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,6.5e-06,4.4e-06,0.00064,6.7e-06,6.3e-06,0.00064,1,1
|
||||
35790000,0.66,0.00043,-0.0049,0.76,-2.2,-1.5,-0.2,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.0042,0.0043,-0.032,0,0,0.0002,0.0002,0.019,0.22,0.24,0.0091,0.55,0.56,0.046,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,6.3e-06,4.3e-06,0.00064,6.6e-06,6.1e-06,0.00064,1,1
|
||||
35890000,0.66,0.00036,-0.0049,0.76,-2.2,-1.5,-0.19,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.47,-0.0043,0.0044,-0.032,0,0,0.0002,0.0002,0.019,0.24,0.26,0.0087,0.59,0.6,0.045,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,6e-06,4.2e-06,0.00064,6.4e-06,5.9e-06,0.00064,1,1
|
||||
35990000,0.66,0.00035,-0.0049,0.76,-2.2,-1.4,-0.18,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0044,0.0045,-0.032,0,0,0.0002,0.0002,0.019,0.26,0.28,0.0084,0.64,0.66,0.045,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,5.9e-06,4.1e-06,0.00064,6.2e-06,5.7e-06,0.00064,1,1
|
||||
36090000,0.66,0.00034,-0.0048,0.76,-2.1,-1.4,-0.18,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0045,0.0045,-0.032,0,0,0.0002,0.0002,0.019,0.29,0.31,0.008,0.69,0.71,0.044,5.8e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,5.7e-06,4.1e-06,0.00064,6.1e-06,5.5e-06,0.00064,1,1
|
||||
36190000,0.66,0.00031,-0.0048,0.76,-2.1,-1.4,-0.17,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0045,0.0046,-0.032,0,0,0.0002,0.0002,0.019,0.31,0.33,0.0077,0.76,0.78,0.043,5.8e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,5.5e-06,4e-06,0.00064,6e-06,5.4e-06,0.00064,1,1
|
||||
36290000,0.66,0.00033,-0.0047,0.76,-2.1,-1.4,-0.16,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0046,0.0047,-0.032,0,0,0.00019,0.0002,0.019,0.34,0.36,0.0075,0.83,0.86,0.043,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,5.4e-06,3.9e-06,0.00064,5.8e-06,5.3e-06,0.00064,1,1
|
||||
36390000,0.66,0.00029,-0.0047,0.76,-2.1,-1.4,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0047,0.0048,-0.032,0,0,0.00019,0.0002,0.02,0.36,0.39,0.0072,0.91,0.94,0.042,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,5.3e-06,3.8e-06,0.00064,5.7e-06,5.1e-06,0.00064,1,1
|
||||
36490000,0.66,0.00023,-0.0046,0.76,-2.1,-1.4,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0048,0.0049,-0.032,0,0,0.00019,0.0002,0.02,0.39,0.42,0.0069,1,1,0.041,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,5.1e-06,3.8e-06,0.00064,5.6e-06,5e-06,0.00064,1,1
|
||||
36590000,0.66,0.00024,-0.0045,0.76,-2.1,-1.4,-0.14,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0048,0.005,-0.032,0,0,0.00019,0.0002,0.02,0.42,0.45,0.0067,1.1,1.1,0.041,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,5e-06,3.7e-06,0.00064,5.5e-06,4.9e-06,0.00064,1,1
|
||||
36690000,0.66,0.00025,-0.0045,0.76,-2.1,-1.4,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0049,0.0051,-0.032,0,0,0.00019,0.0002,0.02,0.45,0.48,0.0066,1.2,1.3,0.04,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,4.9e-06,3.6e-06,0.00064,5.4e-06,4.8e-06,0.00064,1,1
|
||||
36790000,0.66,0.00022,-0.0044,0.76,-2.1,-1.4,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0049,0.0051,-0.032,0,0,0.00019,0.0002,0.02,0.48,0.51,0.0064,1.3,1.4,0.04,5.8e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.8e-06,3.6e-06,0.00064,5.3e-06,4.7e-06,0.00064,1,1
|
||||
36890000,0.66,0.00021,-0.0043,0.76,-2.1,-1.4,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0049,0.0051,-0.032,0,0,0.00019,0.0002,0.02,0.51,0.54,0.0062,1.5,1.5,0.039,5.8e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.7e-06,3.5e-06,0.00064,5.2e-06,4.6e-06,0.00064,1,1
|
||||
36990000,0.66,0.00025,-0.0043,0.76,-2.1,-1.4,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.005,0.0052,-0.032,0,0,0.00019,0.0002,0.02,0.55,0.58,0.0061,1.6,1.7,0.039,5.8e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.7e-06,3.5e-06,0.00064,5.1e-06,4.5e-06,0.00064,1,1
|
||||
37090000,0.66,0.00027,-0.0042,0.76,-2,-1.4,-0.1,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.005,0.0053,-0.032,0,0,0.00019,0.0002,0.02,0.58,0.61,0.006,1.8,1.9,0.038,5.8e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.6e-06,3.4e-06,0.00064,5e-06,4.4e-06,0.00064,1,1
|
||||
37190000,0.66,0.00026,-0.0042,0.76,-2,-1.4,-0.093,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0051,0.0053,-0.032,0,0,0.00019,0.0002,0.02,0.62,0.65,0.0059,2,2.1,0.038,5.9e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.5e-06,3.4e-06,0.00064,5e-06,4.4e-06,0.00064,1,1
|
||||
37290000,0.66,0.00021,-0.0041,0.76,-2,-1.3,-0.086,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0051,0.0053,-0.032,0,0,0.00019,0.0002,0.02,0.65,0.69,0.0058,2.2,2.3,0.037,5.9e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.4e-06,3.3e-06,0.00064,4.9e-06,4.3e-06,0.00064,1,1
|
||||
37390000,0.66,0.00025,-0.0041,0.76,-2,-1.3,-0.08,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0051,0.0054,-0.032,0,0,0.00019,0.0002,0.02,0.69,0.73,0.0057,2.4,2.5,0.037,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.4e-06,3.3e-06,0.00064,4.8e-06,4.2e-06,0.00064,1,1
|
||||
37490000,0.66,0.00024,-0.004,0.76,-2,-1.3,-0.073,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0051,0.0055,-0.032,0,0,0.00019,0.0002,0.02,0.73,0.77,0.0056,2.6,2.8,0.036,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.3e-06,3.3e-06,0.00064,4.7e-06,4.2e-06,0.00064,1,1
|
||||
37590000,0.66,0.00025,-0.004,0.76,-2,-1.3,-0.065,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0051,0.0055,-0.032,0,0,0.00019,0.0002,0.02,0.77,0.81,0.0056,2.9,3.1,0.036,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.2e-06,3.2e-06,0.00064,4.7e-06,4.1e-06,0.00064,1,1
|
||||
37690000,0.66,0.00019,-0.0039,0.76,-2,-1.3,-0.056,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0052,0.0055,-0.032,0,0,0.00019,0.0002,0.02,0.81,0.85,0.0055,3.2,3.4,0.036,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.2e-06,3.2e-06,0.00064,4.6e-06,4e-06,0.00064,1,1
|
||||
37790000,0.66,0.00013,-0.0039,0.76,-2,-1.3,-0.048,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0052,0.0056,-0.032,0,0,0.00019,0.0002,0.02,0.85,0.9,0.0055,3.5,3.7,0.035,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.1e-06,3.1e-06,0.00064,4.6e-06,4e-06,0.00064,1,1
|
||||
37890000,0.66,0.00011,-0.0039,0.75,-2,-1.3,-0.041,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0052,0.0056,-0.032,0,0,0.00019,0.0002,0.02,0.9,0.94,0.0054,3.8,4,0.035,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.1e-06,3.1e-06,0.00064,4.5e-06,3.9e-06,0.00064,1,1
|
||||
37990000,0.66,5.5e-05,-0.0039,0.75,-1.9,-1.3,-0.033,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0053,0.0056,-0.032,0,0,0.00019,0.0002,0.02,0.94,0.99,0.0054,4.2,4.4,0.035,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4e-06,3.1e-06,0.00064,4.5e-06,3.9e-06,0.00064,1,1
|
||||
38090000,0.66,2.6e-05,-0.0038,0.75,-1.9,-1.3,-0.023,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0053,0.0056,-0.032,0,0,0.00019,0.0002,0.02,0.99,1,0.0054,4.6,4.8,0.034,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4e-06,3e-06,0.00064,4.4e-06,3.8e-06,0.00064,1,1
|
||||
38190000,0.66,3.8e-05,-0.0038,0.75,-1.9,-1.3,-0.016,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0053,0.0057,-0.032,0,0,0.00019,0.0002,0.02,1,1.1,0.0053,5,5.2,0.034,6e-07,6e-07,5e-06,0.017,0.016,9.2e-05,3.9e-06,3e-06,0.00064,4.3e-06,3.8e-06,0.00064,1,1
|
||||
38290000,0.66,1.6e-05,-0.0038,0.75,-1.9,-1.3,-0.0091,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0053,0.0057,-0.032,0,0,0.00019,0.0002,0.02,1.1,1.1,0.0053,5.4,5.7,0.034,6e-07,6e-07,5e-06,0.017,0.016,9.2e-05,3.9e-06,3e-06,0.00064,4.3e-06,3.8e-06,0.00064,1,1
|
||||
38390000,0.66,3.2e-05,-0.0038,0.75,-1.9,-1.3,-0.0024,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0053,0.0057,-0.032,0,0,0.00019,0.0002,0.02,1.1,1.2,0.0053,5.9,6.2,0.034,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.8e-06,3e-06,0.00064,4.3e-06,3.7e-06,0.00064,1,1
|
||||
38490000,0.66,1e-05,-0.0037,0.75,-1.9,-1.3,0.0042,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0053,0.0058,-0.032,0,0,0.00019,0.0002,0.02,1.2,1.2,0.0053,6.4,6.7,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.8e-06,2.9e-06,0.00064,4.2e-06,3.7e-06,0.00064,1,1
|
||||
38590000,0.66,5.1e-05,-0.0037,0.75,-1.9,-1.3,0.01,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0054,0.0058,-0.032,0,0,0.00019,0.0002,0.02,1.2,1.3,0.0053,6.9,7.3,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.8e-06,2.9e-06,0.00064,4.2e-06,3.6e-06,0.00064,1,1
|
||||
38690000,0.66,1.1e-05,-0.0036,0.75,-1.9,-1.3,0.017,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0054,0.0058,-0.032,0,0,0.00019,0.0002,0.02,1.3,1.3,0.0053,7.5,7.9,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.7e-06,2.9e-06,0.00064,4.1e-06,3.6e-06,0.00064,1,1
|
||||
38790000,0.66,-5.9e-06,-0.0036,0.75,-1.8,-1.3,0.023,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0054,0.0058,-0.032,0,0,0.00019,0.0002,0.02,1.3,1.4,0.0053,8.1,8.6,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.7e-06,2.8e-06,0.00064,4.1e-06,3.6e-06,0.00064,1,1
|
||||
38890000,0.66,-0.00011,-0.0034,0.75,-1.8,-1.2,0.52,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.051,0.46,-0.0054,0.0058,-0.032,0,0,0.00019,0.0002,0.02,1.4,1.4,0.0053,8.8,9.2,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.7e-06,2.8e-06,0.00064,4e-06,3.5e-06,0.00064,1,1
|
||||
27090000,0.77,0.12,-0.04,-0.62,1.6,1.2,-1.3,0.94,0.82,-3.7e+02,-0.00074,-0.0058,1e-05,0.0092,-0.02,-0.13,-0.1,-0.023,0.51,0.005,-0.1,-0.04,0,0,0.00053,0.0015,0.018,0.097,0.13,0.0062,0.49,0.51,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00015,0.0013,0.00033,0.0013,0.00053,0.0013,0.0013,1,1
|
||||
27190000,0.77,0.11,-0.037,-0.63,1.8,1.3,-1.2,1.1,0.94,-3.7e+02,-0.00074,-0.0058,1e-05,0.0092,-0.02,-0.13,-0.11,-0.023,0.5,0.0051,-0.1,-0.041,0,0,0.0005,0.0013,0.019,0.11,0.14,0.0062,0.53,0.55,0.034,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00015,0.0012,0.00022,0.0013,0.00033,0.0013,0.0013,1,1
|
||||
27290000,0.77,0.097,-0.033,-0.63,1.9,1.4,-1.2,1.3,1.1,-3.7e+02,-0.00074,-0.0058,1e-05,0.0092,-0.019,-0.13,-0.11,-0.024,0.5,0.0054,-0.1,-0.042,0,0,0.00047,0.0011,0.019,0.11,0.16,0.0062,0.57,0.6,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0012,0.00017,0.0013,0.00023,0.0013,0.0013,1,1
|
||||
27390000,0.77,0.08,-0.029,-0.63,2,1.5,-1.2,1.5,1.2,-3.7e+02,-0.00074,-0.0058,1e-05,0.0091,-0.019,-0.13,-0.11,-0.024,0.5,0.0052,-0.1,-0.043,0,0,0.00044,0.00087,0.019,0.12,0.17,0.0063,0.61,0.65,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0012,0.00014,0.0013,0.00018,0.0013,0.0012,1,1
|
||||
27490000,0.78,0.065,-0.025,-0.63,2.1,1.5,-1.2,1.7,1.4,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.009,-0.019,-0.13,-0.11,-0.024,0.5,0.0047,-0.1,-0.043,0,0,0.00041,0.00071,0.019,0.13,0.18,0.0063,0.66,0.71,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0012,0.00013,0.0012,0.00014,0.0012,0.0012,1,1
|
||||
27590000,0.78,0.052,-0.021,-0.63,2.1,1.6,-1.2,1.9,1.5,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.0089,-0.019,-0.13,-0.11,-0.025,0.5,0.0043,-0.099,-0.044,0,0,0.0004,0.0006,0.02,0.14,0.19,0.0063,0.71,0.77,0.034,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0012,0.00011,0.0012,0.00012,0.0012,0.0012,1,1
|
||||
27690000,0.78,0.05,-0.021,-0.63,2.2,1.6,-1.2,2.1,1.7,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.0088,-0.018,-0.13,-0.11,-0.025,0.5,0.0037,-0.096,-0.044,0,0,0.0004,0.00059,0.02,0.14,0.2,0.0063,0.77,0.84,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0011,0.00011,0.0012,0.00011,0.0012,0.0012,1,1
|
||||
27790000,0.78,0.052,-0.021,-0.63,2.2,1.6,-1.2,2.3,1.8,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.0087,-0.018,-0.13,-0.12,-0.025,0.49,0.0032,-0.094,-0.044,0,0,0.0004,0.0006,0.02,0.15,0.21,0.0064,0.83,0.92,0.033,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0011,9.9e-05,0.0011,9.5e-05,0.0012,0.0011,1,1
|
||||
27890000,0.78,0.05,-0.021,-0.63,2.2,1.6,-1.2,2.6,2,-3.7e+02,-0.00074,-0.0058,1.1e-05,0.0085,-0.018,-0.13,-0.12,-0.026,0.49,0.0031,-0.092,-0.044,0,0,0.0004,0.00059,0.02,0.15,0.21,0.0064,0.9,1,0.034,5.9e-07,5.9e-07,5e-06,0.031,0.031,0.00014,0.0011,9.4e-05,0.0011,8.6e-05,0.0011,0.0011,1,1
|
||||
27990000,0.78,0.046,-0.02,-0.63,2.3,1.6,-1.2,2.8,2.2,-3.7e+02,-0.00073,-0.0058,1.1e-05,0.0083,-0.017,-0.13,-0.12,-0.026,0.49,0.003,-0.091,-0.044,0,0,0.0004,0.00056,0.02,0.16,0.22,0.0064,0.96,1.1,0.034,5.9e-07,6e-07,5e-06,0.031,0.031,0.00014,0.0011,9e-05,0.0011,7.8e-05,0.0011,0.0011,1,1
|
||||
28090000,0.78,0.06,-0.024,-0.63,2.3,1.7,-1.2,3,2.3,-3.7e+02,-0.00073,-0.0058,1.1e-05,0.0082,-0.017,-0.12,-0.12,-0.026,0.49,0.0027,-0.09,-0.044,0,0,0.00041,0.00068,0.02,0.16,0.23,0.0065,1,1.2,0.033,5.9e-07,6e-07,5e-06,0.031,0.031,0.00014,0.0011,8.6e-05,0.0011,7.1e-05,0.0011,0.0011,1,1
|
||||
28190000,0.78,0.073,-0.028,-0.63,2.3,1.7,-0.94,3.2,2.5,-3.7e+02,-0.00073,-0.0058,1.1e-05,0.0079,-0.016,-0.12,-0.12,-0.026,0.49,0.0027,-0.09,-0.044,0,0,0.00043,0.00081,0.019,0.17,0.23,0.0066,1.1,1.3,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.0011,8.4e-05,0.0011,6.7e-05,0.0011,0.0011,1,1
|
||||
28290000,0.78,0.056,-0.022,-0.63,2.3,1.7,-0.078,3.5,2.7,-3.7e+02,-0.00073,-0.0058,1.1e-05,0.0077,-0.016,-0.12,-0.12,-0.026,0.49,0.0026,-0.09,-0.044,0,0,0.00041,0.00064,0.02,0.17,0.24,0.0066,1.2,1.4,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.0011,8.2e-05,0.0011,6.2e-05,0.0011,0.0011,1,1
|
||||
28390000,0.78,0.023,-0.0091,-0.63,2.3,1.7,0.78,3.7,2.8,-3.7e+02,-0.00073,-0.0058,1.2e-05,0.0073,-0.015,-0.12,-0.12,-0.026,0.49,0.0024,-0.089,-0.043,0,0,0.00039,0.00043,0.02,0.17,0.24,0.0067,1.3,1.5,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.0011,7.9e-05,0.0011,5.8e-05,0.0011,0.0011,1,1
|
||||
28490000,0.78,0.0035,-0.0023,-0.63,2.3,1.7,1.1,3.9,3,-3.7e+02,-0.00072,-0.0058,1.2e-05,0.0069,-0.014,-0.12,-0.12,-0.027,0.49,0.0023,-0.086,-0.041,0,0,0.00039,0.00039,0.02,0.17,0.23,0.0067,1.4,1.6,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.001,7.5e-05,0.001,5.4e-05,0.0011,0.001,1,1
|
||||
28590000,0.78,-0.00016,-0.00091,-0.63,2.2,1.7,0.97,4.1,3.2,-3.7e+02,-0.00072,-0.0058,1.2e-05,0.0064,-0.013,-0.12,-0.13,-0.028,0.49,0.0021,-0.081,-0.04,0,0,0.00039,0.0004,0.02,0.18,0.23,0.0068,1.5,1.7,0.034,6e-07,6e-07,5e-06,0.031,0.031,0.00013,0.00096,7.1e-05,0.00098,5.1e-05,0.001,0.00098,1,1
|
||||
28690000,0.78,-0.0011,-0.0003,-0.63,2.1,1.6,0.98,4.4,3.3,-3.7e+02,-0.00072,-0.0058,1.3e-05,0.0059,-0.011,-0.12,-0.13,-0.029,0.48,0.0019,-0.076,-0.038,0,0,0.00039,0.0004,0.02,0.18,0.23,0.0068,1.6,1.8,0.034,6e-07,6e-07,5.1e-06,0.031,0.031,0.00013,0.00091,6.8e-05,0.00093,4.8e-05,0.00096,0.00093,1,1
|
||||
28790000,0.78,-0.0015,-2.3e-05,-0.63,2.1,1.6,0.98,4.6,3.5,-3.7e+02,-0.00071,-0.0058,1.3e-05,0.0053,-0.0096,-0.12,-0.13,-0.029,0.48,0.0017,-0.073,-0.037,0,0,0.0004,0.0004,0.02,0.18,0.23,0.0068,1.7,1.9,0.034,6e-07,6e-07,5.1e-06,0.031,0.031,0.00013,0.00088,6.5e-05,0.0009,4.6e-05,0.00092,0.00089,1,1
|
||||
28890000,0.78,-0.0013,2.6e-05,-0.63,2,1.5,0.97,4.8,3.6,-3.7e+02,-0.00071,-0.0058,1.3e-05,0.0047,-0.0081,-0.12,-0.13,-0.03,0.48,0.0016,-0.071,-0.036,0,0,0.0004,0.0004,0.02,0.18,0.23,0.0069,1.8,2.1,0.034,6e-07,6e-07,5.1e-06,0.031,0.031,0.00013,0.00085,6.3e-05,0.00087,4.4e-05,0.00089,0.00087,1,1
|
||||
28990000,0.78,-0.00088,-5.7e-05,-0.63,1.9,1.5,0.97,5,3.8,-3.7e+02,-0.00071,-0.0058,1.4e-05,0.0039,-0.0062,-0.12,-0.14,-0.03,0.48,0.0014,-0.069,-0.034,0,0,0.0004,0.0004,0.02,0.19,0.23,0.0069,1.9,2.2,0.034,6e-07,6.1e-07,5.1e-06,0.031,0.031,0.00013,0.00082,6e-05,0.00084,4.2e-05,0.00086,0.00084,1,1
|
||||
29090000,0.78,-0.00046,-0.00016,-0.63,1.9,1.5,0.96,5.1,3.9,-3.7e+02,-0.0007,-0.0058,1.4e-05,0.0033,-0.0045,-0.12,-0.14,-0.03,0.48,0.0012,-0.067,-0.033,0,0,0.0004,0.0004,0.02,0.19,0.23,0.007,2,2.4,0.034,6e-07,6.1e-07,5.1e-06,0.031,0.031,0.00012,0.0008,5.9e-05,0.00082,4e-05,0.00084,0.00082,1,1
|
||||
29190000,0.78,-0.00012,-0.00026,-0.63,1.8,1.5,0.95,5.3,4.1,-3.7e+02,-0.0007,-0.0058,1.5e-05,0.0025,-0.0026,-0.12,-0.14,-0.031,0.48,0.0013,-0.065,-0.034,0,0,0.0004,0.0004,0.02,0.19,0.23,0.007,2.1,2.5,0.034,6e-07,6.1e-07,5.1e-06,0.031,0.031,0.00012,0.00079,5.7e-05,0.0008,3.9e-05,0.00082,0.0008,1,1
|
||||
29290000,0.78,0.00077,-0.00051,-0.63,1.8,1.4,0.98,5.5,4.2,-3.7e+02,-0.0007,-0.0058,1.5e-05,0.0017,-0.00039,-0.12,-0.14,-0.031,0.47,0.0011,-0.063,-0.033,0,0,0.0004,0.00041,0.02,0.2,0.23,0.0071,2.2,2.6,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00077,5.6e-05,0.00078,3.7e-05,0.00081,0.00078,1,1
|
||||
29390000,0.78,0.0022,-0.00079,-0.63,1.7,1.4,0.99,5.7,4.3,-3.7e+02,-0.00069,-0.0058,1.6e-05,0.00075,0.002,-0.12,-0.14,-0.031,0.47,0.00096,-0.062,-0.033,0,0,0.0004,0.00041,0.02,0.2,0.23,0.0071,2.4,2.8,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00076,5.5e-05,0.00077,3.6e-05,0.00079,0.00077,1,1
|
||||
29490000,0.78,0.0034,-0.0012,-0.63,1.6,1.4,0.99,5.8,4.5,-3.7e+02,-0.00069,-0.0058,1.6e-05,5.5e-05,0.0038,-0.12,-0.14,-0.031,0.47,0.00094,-0.061,-0.032,0,0,0.0004,0.00041,0.02,0.21,0.24,0.0072,2.5,3,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00075,5.4e-05,0.00076,3.5e-05,0.00078,0.00076,1,1
|
||||
29590000,0.78,0.0044,-0.0014,-0.63,1.6,1.4,0.98,6,4.6,-3.7e+02,-0.00069,-0.0058,1.7e-05,-0.001,0.0066,-0.12,-0.14,-0.032,0.47,0.00092,-0.06,-0.032,0,0,0.0004,0.00041,0.02,0.21,0.24,0.0072,2.6,3.1,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00074,5.3e-05,0.00075,3.3e-05,0.00077,0.00075,1,1
|
||||
29690000,0.78,0.0051,-0.0016,-0.63,1.5,1.3,0.98,6.1,4.7,-3.7e+02,-0.00069,-0.0058,1.7e-05,-0.0019,0.0087,-0.12,-0.14,-0.032,0.47,0.00087,-0.059,-0.032,0,0,0.00041,0.00041,0.02,0.22,0.24,0.0072,2.8,3.3,0.034,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00073,5.2e-05,0.00074,3.2e-05,0.00076,0.00074,1,1
|
||||
29790000,0.78,0.0056,-0.0017,-0.63,1.5,1.3,0.97,6.3,4.9,-3.7e+02,-0.00068,-0.0058,1.8e-05,-0.0028,0.011,-0.12,-0.14,-0.032,0.47,0.0009,-0.059,-0.031,0,0,0.00041,0.00041,0.02,0.22,0.25,0.0073,2.9,3.5,0.035,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00072,5.1e-05,0.00073,3.2e-05,0.00076,0.00073,1,1
|
||||
29890000,0.78,0.0058,-0.0018,-0.63,1.5,1.3,0.95,6.4,5,-3.7e+02,-0.00068,-0.0058,1.9e-05,-0.0041,0.015,-0.12,-0.14,-0.032,0.47,0.00083,-0.058,-0.031,0,0,0.00041,0.00041,0.02,0.23,0.25,0.0073,3.1,3.6,0.035,6.1e-07,6.1e-07,5.1e-06,0.031,0.03,0.00012,0.00072,5e-05,0.00073,3.1e-05,0.00075,0.00073,1,1
|
||||
29990000,0.78,0.006,-0.0018,-0.63,1.4,1.3,0.94,6.6,5.1,-3.7e+02,-0.00068,-0.0058,1.9e-05,-0.0052,0.017,-0.12,-0.14,-0.032,0.47,0.00081,-0.058,-0.031,0,0,0.00041,0.00042,0.02,0.24,0.26,0.0073,3.3,3.8,0.035,6.1e-07,6.2e-07,5.1e-06,0.031,0.03,0.00012,0.00071,5e-05,0.00072,3e-05,0.00074,0.00072,1,1
|
||||
30090000,0.78,0.0059,-0.0018,-0.63,1.4,1.3,0.93,6.7,5.2,-3.7e+02,-0.00068,-0.0058,2e-05,-0.0063,0.02,-0.12,-0.15,-0.032,0.47,0.00076,-0.057,-0.031,0,0,0.00041,0.00042,0.02,0.24,0.26,0.0073,3.4,4,0.035,6.1e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.00071,4.9e-05,0.00072,2.9e-05,0.00074,0.00072,1,1
|
||||
30190000,0.78,0.0057,-0.0017,-0.63,1.3,1.2,0.91,6.8,5.4,-3.7e+02,-0.00068,-0.0058,2e-05,-0.0071,0.022,-0.12,-0.15,-0.032,0.47,0.0008,-0.056,-0.031,0,0,0.00041,0.00042,0.02,0.25,0.27,0.0074,3.6,4.2,0.035,6.1e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.0007,4.9e-05,0.00071,2.8e-05,0.00073,0.00071,1,1
|
||||
30290000,0.78,0.0056,-0.0017,-0.63,1.3,1.2,0.9,7,5.5,-3.7e+02,-0.00068,-0.0058,2.1e-05,-0.0079,0.024,-0.12,-0.15,-0.032,0.47,0.00079,-0.055,-0.032,0,0,0.00041,0.00042,0.02,0.26,0.27,0.0074,3.8,4.4,0.035,6.1e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.0007,4.8e-05,0.00071,2.7e-05,0.00073,0.00071,1,1
|
||||
30390000,0.78,0.0054,-0.0016,-0.63,1.3,1.2,0.89,7.1,5.6,-3.7e+02,-0.00067,-0.0058,2.1e-05,-0.0091,0.027,-0.12,-0.15,-0.033,0.47,0.00079,-0.055,-0.031,0,0,0.00041,0.00042,0.02,0.26,0.28,0.0074,4,4.6,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.00069,4.7e-05,0.0007,2.7e-05,0.00072,0.0007,1,1
|
||||
30490000,0.78,0.0051,-0.0015,-0.63,1.2,1.2,0.87,7.2,5.7,-3.7e+02,-0.00067,-0.0058,2.2e-05,-0.0098,0.029,-0.12,-0.15,-0.033,0.47,0.00082,-0.055,-0.031,0,0,0.00041,0.00042,0.02,0.27,0.28,0.0074,4.2,4.9,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.03,0.00011,0.00069,4.7e-05,0.0007,2.6e-05,0.00072,0.0007,1,1
|
||||
30590000,0.78,0.0048,-0.0014,-0.63,1.2,1.2,0.83,7.3,5.8,-3.7e+02,-0.00067,-0.0058,2.2e-05,-0.011,0.032,-0.12,-0.15,-0.033,0.47,0.00087,-0.054,-0.031,0,0,0.00041,0.00042,0.02,0.28,0.29,0.0074,4.4,5.1,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.029,0.00011,0.00068,4.6e-05,0.00069,2.6e-05,0.00071,0.00069,1,1
|
||||
30690000,0.78,0.0045,-0.0013,-0.63,1.1,1.2,0.83,7.5,5.9,-3.7e+02,-0.00067,-0.0058,2.3e-05,-0.012,0.035,-0.12,-0.15,-0.033,0.47,0.00088,-0.054,-0.031,0,0,0.00041,0.00043,0.02,0.29,0.3,0.0074,4.6,5.3,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.029,0.00011,0.00068,4.6e-05,0.00069,2.5e-05,0.00071,0.00069,1,1
|
||||
30790000,0.78,0.0042,-0.0012,-0.63,1.1,1.1,0.82,7.6,6.1,-3.7e+02,-0.00067,-0.0058,2.3e-05,-0.013,0.037,-0.12,-0.15,-0.033,0.47,0.00085,-0.054,-0.031,0,0,0.00041,0.00043,0.02,0.29,0.3,0.0074,4.8,5.6,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.029,0.00011,0.00068,4.6e-05,0.00069,2.5e-05,0.00071,0.00069,1,1
|
||||
30890000,0.78,0.0038,-0.0011,-0.63,1.1,1.1,0.81,7.7,6.2,-3.7e+02,-0.00067,-0.0058,2.4e-05,-0.014,0.039,-0.12,-0.15,-0.033,0.47,0.00087,-0.053,-0.031,0,0,0.00041,0.00043,0.02,0.3,0.31,0.0074,5.1,5.8,0.035,6.2e-07,6.2e-07,5.1e-06,0.031,0.029,0.00011,0.00067,4.5e-05,0.00068,2.4e-05,0.0007,0.00068,1,1
|
||||
30990000,0.78,0.0032,-0.00096,-0.63,1,1.1,0.8,7.8,6.3,-3.7e+02,-0.00067,-0.0058,2.4e-05,-0.015,0.042,-0.12,-0.15,-0.033,0.47,0.00088,-0.053,-0.031,0,0,0.00041,0.00043,0.021,0.31,0.32,0.0074,5.3,6.1,0.035,6.2e-07,6.3e-07,5.1e-06,0.031,0.029,0.00011,0.00067,4.5e-05,0.00068,2.3e-05,0.0007,0.00068,1,1
|
||||
31090000,0.78,0.0028,-0.00081,-0.63,0.99,1.1,0.79,7.9,6.4,-3.7e+02,-0.00067,-0.0058,2.5e-05,-0.016,0.045,-0.12,-0.15,-0.033,0.47,0.00087,-0.053,-0.031,0,0,0.00041,0.00043,0.021,0.32,0.32,0.0074,5.6,6.3,0.035,6.2e-07,6.3e-07,5.1e-06,0.031,0.029,0.00011,0.00067,4.4e-05,0.00068,2.3e-05,0.0007,0.00068,1,1
|
||||
31190000,0.78,0.0026,-0.00071,-0.63,0.95,1.1,0.78,8,6.5,-3.7e+02,-0.00067,-0.0058,2.6e-05,-0.018,0.049,-0.12,-0.15,-0.033,0.47,0.00088,-0.052,-0.031,0,0,0.00041,0.00043,0.021,0.33,0.33,0.0074,5.8,6.6,0.035,6.2e-07,6.3e-07,5.1e-06,0.03,0.029,0.0001,0.00066,4.4e-05,0.00067,2.3e-05,0.00069,0.00067,1,1
|
||||
31290000,0.78,0.0021,-0.00056,-0.63,0.91,1.1,0.78,8.1,6.6,-3.7e+02,-0.00068,-0.0058,2.7e-05,-0.019,0.053,-0.12,-0.15,-0.033,0.47,0.0009,-0.052,-0.032,0,0,0.00041,0.00043,0.021,0.34,0.34,0.0074,6.1,6.9,0.035,6.2e-07,6.3e-07,5.1e-06,0.03,0.029,0.0001,0.00066,4.4e-05,0.00067,2.2e-05,0.00069,0.00067,1,1
|
||||
31390000,0.78,0.0015,-0.00039,-0.63,0.87,1,0.78,8.2,6.7,-3.7e+02,-0.00068,-0.0058,2.7e-05,-0.02,0.056,-0.12,-0.15,-0.033,0.47,0.00091,-0.052,-0.032,0,0,0.00041,0.00044,0.021,0.35,0.35,0.0074,6.4,7.2,0.035,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,0.0001,0.00066,4.3e-05,0.00067,2.2e-05,0.00069,0.00067,1,1
|
||||
31490000,0.78,0.00099,-0.00026,-0.63,0.83,1,0.78,8.3,6.8,-3.7e+02,-0.00068,-0.0058,2.8e-05,-0.022,0.059,-0.11,-0.15,-0.033,0.47,0.00088,-0.051,-0.032,0,0,0.00042,0.00044,0.021,0.36,0.36,0.0074,6.7,7.5,0.036,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,0.0001,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31590000,0.78,0.00066,-0.00015,-0.63,0.8,1,0.77,8.3,6.9,-3.7e+02,-0.00068,-0.0058,2.8e-05,-0.023,0.062,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00044,0.021,0.37,0.37,0.0073,7,7.8,0.035,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,0.0001,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31690000,0.78,6.1e-05,1.1e-05,-0.63,0.76,0.99,0.78,8.4,7,-3.7e+02,-0.00068,-0.0058,2.9e-05,-0.024,0.066,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00044,0.021,0.38,0.38,0.0073,7.3,8.1,0.035,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,0.0001,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31790000,0.78,-0.00061,0.00024,-0.63,0.72,0.97,0.78,8.5,7.1,-3.7e+02,-0.00068,-0.0058,3e-05,-0.026,0.069,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00044,0.021,0.39,0.39,0.0073,7.6,8.5,0.036,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,9.9e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31890000,0.78,-0.0012,0.00039,-0.63,0.68,0.95,0.77,8.6,7.2,-3.7e+02,-0.00068,-0.0058,3e-05,-0.027,0.073,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00045,0.021,0.4,0.4,0.0073,7.9,8.8,0.036,6.3e-07,6.3e-07,5.1e-06,0.03,0.028,9.8e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
31990000,0.78,-0.0016,0.00052,-0.63,0.64,0.93,0.77,8.6,7.3,-3.7e+02,-0.00069,-0.0058,3.1e-05,-0.029,0.076,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00045,0.021,0.41,0.41,0.0072,8.3,9.1,0.035,6.3e-07,6.4e-07,5.1e-06,0.03,0.028,9.8e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32090000,0.78,-0.0023,0.00071,-0.63,0.6,0.91,0.78,8.7,7.4,-3.7e+02,-0.00069,-0.0058,3.2e-05,-0.03,0.08,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00045,0.021,0.42,0.42,0.0072,8.6,9.5,0.036,6.3e-07,6.4e-07,5.1e-06,0.03,0.027,9.7e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32190000,0.78,-0.0031,0.0009,-0.63,0.56,0.9,0.78,8.8,7.5,-3.7e+02,-0.00069,-0.0058,3.3e-05,-0.032,0.085,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00046,0.021,0.43,0.43,0.0072,9,9.9,0.036,6.3e-07,6.4e-07,5.1e-06,0.03,0.027,9.6e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32290000,0.78,-0.0037,0.001,-0.63,0.53,0.88,0.77,8.8,7.6,-3.7e+02,-0.0007,-0.0058,3.3e-05,-0.034,0.089,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00046,0.021,0.44,0.44,0.0072,9.4,10,0.036,6.3e-07,6.4e-07,5.1e-06,0.03,0.027,9.5e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32390000,0.78,-0.0043,0.0012,-0.63,0.48,0.86,0.77,8.9,7.7,-3.7e+02,-0.0007,-0.0058,3.4e-05,-0.035,0.091,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00046,0.021,0.46,0.45,0.0072,9.8,11,0.036,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.5e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32490000,0.78,-0.0044,0.0012,-0.63,0.44,0.84,0.78,9,7.8,-3.7e+02,-0.0007,-0.0058,3.4e-05,-0.036,0.095,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00046,0.021,0.47,0.46,0.0071,10,11,0.036,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.4e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32590000,0.78,-0.0047,0.0013,-0.63,-1.6,-0.84,0.63,-1e+06,1.2e+04,-3.7e+02,-0.0007,-0.0058,3.5e-05,-0.037,0.096,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.25,0.25,0.56,0.25,0.25,0.038,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.3e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32690000,0.78,-0.0047,0.0013,-0.63,-1.6,-0.86,0.62,-1e+06,1.2e+04,-3.7e+02,-0.0007,-0.0058,3.5e-05,-0.038,0.098,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.25,0.25,0.55,0.26,0.26,0.049,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.3e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32790000,0.78,-0.0046,0.0013,-0.63,-1.6,-0.84,0.62,-1e+06,1.2e+04,-3.7e+02,-0.00071,-0.0058,3.4e-05,-0.038,0.1,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.13,0.13,0.27,0.26,0.26,0.049,6.4e-07,6.4e-07,5.1e-06,0.03,0.027,9.2e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32890000,0.78,-0.0045,0.0012,-0.63,-1.6,-0.85,0.6,-1e+06,1.2e+04,-3.7e+02,-0.00071,-0.0058,3.5e-05,-0.039,0.1,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.13,0.13,0.26,0.27,0.27,0.059,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.2e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
32990000,0.78,-0.0044,0.0011,-0.63,-1.6,-0.85,0.6,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,3.3e-05,-0.04,0.1,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.085,0.085,0.17,0.27,0.27,0.057,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.2e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
33090000,0.78,-0.0044,0.0011,-0.63,-1.6,-0.86,0.59,-1e+06,1.2e+04,-3.7e+02,-0.00073,-0.0058,3.4e-05,-0.04,0.11,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.086,0.086,0.16,0.28,0.28,0.065,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.2e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
33190000,0.78,-0.0029,-0.0022,-0.62,-1.6,-0.85,0.53,-1e+06,1.2e+04,-3.7e+02,-0.00076,-0.0058,2.9e-05,-0.041,0.11,-0.11,-0.15,-0.033,0.47,0.00091,-0.051,-0.032,0,0,0.00042,0.00047,0.021,0.065,0.065,0.11,0.28,0.28,0.062,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00065,4.3e-05,0.00066,2.1e-05,0.00068,0.00066,1,1
|
||||
33290000,0.82,-0.00064,-0.014,-0.57,-1.6,-0.87,0.51,-1e+06,1.2e+04,-3.7e+02,-0.00076,-0.0058,2.9e-05,-0.041,0.11,-0.11,-0.15,-0.034,0.47,0.00083,-0.05,-0.032,0,0,0.00043,0.00047,0.021,0.066,0.067,0.11,0.29,0.29,0.067,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00064,4.2e-05,0.00066,2.1e-05,0.00067,0.00066,1,1
|
||||
33390000,0.89,-0.00074,-0.012,-0.46,-1.5,-0.86,0.71,-1e+06,1.2e+04,-3.7e+02,-0.00079,-0.0058,2.4e-05,-0.041,0.11,-0.11,-0.16,-0.035,0.47,0.0006,-0.045,-0.031,0,0,0.00043,0.00048,0.021,0.053,0.054,0.083,0.29,0.29,0.065,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00057,3.9e-05,0.00066,2e-05,0.00059,0.00066,1,1
|
||||
33490000,0.95,0.0012,-0.0053,-0.31,-1.6,-0.88,0.73,-1e+06,1.2e+04,-3.7e+02,-0.00079,-0.0058,2.5e-05,-0.041,0.11,-0.11,-0.17,-0.038,0.47,0.00022,-0.03,-0.032,0,0,0.00043,0.00048,0.021,0.055,0.056,0.075,0.3,0.3,0.068,6.4e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00038,2.9e-05,0.00065,2e-05,0.00039,0.00066,1,1
|
||||
33590000,0.99,-0.0012,4.4e-05,-0.15,-1.5,-0.85,0.69,-1e+06,1.2e+04,-3.7e+02,-0.00084,-0.0058,2.3e-05,-0.041,0.11,-0.11,-0.18,-0.041,0.47,7.4e-05,-0.017,-0.032,0,0,0.00045,0.00047,0.021,0.047,0.048,0.061,0.3,0.3,0.065,6.3e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00021,2.1e-05,0.00065,1.9e-05,0.00022,0.00065,1,1
|
||||
33690000,1,-0.0045,0.0029,0.022,-1.6,-0.88,0.7,-1e+06,1.2e+04,-3.7e+02,-0.00084,-0.0058,2.3e-05,-0.041,0.11,-0.11,-0.19,-0.042,0.47,-0.00012,-0.011,-0.033,0,0,0.00049,0.00046,0.021,0.05,0.052,0.056,0.31,0.31,0.068,6.3e-07,6.4e-07,5.1e-06,0.03,0.026,9.1e-05,0.00013,1.6e-05,0.00065,1.9e-05,0.00013,0.00065,1,1
|
||||
33790000,0.98,-0.0054,0.0036,0.19,-1.6,-0.87,0.69,-1e+06,1.2e+04,-3.7e+02,-0.00092,-0.0059,2.6e-05,-0.041,0.11,-0.11,-0.19,-0.043,0.47,-0.0002,-0.0069,-0.034,0,0,0.0005,0.00044,0.021,0.046,0.048,0.047,0.31,0.31,0.064,6.2e-07,6.3e-07,5.1e-06,0.03,0.026,9.1e-05,7.7e-05,1.3e-05,0.00065,1.8e-05,8e-05,0.00065,1,1
|
||||
33890000,0.94,-0.0063,0.0044,0.35,-1.6,-0.92,0.67,-1e+06,1.2e+04,-3.7e+02,-0.00092,-0.0059,2.6e-05,-0.041,0.11,-0.11,-0.2,-0.044,0.47,-0.00023,-0.0049,-0.034,0,0,0.00052,0.00044,0.021,0.052,0.055,0.043,0.32,0.32,0.065,6.3e-07,6.3e-07,5.1e-06,0.03,0.026,9.1e-05,5.2e-05,1.1e-05,0.00065,1.6e-05,5.3e-05,0.00065,1,1
|
||||
33990000,0.87,-0.0083,0.00062,0.49,-1.6,-0.91,0.65,-1e+06,1.2e+04,-3.7e+02,-0.001,-0.0059,3.8e-05,-0.041,0.11,-0.11,-0.2,-0.044,0.47,-0.00043,-0.0032,-0.034,0,0,0.00048,0.00043,0.02,0.05,0.054,0.036,0.32,0.32,0.062,6.1e-07,6.2e-07,5e-06,0.03,0.026,9.1e-05,3.7e-05,1e-05,0.00065,1.5e-05,3.8e-05,0.00065,1,1
|
||||
34090000,0.81,-0.0099,-0.00096,0.59,-1.6,-0.97,0.66,-1e+06,1.2e+04,-3.7e+02,-0.001,-0.0059,3.8e-05,-0.041,0.11,-0.11,-0.2,-0.045,0.47,-0.00067,-0.0023,-0.034,0,0,0.00047,0.00044,0.02,0.059,0.064,0.034,0.33,0.33,0.063,6.2e-07,6.2e-07,5e-06,0.03,0.026,9.1e-05,2.9e-05,9.2e-06,0.00065,1.4e-05,2.9e-05,0.00065,1,1
|
||||
34190000,0.75,-0.0074,-0.0036,0.66,-1.6,-0.95,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0011,-0.006,5.8e-05,-0.077,0.13,-0.11,-0.2,-0.045,0.47,-0.00085,-0.0013,-0.034,0,0,0.00042,0.00041,0.02,0.057,0.062,0.029,0.33,0.33,0.06,6e-07,6e-07,5e-06,0.028,0.025,9.1e-05,2.3e-05,8.4e-06,0.00065,1.3e-05,2.3e-05,0.00065,1,1
|
||||
34290000,0.72,-0.0049,-0.0025,0.7,-1.6,-0.99,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0011,-0.006,5.8e-05,-0.077,0.13,-0.11,-0.2,-0.046,0.47,-0.001,-0.00057,-0.034,0,0,0.00041,0.00042,0.02,0.068,0.074,0.027,0.34,0.34,0.06,6e-07,6.1e-07,5e-06,0.028,0.025,9.1e-05,1.9e-05,7.8e-06,0.00065,1.2e-05,1.9e-05,0.00065,1,1
|
||||
34390000,0.69,-0.0011,-0.0052,0.72,-1.5,-0.93,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.006,8.9e-05,-0.12,0.16,-0.11,-0.2,-0.046,0.47,-0.0013,0.00011,-0.034,0,0,0.00036,0.00037,0.02,0.064,0.069,0.024,0.34,0.34,0.059,5.9e-07,5.9e-07,5e-06,0.026,0.023,9.1e-05,1.6e-05,7.3e-06,0.00065,1.1e-05,1.6e-05,0.00065,1,1
|
||||
34490000,0.68,0.00076,-0.0041,0.73,-1.5,-0.95,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.006,8.9e-05,-0.12,0.16,-0.11,-0.2,-0.046,0.47,-0.0015,0.00051,-0.034,0,0,0.00035,0.00037,0.02,0.077,0.083,0.022,0.35,0.35,0.058,5.9e-07,5.9e-07,5e-06,0.026,0.023,9.1e-05,1.4e-05,6.9e-06,0.00065,1.1e-05,1.4e-05,0.00065,1,1
|
||||
34590000,0.67,0.0027,-0.0069,0.74,-1.5,-0.87,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0013,-0.0061,0.00012,-0.17,0.19,-0.11,-0.2,-0.047,0.47,-0.0019,0.00096,-0.034,0,0,0.0003,0.00031,0.019,0.07,0.074,0.019,0.35,0.35,0.056,5.8e-07,5.8e-07,5e-06,0.022,0.02,9e-05,1.3e-05,6.5e-06,0.00065,1e-05,1.2e-05,0.00065,1,1
|
||||
34690000,0.66,0.0033,-0.0065,0.75,-1.5,-0.89,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0013,-0.0061,0.00012,-0.17,0.19,-0.11,-0.2,-0.047,0.47,-0.0021,0.0015,-0.034,0,0,0.0003,0.00031,0.019,0.083,0.088,0.018,0.36,0.36,0.056,5.8e-07,5.8e-07,5e-06,0.022,0.02,9e-05,1.2e-05,6.2e-06,0.00065,9.7e-06,1.1e-05,0.00065,1,1
|
||||
34790000,0.66,0.0041,-0.0099,0.75,-1.4,-0.8,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0061,0.00014,-0.2,0.23,-0.11,-0.2,-0.047,0.47,-0.0023,0.002,-0.033,0,0,0.00025,0.00025,0.019,0.073,0.077,0.016,0.36,0.36,0.054,5.7e-07,5.8e-07,4.9e-06,0.019,0.018,9e-05,1.1e-05,6e-06,0.00065,9.2e-06,1e-05,0.00065,1,1
|
||||
34890000,0.66,0.0042,-0.0099,0.75,-1.4,-0.8,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0061,0.00014,-0.2,0.23,-0.11,-0.2,-0.047,0.47,-0.0026,0.0023,-0.033,0,0,0.00025,0.00025,0.019,0.085,0.09,0.015,0.37,0.37,0.053,5.7e-07,5.8e-07,5e-06,0.019,0.018,9e-05,9.7e-06,5.7e-06,0.00064,8.8e-06,9.6e-06,0.00064,1,1
|
||||
34990000,0.65,0.00073,-0.0051,0.76,-2.2,-1.5,-0.19,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.048,0.47,-0.0027,0.0026,-0.033,0,0,0.0002,0.00021,0.019,0.096,0.11,0.014,0.37,0.37,0.052,5.6e-07,5.7e-07,4.9e-06,0.017,0.016,8.9e-05,9.1e-06,5.5e-06,0.00064,8.5e-06,9e-06,0.00064,1,1
|
||||
35090000,0.65,0.00068,-0.0051,0.76,-2.3,-1.5,-0.24,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.048,0.47,-0.003,0.0029,-0.033,0,0,0.0002,0.00021,0.019,0.11,0.12,0.013,0.38,0.38,0.051,5.7e-07,5.7e-07,4.9e-06,0.017,0.016,8.9e-05,8.6e-06,5.3e-06,0.00064,8.2e-06,8.4e-06,0.00064,1,1
|
||||
35190000,0.65,0.00061,-0.0051,0.76,-2.3,-1.5,-0.24,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.048,0.47,-0.0032,0.0032,-0.032,0,0,0.0002,0.00021,0.019,0.12,0.14,0.013,0.39,0.4,0.05,5.7e-07,5.7e-07,4.9e-06,0.017,0.016,8.9e-05,8.1e-06,5.1e-06,0.00064,7.9e-06,7.9e-06,0.00064,1,1
|
||||
35290000,0.65,0.00052,-0.0051,0.76,-2.2,-1.5,-0.23,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.048,0.47,-0.0033,0.0035,-0.032,0,0,0.0002,0.00021,0.019,0.14,0.15,0.012,0.41,0.41,0.049,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,7.7e-06,4.9e-06,0.00064,7.6e-06,7.5e-06,0.00064,1,1
|
||||
35390000,0.65,0.00052,-0.005,0.76,-2.2,-1.5,-0.22,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.048,0.47,-0.0034,0.0036,-0.032,0,0,0.0002,0.00021,0.019,0.15,0.17,0.011,0.43,0.43,0.049,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,7.3e-06,4.8e-06,0.00064,7.4e-06,7.2e-06,0.00064,1,1
|
||||
35490000,0.65,0.00048,-0.005,0.76,-2.2,-1.5,-0.21,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.0036,0.0038,-0.032,0,0,0.0002,0.00021,0.019,0.17,0.18,0.011,0.45,0.46,0.048,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,7e-06,4.7e-06,0.00064,7.1e-06,6.8e-06,0.00064,1,1
|
||||
35590000,0.65,0.00046,-0.005,0.76,-2.2,-1.5,-0.21,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.0038,0.004,-0.032,0,0,0.0002,0.0002,0.019,0.18,0.2,0.01,0.48,0.49,0.047,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,6.7e-06,4.5e-06,0.00064,6.9e-06,6.6e-06,0.00064,1,1
|
||||
35690000,0.65,0.00047,-0.005,0.76,-2.2,-1.5,-0.2,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.0038,0.0041,-0.032,0,0,0.0002,0.0002,0.019,0.2,0.22,0.0096,0.51,0.52,0.047,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,8.9e-05,6.5e-06,4.4e-06,0.00064,6.7e-06,6.3e-06,0.00064,1,1
|
||||
35790000,0.66,0.00043,-0.0049,0.76,-2.2,-1.5,-0.2,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.23,0.26,-0.11,-0.2,-0.049,0.47,-0.004,0.0042,-0.032,0,0,0.0002,0.0002,0.019,0.22,0.24,0.0091,0.55,0.56,0.046,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,6.3e-06,4.3e-06,0.00064,6.6e-06,6.1e-06,0.00064,1,1
|
||||
35890000,0.66,0.00036,-0.0049,0.76,-2.2,-1.5,-0.19,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.049,0.46,-0.0041,0.0043,-0.032,0,0,0.0002,0.0002,0.019,0.24,0.26,0.0087,0.59,0.6,0.045,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,6e-06,4.2e-06,0.00064,6.4e-06,5.9e-06,0.00064,1,1
|
||||
35990000,0.66,0.00035,-0.0049,0.76,-2.2,-1.4,-0.18,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.049,0.46,-0.0042,0.0044,-0.032,0,0,0.0002,0.0002,0.019,0.26,0.28,0.0084,0.64,0.66,0.045,5.7e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,5.9e-06,4.1e-06,0.00064,6.2e-06,5.7e-06,0.00064,1,1
|
||||
36090000,0.66,0.00034,-0.0048,0.76,-2.1,-1.4,-0.18,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.049,0.46,-0.0043,0.0045,-0.032,0,0,0.0002,0.0002,0.019,0.29,0.31,0.008,0.69,0.71,0.044,5.8e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,5.7e-06,4.1e-06,0.00064,6.1e-06,5.5e-06,0.00064,1,1
|
||||
36190000,0.66,0.00031,-0.0048,0.76,-2.1,-1.4,-0.17,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.049,0.46,-0.0043,0.0046,-0.032,0,0,0.0002,0.0002,0.019,0.31,0.33,0.0077,0.76,0.78,0.043,5.8e-07,5.8e-07,4.9e-06,0.017,0.016,9e-05,5.5e-06,4e-06,0.00064,6e-06,5.4e-06,0.00064,1,1
|
||||
36290000,0.66,0.00033,-0.0047,0.76,-2.1,-1.4,-0.16,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.049,0.46,-0.0044,0.0047,-0.032,0,0,0.00019,0.0002,0.019,0.34,0.36,0.0075,0.83,0.86,0.043,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,5.4e-06,3.9e-06,0.00064,5.8e-06,5.3e-06,0.00064,1,1
|
||||
36390000,0.66,0.00029,-0.0047,0.76,-2.1,-1.4,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.049,0.46,-0.0045,0.0048,-0.032,0,0,0.00019,0.0002,0.02,0.36,0.39,0.0072,0.91,0.94,0.042,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,5.3e-06,3.8e-06,0.00064,5.7e-06,5.1e-06,0.00064,1,1
|
||||
36490000,0.66,0.00023,-0.0046,0.76,-2.1,-1.4,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0046,0.0049,-0.032,0,0,0.00019,0.0002,0.02,0.39,0.42,0.0069,1,1,0.041,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,5.2e-06,3.8e-06,0.00064,5.6e-06,5e-06,0.00064,1,1
|
||||
36590000,0.66,0.00024,-0.0045,0.76,-2.1,-1.4,-0.14,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0047,0.005,-0.032,0,0,0.00019,0.0002,0.02,0.42,0.45,0.0067,1.1,1.1,0.041,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,5e-06,3.7e-06,0.00064,5.5e-06,4.9e-06,0.00064,1,1
|
||||
36690000,0.66,0.00025,-0.0045,0.76,-2.1,-1.4,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0047,0.005,-0.032,0,0,0.00019,0.0002,0.02,0.45,0.48,0.0066,1.2,1.3,0.04,5.8e-07,5.9e-07,5e-06,0.017,0.016,9e-05,4.9e-06,3.6e-06,0.00064,5.4e-06,4.8e-06,0.00064,1,1
|
||||
36790000,0.66,0.00022,-0.0044,0.76,-2.1,-1.4,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00016,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0047,0.005,-0.032,0,0,0.00019,0.0002,0.02,0.48,0.51,0.0064,1.3,1.4,0.04,5.8e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.8e-06,3.6e-06,0.00064,5.3e-06,4.7e-06,0.00064,1,1
|
||||
36890000,0.66,0.00021,-0.0043,0.76,-2.1,-1.4,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0048,0.0051,-0.032,0,0,0.00019,0.0002,0.02,0.51,0.54,0.0062,1.5,1.5,0.039,5.8e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.7e-06,3.5e-06,0.00064,5.2e-06,4.6e-06,0.00064,1,1
|
||||
36990000,0.66,0.00025,-0.0043,0.76,-2.1,-1.4,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.2,-0.05,0.46,-0.0048,0.0051,-0.032,0,0,0.00019,0.0002,0.02,0.55,0.58,0.0061,1.6,1.7,0.039,5.8e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.7e-06,3.5e-06,0.00064,5.1e-06,4.5e-06,0.00064,1,1
|
||||
37090000,0.66,0.00027,-0.0042,0.76,-2,-1.4,-0.1,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0049,0.0052,-0.032,0,0,0.00019,0.0002,0.02,0.58,0.61,0.006,1.8,1.9,0.038,5.8e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.6e-06,3.4e-06,0.00064,5e-06,4.4e-06,0.00064,1,1
|
||||
37190000,0.66,0.00026,-0.0042,0.76,-2,-1.4,-0.093,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0049,0.0053,-0.032,0,0,0.00019,0.0002,0.02,0.62,0.65,0.0059,2,2.1,0.038,5.9e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.5e-06,3.4e-06,0.00064,5e-06,4.4e-06,0.00064,1,1
|
||||
37290000,0.66,0.00021,-0.0041,0.76,-2,-1.3,-0.086,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.005,0.0053,-0.032,0,0,0.00019,0.0002,0.02,0.65,0.69,0.0058,2.2,2.3,0.037,5.9e-07,5.9e-07,5e-06,0.017,0.016,9.1e-05,4.4e-06,3.3e-06,0.00064,4.9e-06,4.3e-06,0.00064,1,1
|
||||
37390000,0.66,0.00025,-0.0041,0.76,-2,-1.3,-0.08,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.005,0.0054,-0.032,0,0,0.00019,0.0002,0.02,0.69,0.73,0.0057,2.4,2.5,0.037,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.4e-06,3.3e-06,0.00064,4.8e-06,4.2e-06,0.00064,1,1
|
||||
37490000,0.66,0.00024,-0.004,0.76,-2,-1.3,-0.073,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.005,0.0054,-0.032,0,0,0.00019,0.0002,0.02,0.73,0.77,0.0056,2.6,2.8,0.036,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.3e-06,3.2e-06,0.00064,4.8e-06,4.2e-06,0.00064,1,1
|
||||
37590000,0.66,0.00025,-0.004,0.76,-2,-1.3,-0.065,-1e+06,1.2e+04,-3.7e+02,-0.0015,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.005,0.0055,-0.032,0,0,0.00019,0.0002,0.02,0.77,0.81,0.0056,2.9,3.1,0.036,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.2e-06,3.2e-06,0.00064,4.7e-06,4.1e-06,0.00064,1,1
|
||||
37690000,0.66,0.00019,-0.0039,0.76,-2,-1.3,-0.056,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0051,0.0055,-0.032,0,0,0.00019,0.0002,0.02,0.81,0.85,0.0055,3.2,3.4,0.036,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.2e-06,3.2e-06,0.00064,4.6e-06,4e-06,0.00064,1,1
|
||||
37790000,0.66,0.00013,-0.0039,0.76,-2,-1.3,-0.048,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0051,0.0055,-0.032,0,0,0.00019,0.0002,0.02,0.85,0.9,0.0055,3.5,3.7,0.035,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.1e-06,3.1e-06,0.00064,4.6e-06,4e-06,0.00064,1,1
|
||||
37890000,0.66,0.00011,-0.0039,0.75,-2,-1.3,-0.041,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0051,0.0055,-0.032,0,0,0.00019,0.0002,0.02,0.9,0.94,0.0054,3.8,4,0.035,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4.1e-06,3.1e-06,0.00064,4.5e-06,3.9e-06,0.00064,1,1
|
||||
37990000,0.66,5.5e-05,-0.0039,0.75,-1.9,-1.3,-0.033,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0052,0.0056,-0.032,0,0,0.00019,0.0002,0.02,0.94,0.99,0.0054,4.2,4.4,0.035,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4e-06,3.1e-06,0.00064,4.5e-06,3.9e-06,0.00064,1,1
|
||||
38090000,0.66,2.6e-05,-0.0038,0.75,-1.9,-1.3,-0.023,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0052,0.0056,-0.032,0,0,0.00019,0.0002,0.02,0.99,1,0.0054,4.6,4.8,0.034,5.9e-07,6e-07,5e-06,0.017,0.016,9.1e-05,4e-06,3e-06,0.00064,4.4e-06,3.8e-06,0.00064,1,1
|
||||
38190000,0.66,3.8e-05,-0.0038,0.75,-1.9,-1.3,-0.016,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0052,0.0056,-0.032,0,0,0.00019,0.0002,0.02,1,1.1,0.0053,5,5.2,0.034,6e-07,6e-07,5e-06,0.017,0.016,9.2e-05,3.9e-06,3e-06,0.00064,4.3e-06,3.8e-06,0.00064,1,1
|
||||
38290000,0.66,1.6e-05,-0.0038,0.75,-1.9,-1.3,-0.0091,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0052,0.0056,-0.032,0,0,0.00019,0.0002,0.02,1.1,1.1,0.0053,5.4,5.7,0.034,6e-07,6e-07,5e-06,0.017,0.016,9.2e-05,3.9e-06,3e-06,0.00064,4.3e-06,3.8e-06,0.00064,1,1
|
||||
38390000,0.66,3.2e-05,-0.0038,0.75,-1.9,-1.3,-0.0024,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0053,0.0057,-0.032,0,0,0.00019,0.0002,0.02,1.1,1.2,0.0053,5.9,6.2,0.034,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.9e-06,3e-06,0.00064,4.3e-06,3.7e-06,0.00064,1,1
|
||||
38490000,0.66,1e-05,-0.0037,0.75,-1.9,-1.3,0.0042,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0053,0.0057,-0.032,0,0,0.00019,0.0002,0.02,1.2,1.2,0.0053,6.4,6.7,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.8e-06,2.9e-06,0.00064,4.2e-06,3.7e-06,0.00064,1,1
|
||||
38590000,0.66,5.1e-05,-0.0037,0.75,-1.9,-1.3,0.01,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0053,0.0057,-0.032,0,0,0.00019,0.0002,0.02,1.2,1.3,0.0053,6.9,7.3,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.8e-06,2.9e-06,0.00064,4.2e-06,3.6e-06,0.00064,1,1
|
||||
38690000,0.66,1.1e-05,-0.0036,0.75,-1.9,-1.3,0.017,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0053,0.0058,-0.032,0,0,0.00019,0.0002,0.02,1.3,1.3,0.0053,7.5,7.9,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.7e-06,2.9e-06,0.00064,4.1e-06,3.6e-06,0.00064,1,1
|
||||
38790000,0.66,-5.9e-06,-0.0036,0.75,-1.8,-1.3,0.023,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0054,0.0058,-0.032,0,0,0.00019,0.0002,0.02,1.3,1.4,0.0053,8.1,8.6,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.7e-06,2.8e-06,0.00064,4.1e-06,3.6e-06,0.00064,1,1
|
||||
38890000,0.66,-0.00011,-0.0034,0.75,-1.8,-1.2,0.52,-1e+06,1.2e+04,-3.7e+02,-0.0014,-0.0062,0.00017,-0.22,0.26,-0.11,-0.21,-0.05,0.46,-0.0054,0.0058,-0.032,0,0,0.00019,0.0002,0.02,1.4,1.4,0.0053,8.8,9.2,0.033,6e-07,6.1e-07,5e-06,0.017,0.016,9.2e-05,3.7e-06,2.8e-06,0.00064,4e-06,3.5e-06,0.00064,1,1
|
||||
|
||||
|
@@ -40,3 +40,5 @@ px4_add_library(FlightTaskUtility
|
||||
|
||||
target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib)
|
||||
target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility)
|
||||
|
||||
@@ -40,7 +40,18 @@ using namespace matrix;
|
||||
|
||||
StickTiltXY::StickTiltXY(ModuleParams *parent) :
|
||||
ModuleParams(parent)
|
||||
{}
|
||||
{
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void StickTiltXY::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
// Consider maximum tilt but only between [0.02,3]g sideways acceleration -> ~[1,71]° tilt
|
||||
// Constrain tilt already because tanf(90+°) will give negative result
|
||||
const float maximum_tilt = math::radians(math::constrain(_param_mpc_man_tilt_max.get(), 0.f, 89.f));
|
||||
_maximum_acceleration = math::constrain(tanf(maximum_tilt), .02f, 3.f) * CONSTANTS_ONE_G;
|
||||
}
|
||||
|
||||
Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const float dt, const float yaw,
|
||||
const float yaw_setpoint)
|
||||
@@ -49,5 +60,5 @@ Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const flo
|
||||
_man_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
|
||||
stick_xy = _man_input_filter.update(stick_xy);
|
||||
Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_setpoint);
|
||||
return stick_xy * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
|
||||
return stick_xy * _maximum_acceleration;
|
||||
}
|
||||
|
||||
@@ -63,6 +63,9 @@ public:
|
||||
matrix::Vector2f generateAccelerationSetpoints(matrix::Vector2f stick_xy, const float dt, const float yaw,
|
||||
const float yaw_setpoint);
|
||||
private:
|
||||
void updateParams() override;
|
||||
|
||||
float _maximum_acceleration{0.f};
|
||||
AlphaFilter<matrix::Vector2f> _man_input_filter;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
|
||||
@@ -0,0 +1,102 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include "StickTiltXY.hpp"
|
||||
|
||||
#include <geo/geo.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(StickTiltXYTest, AllZeroCase)
|
||||
{
|
||||
StickTiltXY stick_tilt_xy{nullptr};
|
||||
Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(), 0.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f());
|
||||
}
|
||||
|
||||
TEST(StickTiltXYTest, NormalRollPitchCases)
|
||||
{
|
||||
// Disable autosaving parameters to avoid busy loop in param_set()
|
||||
param_control_autosave(false);
|
||||
|
||||
float value = 45.f;
|
||||
param_set(param_find("MPC_MAN_TILT_MAX"), &value);
|
||||
|
||||
StickTiltXY stick_tilt_xy{nullptr};
|
||||
// Pitch
|
||||
Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 0.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G, 0.f));
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(.5f, 0.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / 2.f, 0.f));
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-.5f, 0.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / 2.f, 0.f));
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, 0.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G, 0.f));
|
||||
// Roll
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, 1.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(0.f, CONSTANTS_ONE_G));
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, .5f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(0.f, CONSTANTS_ONE_G / 2.f));
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, -.5f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(0.f, -CONSTANTS_ONE_G / 2.f));
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, -1.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(0.f, -CONSTANTS_ONE_G));
|
||||
// Roll & Pitch
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 1.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / M_SQRT2_F, CONSTANTS_ONE_G / M_SQRT2_F));
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, -1.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / M_SQRT2_F, -CONSTANTS_ONE_G / M_SQRT2_F));
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, 1.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / M_SQRT2_F, CONSTANTS_ONE_G / M_SQRT2_F));
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, -1.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / M_SQRT2_F, -CONSTANTS_ONE_G / M_SQRT2_F));
|
||||
}
|
||||
|
||||
TEST(StickTiltXYTest, 90degreeCase)
|
||||
{
|
||||
// Disable autosaving parameters to avoid busy loop in param_set()
|
||||
param_control_autosave(false);
|
||||
|
||||
float value = 90.f;
|
||||
param_set(param_find("MPC_MAN_TILT_MAX"), &value);
|
||||
|
||||
StickTiltXY stick_tilt_xy{nullptr};
|
||||
// Pitch
|
||||
// Zero input leads to zero output
|
||||
Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f());
|
||||
// Maximum input leads to the maximum of 3g sideways acceleration
|
||||
acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 0.f), 1.f, 0.f, 0.f);
|
||||
EXPECT_EQ(acc_xy, Vector2f(3.f * CONSTANTS_ONE_G, 0.f));
|
||||
}
|
||||
@@ -92,7 +92,7 @@ public:
|
||||
|
||||
private:
|
||||
bool _input_available{false};
|
||||
matrix::Vector4f _positions; ///< unmodified manual stick inputs
|
||||
matrix::Vector4f _positions; ///< unmodified manual stick inputs that usually move vehicle in x, y, z and yaw direction
|
||||
matrix::Vector4f _positions_expo; ///< modified manual sticks using expo function
|
||||
|
||||
matrix::Vector<float, 6> _aux_positions;
|
||||
|
||||
@@ -102,6 +102,7 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic("px4io_status");
|
||||
add_topic("radio_status");
|
||||
add_topic("rtl_time_estimate", 1000);
|
||||
add_optional_topic("rtl_status", 5000);
|
||||
add_optional_topic("sensor_airflow", 100);
|
||||
add_topic("sensor_combined");
|
||||
add_optional_topic("sensor_correction");
|
||||
|
||||
@@ -35,8 +35,8 @@
|
||||
* Maximal tilt angle in Stabilized or Altitude mode
|
||||
*
|
||||
* @unit deg
|
||||
* @min 0
|
||||
* @max 90
|
||||
* @min 1
|
||||
* @max 70
|
||||
* @decimal 0
|
||||
* @increment 1
|
||||
* @group Multicopter Position Control
|
||||
|
||||
@@ -275,10 +275,10 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
|
||||
uORB::Manager::get_instance()->set_uorb_communicator(
|
||||
uORB::ProtobufChannel::GetInstance());
|
||||
|
||||
param_init();
|
||||
|
||||
px4::WorkQueueManagerStart();
|
||||
|
||||
param_init();
|
||||
|
||||
uORB::ProtobufChannel::GetInstance()->RegisterSendHandler(muorb_func_ptrs.topic_data_func_ptr);
|
||||
|
||||
// Configure the I2C driver function pointers
|
||||
|
||||
@@ -125,6 +125,12 @@ void FeasibilityChecker::updateData()
|
||||
_current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
|
||||
}
|
||||
|
||||
if (_rtl_status_sub.updated()) {
|
||||
rtl_status_s rtl_status = {};
|
||||
_rtl_status_sub.copy(&rtl_status);
|
||||
_has_vtol_approach = rtl_status.has_vtol_approach;
|
||||
}
|
||||
|
||||
param_t handle = param_find("FW_LND_ANG");
|
||||
|
||||
if (handle != PARAM_INVALID) {
|
||||
@@ -577,17 +583,22 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
|
||||
break;
|
||||
|
||||
case 4:
|
||||
result = _has_takeoff == _landing_valid;
|
||||
result = hasMissionBothOrNeitherTakeoffAndLanding();
|
||||
|
||||
if (!result && (_has_takeoff)) {
|
||||
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t");
|
||||
events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Mission rejected: Add Landing item or remove Takeoff");
|
||||
break;
|
||||
|
||||
} else if (!result && (_landing_valid)) {
|
||||
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t");
|
||||
events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Mission rejected: Add Takeoff item or remove Landing");
|
||||
case 5:
|
||||
if (!_is_landed && !_has_vtol_approach) {
|
||||
result = _landing_valid;
|
||||
|
||||
if (!result) {
|
||||
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Landing waypoint/pattern required.");
|
||||
events::send(events::ID("feasibility_mis_in_air_landing_req"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Mission rejected: Landing waypoint/pattern required");
|
||||
}
|
||||
|
||||
} else {
|
||||
result = hasMissionBothOrNeitherTakeoffAndLanding();
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -600,6 +611,23 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
|
||||
return result;
|
||||
}
|
||||
|
||||
bool FeasibilityChecker::hasMissionBothOrNeitherTakeoffAndLanding()
|
||||
{
|
||||
bool result{_has_takeoff == _landing_valid};
|
||||
|
||||
if (!result && (_has_takeoff)) {
|
||||
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t");
|
||||
events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Mission rejected: Add Landing item or remove Takeoff");
|
||||
|
||||
} else if (!result && (_landing_valid)) {
|
||||
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t");
|
||||
events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info},
|
||||
"Mission rejected: Add Takeoff item or remove Landing");
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item)
|
||||
{
|
||||
|
||||
@@ -36,6 +36,7 @@
|
||||
#include "../navigation.h"
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/rtl_status.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
@@ -97,6 +98,7 @@ private:
|
||||
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Subscription _rtl_status_sub{ORB_ID(rtl_status)};
|
||||
|
||||
// parameters
|
||||
float _param_fw_lnd_ang{0.f};
|
||||
@@ -106,6 +108,7 @@ private:
|
||||
|
||||
bool _is_landed{false};
|
||||
float _home_alt_msl{NAN};
|
||||
bool _has_vtol_approach{false};
|
||||
matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
|
||||
matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
|
||||
VehicleType _vehicle_type{VehicleType::RotaryWing};
|
||||
@@ -247,4 +250,8 @@ private:
|
||||
* @return False if the check failed.
|
||||
*/
|
||||
void doMulticopterChecks(mission_item_s &mission_item, const int current_index);
|
||||
|
||||
// Helper functions
|
||||
|
||||
bool hasMissionBothOrNeitherTakeoffAndLanding();
|
||||
};
|
||||
|
||||
@@ -83,7 +83,7 @@ Land::on_active()
|
||||
|
||||
// create a virtual wp 1m in front of the vehicle to track during the backtransition
|
||||
waypoint_from_heading_and_distance(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
_navigator->get_position_setpoint_triplet()->current.yaw, 1.f,
|
||||
_navigator->get_local_position()->heading, 1.f,
|
||||
&pos_sp_triplet->current.lat, &pos_sp_triplet->current.lon);
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
|
||||
@@ -89,10 +89,9 @@ MissionBase::updateDatamanCache()
|
||||
{
|
||||
if ((_mission.count > 0) && (_mission.current_seq != _load_mission_index)) {
|
||||
|
||||
int32_t start_index = _mission.current_seq;
|
||||
int32_t end_index = start_index + _dataman_cache_size_signed;
|
||||
|
||||
end_index = math::max(math::min(end_index, static_cast<int32_t>(_mission.count)), INT32_C(0));
|
||||
const int32_t start_index = math::constrain(_mission.current_seq, INT32_C(0), int32_t(_mission.count) - 1);
|
||||
const int32_t end_index = math::constrain(start_index + _dataman_cache_size_signed, INT32_C(0),
|
||||
int32_t(_mission.count) - 1);
|
||||
|
||||
for (int32_t index = start_index; index != end_index; index += math::signNoZero(_dataman_cache_size_signed)) {
|
||||
|
||||
@@ -115,8 +114,8 @@ void MissionBase::updateMavlinkMission()
|
||||
const bool mission_data_changed = checkMissionDataChanged(new_mission);
|
||||
|
||||
if (new_mission.current_seq < 0) {
|
||||
new_mission.current_seq = math::max(math::min(_mission.current_seq, static_cast<int32_t>(new_mission.count) - 1),
|
||||
INT32_C(0));
|
||||
new_mission.current_seq = math::constrain(_mission.current_seq, INT32_C(0),
|
||||
static_cast<int32_t>(new_mission.count) - 1);
|
||||
}
|
||||
|
||||
_mission = new_mission;
|
||||
@@ -1147,22 +1146,20 @@ void MissionBase::resetMission()
|
||||
}
|
||||
|
||||
/* Set a new mission*/
|
||||
mission_s new_mission{_mission};
|
||||
new_mission.timestamp = hrt_absolute_time();
|
||||
new_mission.current_seq = 0;
|
||||
new_mission.land_start_index = -1;
|
||||
new_mission.land_index = -1;
|
||||
new_mission.count = 0u;
|
||||
new_mission.mission_id = 0u;
|
||||
new_mission.mission_dataman_id = _mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ?
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
_mission.timestamp = hrt_absolute_time();
|
||||
_mission.current_seq = 0;
|
||||
_mission.land_start_index = -1;
|
||||
_mission.land_index = -1;
|
||||
_mission.count = 0u;
|
||||
_mission.mission_id = 0u;
|
||||
_mission.mission_dataman_id = _mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ?
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
|
||||
bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&new_mission),
|
||||
bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&_mission),
|
||||
sizeof(mission_s));
|
||||
|
||||
if (success) {
|
||||
_mission = new_mission;
|
||||
_mission_pub.publish(_mission);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -68,6 +68,7 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
|
||||
* @value 2 Require a landing
|
||||
* @value 3 Require a takeoff and a landing
|
||||
* @value 4 Require both a takeoff and a landing, or neither
|
||||
* @value 5 Same as previous, but require a landing if in air and no valid VTOL landing approach is present
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
|
||||
|
||||
@@ -208,7 +208,7 @@ void Navigator::run()
|
||||
|
||||
if (mission.safe_points_id != safe_points_id) {
|
||||
safe_points_id = mission.safe_points_id;
|
||||
_rtl.updateSafePoints();
|
||||
_rtl.updateSafePoints(safe_points_id);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -294,12 +294,14 @@ void RTL::setRtlTypeAndDestination()
|
||||
|
||||
init_rtl_mission_type();
|
||||
|
||||
uint8_t safe_point_index{0U};
|
||||
|
||||
if (_param_rtl_type.get() != 2) {
|
||||
// check the closest allowed destination.
|
||||
DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
|
||||
PositionYawSetpoint rtl_position;
|
||||
float rtl_alt;
|
||||
findRtlDestination(destination_type, rtl_position, rtl_alt);
|
||||
findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index);
|
||||
|
||||
switch (destination_type) {
|
||||
case DestinationType::DESTINATION_TYPE_MISSION_LAND:
|
||||
@@ -331,9 +333,29 @@ void RTL::setRtlTypeAndDestination()
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Publish rtl status
|
||||
_rtl_status_pub.get().timestamp = hrt_absolute_time();
|
||||
_rtl_status_pub.get().safe_points_id = _safe_points_id;
|
||||
_rtl_status_pub.get().is_evaluation_pending = _dataman_state != DatamanState::UpdateRequestWait;
|
||||
_rtl_status_pub.get().has_vtol_approach = false;
|
||||
|
||||
if ((_param_rtl_type.get() == 0) || (_param_rtl_type.get() == 3)) {
|
||||
_rtl_status_pub.get().has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach;
|
||||
|
||||
} else if (_param_rtl_type.get() == 1) {
|
||||
_rtl_status_pub.get().has_vtol_approach = _one_rally_point_has_land_approach;
|
||||
}
|
||||
|
||||
_rtl_status_pub.get().rtl_type = static_cast<uint8_t>(_rtl_type);
|
||||
_rtl_status_pub.get().safe_point_index = safe_point_index;
|
||||
|
||||
_rtl_status_pub.update();
|
||||
|
||||
}
|
||||
|
||||
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt)
|
||||
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
|
||||
uint8_t &safe_point_index)
|
||||
{
|
||||
// set destination to home per default, then check if other valid landing spot is closer
|
||||
rtl_position.alt = _home_pos_sub.get().alt;
|
||||
@@ -352,8 +374,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
|
||||
float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)};
|
||||
float min_dist;
|
||||
|
||||
_home_has_land_approach = hasVtolLandApproach(rtl_position);
|
||||
|
||||
if (((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) || (vtol_in_fw_mode && (_param_rtl_approach_force.get() == 1)
|
||||
&& !hasVtolLandApproach(rtl_position))) {
|
||||
&& !_home_has_land_approach)) {
|
||||
// Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we forces approach landing for vtol in fw and it is not defined for home.
|
||||
min_dist = FLT_MAX;
|
||||
|
||||
@@ -394,6 +418,8 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
|
||||
|
||||
if (_safe_points_updated) {
|
||||
|
||||
_one_rally_point_has_land_approach = false;
|
||||
|
||||
for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
|
||||
mission_item_s mission_safe_point;
|
||||
|
||||
@@ -416,11 +442,16 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
|
||||
PositionYawSetpoint safepoint_position;
|
||||
setSafepointAsDestination(safepoint_position, mission_safe_point);
|
||||
|
||||
bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)};
|
||||
|
||||
_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
|
||||
|
||||
if (((dist + MIN_DIST_THRESHOLD) < min_dist) && (!vtol_in_fw_mode || (_param_rtl_approach_force.get() == 0)
|
||||
|| hasVtolLandApproach(safepoint_position))) {
|
||||
|| current_safe_point_has_approaches)) {
|
||||
min_dist = dist;
|
||||
rtl_position = safepoint_position;
|
||||
destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
|
||||
safe_point_index = current_seq;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -58,6 +58,7 @@
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/rtl_status.h>
|
||||
#include <uORB/topics/rtl_time_estimate.h>
|
||||
|
||||
class Navigator;
|
||||
@@ -86,7 +87,7 @@ public:
|
||||
|
||||
void set_return_alt_min(bool min) { _enforce_rtl_alt = min; }
|
||||
|
||||
void updateSafePoints() { _initiate_safe_points_updated = true; }
|
||||
void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; }
|
||||
|
||||
private:
|
||||
enum class DestinationType {
|
||||
@@ -109,7 +110,8 @@ private:
|
||||
* @brief Find RTL destination.
|
||||
*
|
||||
*/
|
||||
void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt);
|
||||
void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
|
||||
uint8_t &safe_point_index);
|
||||
|
||||
/**
|
||||
* @brief Set the position of the land start marker in the planned mission as destination.
|
||||
@@ -188,6 +190,9 @@ private:
|
||||
|
||||
RtlType _rtl_type{RtlType::RTL_DIRECT};
|
||||
|
||||
bool _home_has_land_approach; ///< Flag if the home position has a land approach defined
|
||||
bool _one_rally_point_has_land_approach; ///< Flag if a rally point has a land approach defined
|
||||
|
||||
DatamanState _dataman_state{DatamanState::UpdateRequestWait};
|
||||
DatamanState _error_state{DatamanState::UpdateRequestWait};
|
||||
uint32_t _opaque_id{0}; ///< dataman safepoint id: if it does not match, safe points data was updated
|
||||
@@ -197,6 +202,7 @@ private:
|
||||
bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed
|
||||
mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
|
||||
uint32_t _mission_id = 0u;
|
||||
uint32_t _safe_points_id = 0u;
|
||||
|
||||
mission_stats_entry_s _stats;
|
||||
|
||||
@@ -222,4 +228,5 @@ private:
|
||||
uORB::SubscriptionData<wind_s> _wind_sub{ORB_ID(wind)};
|
||||
|
||||
uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
|
||||
uORB::PublicationData<rtl_status_s> _rtl_status_pub{ORB_ID(rtl_status)};
|
||||
};
|
||||
|
||||
@@ -438,15 +438,18 @@ void AutopilotTester::fly_forward_in_posctl()
|
||||
}
|
||||
|
||||
CHECK(_manual_control->start_position_control() == ManualControl::Result::Success);
|
||||
store_home();
|
||||
wait_until_ready();
|
||||
arm();
|
||||
|
||||
// Climb up for 20 seconds
|
||||
for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) {
|
||||
// Climb up for 5 seconds
|
||||
for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) {
|
||||
CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success);
|
||||
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
|
||||
}
|
||||
|
||||
// Fly forward for 60 seconds
|
||||
for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
|
||||
// Fly forward for 10 seconds
|
||||
for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) {
|
||||
CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
|
||||
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
|
||||
}
|
||||
@@ -473,15 +476,18 @@ void AutopilotTester::fly_forward_in_altctl()
|
||||
}
|
||||
|
||||
CHECK(_manual_control->start_altitude_control() == ManualControl::Result::Success);
|
||||
store_home();
|
||||
wait_until_ready();
|
||||
arm();
|
||||
|
||||
// Climb up for 20 seconds
|
||||
for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) {
|
||||
// Climb up for 5 seconds
|
||||
for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) {
|
||||
CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success);
|
||||
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
|
||||
}
|
||||
|
||||
// Fly forward for 60 seconds
|
||||
for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
|
||||
// Fly forward for 10 seconds
|
||||
for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) {
|
||||
CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
|
||||
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
|
||||
}
|
||||
|
||||
@@ -38,9 +38,6 @@ TEST_CASE("Fly forward in position control", "[multicopter][vtol]")
|
||||
{
|
||||
AutopilotTester tester;
|
||||
tester.connect(connection_url);
|
||||
tester.wait_until_ready();
|
||||
tester.store_home();
|
||||
tester.arm();
|
||||
tester.fly_forward_in_posctl();
|
||||
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
|
||||
tester.wait_until_disarmed(until_disarmed_timeout);
|
||||
@@ -51,9 +48,6 @@ TEST_CASE("Fly forward in altitude control", "[multicopter][vtol]")
|
||||
{
|
||||
AutopilotTester tester;
|
||||
tester.connect(connection_url);
|
||||
tester.wait_until_ready();
|
||||
tester.store_home();
|
||||
tester.arm();
|
||||
tester.fly_forward_in_altctl();
|
||||
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
|
||||
tester.wait_until_disarmed(until_disarmed_timeout);
|
||||
|
||||
@@ -34,6 +34,7 @@
|
||||
#include "autopilot_tester.h"
|
||||
#include <chrono>
|
||||
|
||||
static constexpr float acceptance_radius = 0.3f;
|
||||
|
||||
TEST_CASE("Offboard takeoff and land", "[multicopter][offboard]")
|
||||
{
|
||||
@@ -45,7 +46,7 @@ TEST_CASE("Offboard takeoff and land", "[multicopter][offboard]")
|
||||
tester.set_rc_loss_exception(AutopilotTester::RcLossException::Offboard);
|
||||
tester.arm();
|
||||
std::chrono::seconds goto_timeout = std::chrono::seconds(90);
|
||||
tester.offboard_goto(takeoff_position, 0.1f, goto_timeout);
|
||||
tester.offboard_goto(takeoff_position, acceptance_radius, goto_timeout);
|
||||
tester.offboard_land();
|
||||
tester.wait_until_disarmed(std::chrono::seconds(120));
|
||||
tester.check_home_within(1.0f);
|
||||
@@ -63,12 +64,12 @@ TEST_CASE("Offboard position control", "[multicopter][offboard]")
|
||||
tester.store_home();
|
||||
tester.set_rc_loss_exception(AutopilotTester::RcLossException::Offboard);
|
||||
tester.arm();
|
||||
std::chrono::seconds goto_timeout = std::chrono::seconds(120);
|
||||
tester.offboard_goto(takeoff_position, 0.1f, goto_timeout);
|
||||
tester.offboard_goto(setpoint_1, 0.1f, goto_timeout);
|
||||
tester.offboard_goto(setpoint_2, 0.1f, goto_timeout);
|
||||
tester.offboard_goto(setpoint_3, 0.1f, goto_timeout);
|
||||
tester.offboard_goto(takeoff_position, 0.1f, goto_timeout);
|
||||
std::chrono::seconds goto_timeout = std::chrono::seconds(10);
|
||||
tester.offboard_goto(takeoff_position, acceptance_radius, goto_timeout);
|
||||
tester.offboard_goto(setpoint_1, acceptance_radius, goto_timeout);
|
||||
tester.offboard_goto(setpoint_2, acceptance_radius, goto_timeout);
|
||||
tester.offboard_goto(setpoint_3, acceptance_radius, goto_timeout);
|
||||
tester.offboard_goto(takeoff_position, acceptance_radius, goto_timeout);
|
||||
tester.offboard_land();
|
||||
tester.wait_until_disarmed(std::chrono::seconds(120));
|
||||
tester.check_home_within(1.0f);
|
||||
|
||||
Reference in New Issue
Block a user