mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-07 16:33:58 -04:00
AP_Compass: make compass LearnType enum-class and parameter AP_Enum
This commit is contained in:
parent
a2322abe02
commit
6298f7dc58
@ -56,7 +56,7 @@ extern const AP_HAL::HAL& hal;
|
||||
#endif
|
||||
|
||||
#ifndef COMPASS_LEARN_DEFAULT
|
||||
#define COMPASS_LEARN_DEFAULT Compass::LEARN_NONE
|
||||
#define COMPASS_LEARN_DEFAULT Compass::LearnType::NONE
|
||||
#endif
|
||||
|
||||
#ifndef AP_COMPASS_OFFSETS_MAX_DEFAULT
|
||||
@ -119,7 +119,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @Description: Enable or disable the automatic learning of compass offsets. You can enable learning either using a compass-only method that is suitable only for fixed wing aircraft or using the offsets learnt by the active EKF state estimator. If this option is enabled then the learnt offsets are saved when you disarm the vehicle. If InFlight learning is enabled then the compass with automatically start learning once a flight starts (must be armed). While InFlight learning is running you cannot use position control modes.
|
||||
// @Values: 0:Disabled,2:EKF-Learning,3:InFlight-Learning
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("LEARN", 3, Compass, _learn, COMPASS_LEARN_DEFAULT),
|
||||
AP_GROUPINFO("LEARN", 3, Compass, _learn, float(COMPASS_LEARN_DEFAULT)),
|
||||
#endif
|
||||
|
||||
#ifndef HAL_BUILD_AP_PERIPH
|
||||
@ -1793,11 +1793,11 @@ Compass::read(void)
|
||||
any_healthy |= _state[i].healthy;
|
||||
}
|
||||
#if COMPASS_LEARN_ENABLED
|
||||
if (_learn == LEARN_INFLIGHT && !learn_allocated) {
|
||||
if (_learn == LearnType::INFLIGHT && !learn_allocated) {
|
||||
learn_allocated = true;
|
||||
learn = NEW_NOTHROW CompassLearn(*this);
|
||||
}
|
||||
if (_learn == LEARN_INFLIGHT && learn != nullptr) {
|
||||
if (_learn == LearnType::INFLIGHT && learn != nullptr) {
|
||||
learn->update();
|
||||
}
|
||||
#endif
|
||||
@ -1985,7 +1985,7 @@ Compass::use_for_yaw(uint8_t i) const
|
||||
// when we are doing in-flight compass learning the state
|
||||
// estimator must not use the compass. The learning code turns off
|
||||
// inflight learning when it has converged
|
||||
return _use_for_yaw[Priority(i)] && _learn.get() != LEARN_INFLIGHT;
|
||||
return _use_for_yaw[Priority(i)] && _learn != LearnType::INFLIGHT;
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -226,7 +226,7 @@ public:
|
||||
#endif // AP_COMPASS_DIAGONALS_ENABLED
|
||||
|
||||
// learn offsets accessor
|
||||
bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; }
|
||||
bool learn_offsets_enabled() const { return _learn == LearnType::INFLIGHT; }
|
||||
|
||||
/// return true if the compass should be used for yaw calculations
|
||||
bool use_for_yaw(uint8_t i) const;
|
||||
@ -318,24 +318,24 @@ public:
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
enum LearnType {
|
||||
LEARN_NONE=0,
|
||||
// LEARN_INTERNAL=1,
|
||||
LEARN_EKF=2,
|
||||
LEARN_INFLIGHT=3
|
||||
enum class LearnType {
|
||||
NONE = 0,
|
||||
// INTERNAL = 1,
|
||||
COPY_FROM_EKF = 2,
|
||||
INFLIGHT = 3,
|
||||
};
|
||||
|
||||
// return the chosen learning type
|
||||
enum LearnType get_learn_type(void) const {
|
||||
return (enum LearnType)_learn.get();
|
||||
LearnType get_learn_type(void) const {
|
||||
return (LearnType)_learn.get();
|
||||
}
|
||||
|
||||
// set the learning type
|
||||
void set_learn_type(enum LearnType type, bool save) {
|
||||
void set_learn_type(LearnType type, bool save) {
|
||||
if (save) {
|
||||
_learn.set_and_save((int8_t)type);
|
||||
_learn.set_and_save(type);
|
||||
} else {
|
||||
_learn.set((int8_t)type);
|
||||
_learn.set(type);
|
||||
}
|
||||
}
|
||||
|
||||
@ -520,7 +520,7 @@ private:
|
||||
uint8_t _unreg_compass_count;
|
||||
|
||||
// settable parameters
|
||||
AP_Int8 _learn;
|
||||
AP_Enum<LearnType> _learn;
|
||||
|
||||
// board orientation from AHRS
|
||||
enum Rotation _board_orientation = ROTATION_NONE;
|
||||
|
@ -105,7 +105,7 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
|
||||
}
|
||||
|
||||
// disable compass learning both for calibration and after completion
|
||||
_learn.set_and_save(0);
|
||||
_learn.set_and_save(LearnType::NONE);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
@ -29,7 +29,7 @@ CompassLearn::CompassLearn(Compass &_compass) :
|
||||
void CompassLearn::update(void)
|
||||
{
|
||||
const AP_Vehicle *vehicle = AP::vehicle();
|
||||
if (compass.get_learn_type() != Compass::LEARN_INFLIGHT ||
|
||||
if (compass.get_learn_type() != Compass::LearnType::INFLIGHT ||
|
||||
!hal.util->get_soft_armed() ||
|
||||
vehicle->get_time_flying_ms() < 3000) {
|
||||
// only learn when flying and with enough time to be clear of
|
||||
@ -65,7 +65,7 @@ void CompassLearn::update(void)
|
||||
const bool result = compass.mag_cal_fixed_yaw(degrees(yaw_rad), (1U<<HAL_COMPASS_MAX_SENSORS)-1, 0, 0, true);
|
||||
if (result) {
|
||||
AP_Notify::flags.compass_cal_running = false;
|
||||
compass.set_learn_type(Compass::LEARN_NONE, true);
|
||||
compass.set_learn_type(Compass::LearnType::NONE, true);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CompassLearn: Finished");
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user