10 #if FRAME_CONFIG == HELI_FRAME 12 return MAV_RESULT_UNSUPPORTED;
20 float throttle_pct_max = 0.0f;
21 float current_amps_max = 0.0f;
23 uint32_t last_run_time;
24 uint32_t last_send_time;
31 return MAV_RESULT_TEMPORARILY_REJECTED;
33 ap.compass_mot =
true;
38 interference_pct[i] = 0.0f;
45 gcs_chan.
send_text(MAV_SEVERITY_CRITICAL,
"Compass disabled");
46 ap.compass_mot =
false;
47 return MAV_RESULT_TEMPORARILY_REJECTED;
54 gcs_chan.
send_text(MAV_SEVERITY_CRITICAL,
"Check compass");
55 ap.compass_mot =
false;
56 return MAV_RESULT_TEMPORARILY_REJECTED;
62 gcs_chan.
send_text(MAV_SEVERITY_CRITICAL,
"RC not calibrated");
63 ap.compass_mot =
false;
64 return MAV_RESULT_TEMPORARILY_REJECTED;
70 gcs_chan.
send_text(MAV_SEVERITY_CRITICAL,
"Throttle not zero");
71 ap.compass_mot =
false;
72 return MAV_RESULT_TEMPORARILY_REJECTED;
76 if (!
ap.land_complete) {
77 gcs_chan.
send_text(MAV_SEVERITY_CRITICAL,
"Not landed");
78 ap.compass_mot =
false;
79 return MAV_RESULT_TEMPORARILY_REJECTED;
96 mavlink_msg_command_ack_send(chan, MAV_CMD_PREFLIGHT_CALIBRATION,0);
102 gcs_chan.
send_text(MAV_SEVERITY_INFO,
"Starting calibration");
106 gcs_chan.
send_text(MAV_SEVERITY_INFO,
"Current");
108 gcs_chan.
send_text(MAV_SEVERITY_INFO,
"Throttle");
122 while (
millis() - last_run_time < 500 ) {
131 interference_pct[i] = 0.0f;
141 last_send_time =
millis();
146 if (
millis() - last_run_time < 20) {
173 if (throttle_pct <= 0.0
f) {
191 motor_impact_scaled[i] = motor_impact[i] / throttle_pct;
193 motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
205 motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
225 throttle_pct_max = MAX(throttle_pct_max, throttle_pct);
231 mavlink_msg_compassmot_status_send(chan,
253 gcs_chan.
send_text(MAV_SEVERITY_INFO,
"Calibration successful");
256 gcs_chan.
send_text(MAV_SEVERITY_NOTICE,
"Failed");
273 ap.compass_mot =
false;
275 return MAV_RESULT_ACCEPTED;
276 #endif // FRAME_CONFIG != HELI_FRAME #define AP_COMPASS_MOT_COMP_DISABLED
MAV_RESULT mavlink_compassmot(mavlink_channel_t chan)
#define AP_COMPASS_MOT_COMP_THROTTLE
void set_motor_compensation(uint8_t i, const Vector3f &motor_comp_factor)
bool rc_calibration_checks(bool display_failure) override
uint8_t get_primary(void) const
const Vector3f & get_field(uint8_t i) const
virtual void delay(uint16_t ms)=0
RC_Channel * channel_throttle
int16_t get_control_in() const
bool has_current(uint8_t instance) const
GCS_MAVLINK_Copter & chan(const uint8_t ofs) override
bool healthy[COMPASS_MAX_INSTANCES]
float current_amps(uint8_t instance) const
void send_text(MAV_SEVERITY severity, const char *fmt,...)
uint16_t compass_magfield_expected() const
uint8_t get_count(void) const
float constrain_float(const float amt, const float low, const float high)
#define COMPASS_MAX_INSTANCES
uint8_t command_ack_counter
void motor_compensation_type(const uint8_t comp_type)
static struct notify_flags_and_values_type flags
AP_Int8 failsafe_throttle
#define AP_COMPASS_MOT_COMP_CURRENT
void enable_motor_output()
AP_HAL::Scheduler * scheduler
void save_motor_compensation()