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

AP_Compass: Send message to gcs for compass learn

Browse Source
This commit is contained in:
Kawamura 2021-03-03 15:30:24 +09:00 committed by Peter Barker
parent 2261f94361
commit b305b3b074
2 changed files with 31 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_Compass/Compass_learn.cpp
View File

@ -15,6 +15,10 @@
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
static const uint8_t COMPASS_LEARN_NUM_SAMPLES = 30;
static const uint8_t COMPASS_LEARN_BEST_ERROR_THRESHOLD = 50;
static const uint8_t COMPASS_LEARN_WORST_ERROR_THRESHOLD = 65;
// constructor // constructor
CompassLearn::CompassLearn(Compass &_compass) : CompassLearn::CompassLearn(Compass &_compass) :
compass(_compass) compass(_compass)
@ -74,7 +78,7 @@ void CompassLearn::update(void)
for (uint16_t i=0; i<num_sectors; i++) { for (uint16_t i=0; i<num_sectors; i++) {
errors[i] = intensity; errors[i] = intensity;
} }
gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: have earth field"); gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: have earth field");
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void)); hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void));
} }
@ -104,16 +108,16 @@ void CompassLearn::update(void)
} }
if (sample_available) { if (sample_available) {
// @LoggerMessage: COFS // @LoggerMessage: COFS
// @Description: Current compass learn offsets // @Description: Current compass learn offsets
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
// @Field: OfsX: best learnt offset, x-axis // @Field: OfsX: best learnt offset, x-axis
// @Field: OfsY: best learnt offset, y-axis // @Field: OfsY: best learnt offset, y-axis
// @Field: OfsZ: best learnt offset, z-axis // @Field: OfsZ: best learnt offset, z-axis
// @Field: Var: error of best offset vector // @Field: Var: error of best offset vector
// @Field: Yaw: best learnt yaw // @Field: Yaw: best learnt yaw
// @Field: WVar: error of best learn yaw // @Field: WVar: error of best learn yaw
// @Field: N: number of samples used // @Field: N: number of samples used
AP::logger().Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI", AP::logger().Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
AP_HAL::micros64(), AP_HAL::micros64(),
(double)best_offsets.x, (double)best_offsets.x,
@ -143,7 +147,9 @@ void CompassLearn::update(void)
} }
// stop updating the offsets once converged // stop updating the offsets once converged
if (num_samples > 30 && best_error < 50 && worst_error > 65) { if (num_samples > COMPASS_LEARN_NUM_SAMPLES &&
best_error < COMPASS_LEARN_BEST_ERROR_THRESHOLD &&
worst_error > COMPASS_LEARN_WORST_ERROR_THRESHOLD) {
// set the offsets and enable compass for EKF use. Let the // set the offsets and enable compass for EKF use. Let the
// EKF learn the remaining compass offset error // EKF learn the remaining compass offset error
for (uint8_t i=0; i<compass.get_count(); i++) { for (uint8_t i=0; i<compass.get_count(); i++) {
@ -246,7 +252,16 @@ void CompassLearn::process_sample(const struct sample &s)
best_error = bestv; best_error = bestv;
worst_error = worstv; worst_error = worstv;
best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors)); best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors));
// send current learn state to gcs
const uint32_t now = AP_HAL::millis();
if (!converged && now - last_learn_progress_sent_ms >= 5000) {
float percent = (MIN(num_samples / COMPASS_LEARN_NUM_SAMPLES, 1.0f) +
MIN(COMPASS_LEARN_BEST_ERROR_THRESHOLD / (best_error + 1.0f), 1.0f) +
MIN(worst_error / COMPASS_LEARN_WORST_ERROR_THRESHOLD, 1.0f)) / 3.0f * 100.f;
gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: %d%%", (int) percent);
last_learn_progress_sent_ms = now;
}
} }
#endif // COMPASS_LEARN_ENABLED #endif // COMPASS_LEARN_ENABLED

3
libraries/AP_Compass/Compass_learn.h
View File

@ -52,6 +52,9 @@ private:
float worst_error; float worst_error;
bool converged; bool converged;
// notification
uint32_t last_learn_progress_sent_ms;
void io_timer(void); void io_timer(void);
void process_sample(const struct sample &s); void process_sample(const struct sample &s);
}; };