aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: Added is_still() check

Browse Source 
very strict check that all axis are not vibrating much at all
new param: INS_STILL_THRESH used to be a vibration threshold for different platforms
// @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
This commit is contained in:
Tom Pittenger 2015-07-29 12:34:48 -07:00 committed by Andrew Tridgell
parent fb1bb3f571
commit 2620a57700
2 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -27,12 +27,15 @@ extern const AP_HAL::HAL& hal;
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
#define DEFAULT_GYRO_FILTER 20
#define DEFAULT_ACCEL_FILTER 20
#define DEFAULT_STILL_THRESH 2.5f
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
#define DEFAULT_GYRO_FILTER 10
#define DEFAULT_ACCEL_FILTER 10
#define DEFAULT_STILL_THRESH 0.1f
#else
#define DEFAULT_GYRO_FILTER 20
#define DEFAULT_ACCEL_FILTER 20
#define DEFAULT_STILL_THRESH 0.1f
#endif
#define SAMPLE_UNIT 1
@ -289,6 +292,15 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
AP_GROUPINFO("USE3", 22, AP_InertialSensor, _use[2], 0),
#endif
#if INS_VIBRATION_CHECK
// @Param: STILL_THRESH
// @DisplayName: Stillness threshold for detecting if we are moving
// @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
// @Range: 0.05 to 50
// @User: Advanced
AP_GROUPINFO("STILL_THRESH", 23, AP_InertialSensor, _still_threshold, DEFAULT_STILL_THRESH),
#endif
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully
@ -1499,3 +1511,16 @@ Vector3f AP_InertialSensor::get_vibration_levels(uint8_t instance) const
return vibe;
}
#endif
// check for vibration movement. Return true if all axis show nearly zero movement
bool AP_InertialSensor::is_still()
{
#if INS_VIBRATION_CHECK
Vector3f vibe = get_vibration_levels();
return (vibe.x < _still_threshold) &&
(vibe.y < _still_threshold) &&
(vibe.z < _still_threshold);
#else
return false;
#endif
}

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -224,6 +224,9 @@ public:
uint32_t get_accel_clip_count(uint8_t instance) const;
#endif
// check for vibration movement. True when all axis show nearly zero movement
bool is_still();
/*
HIL set functions. The minimum for HIL is set_accel() and
set_gyro(). The others are option for higher fidelity log
@ -355,6 +358,9 @@ private:
uint32_t _accel_clip_count[INS_MAX_INSTANCES];
LowPassFilterVector3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES];
LowPassFilterVector3f _accel_vibe_filter[INS_VIBRATION_CHECK_INSTANCES];
// threshold for detecting stillness
AP_Float _still_threshold;
#endif
/*