Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: compassmot format fixes

Browse Source
This commit is contained in:
Randy Mackay 2018-08-02 11:34:11 +09:00
parent 13f96bcb00
commit 6b50a810f6
1 changed files with 10 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduCopter/compassmot.cpp
View File

@ -29,7 +29,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
if (ap.compass_mot) { if (ap.compass_mot) {
// ignore restart messages // ignore restart messages
return MAV_RESULT_TEMPORARILY_REJECTED; return MAV_RESULT_TEMPORARILY_REJECTED;
}else{ } else {
ap.compass_mot = true; ap.compass_mot = true;
} }
@ -88,7 +88,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
// default compensation type to use current if possible // default compensation type to use current if possible
if (battery.has_current()) { if (battery.has_current()) {
comp_type = AP_COMPASS_MOT_COMP_CURRENT; comp_type = AP_COMPASS_MOT_COMP_CURRENT;
}else{ } else {
comp_type = AP_COMPASS_MOT_COMP_THROTTLE; comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
} }
@ -104,7 +104,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
// inform what type of compensation we are attempting // inform what type of compensation we are attempting
if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) { if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
gcs_chan.send_text(MAV_SEVERITY_INFO, "Current"); gcs_chan.send_text(MAV_SEVERITY_INFO, "Current");
} else{ } else {
gcs_chan.send_text(MAV_SEVERITY_INFO, "Throttle"); gcs_chan.send_text(MAV_SEVERITY_INFO, "Throttle");
} }
@ -153,18 +153,18 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
// read radio input // read radio input
read_radio(); read_radio();
// pass through throttle to motors // pass through throttle to motors
SRV_Channels::cork(); SRV_Channels::cork();
motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f); motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f);
SRV_Channels::push(); SRV_Channels::push();
// read some compass values // read some compass values
compass.read(); compass.read();
// read current // read current
battery.read(); battery.read();
// calculate scaling for throttle // calculate scaling for throttle
throttle_pct = (float)channel_throttle->get_control_in() / 1000.0f; throttle_pct = (float)channel_throttle->get_control_in() / 1000.0f;
throttle_pct = constrain_float(throttle_pct,0.0f,1.0f); throttle_pct = constrain_float(throttle_pct,0.0f,1.0f);
@ -174,8 +174,6 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
for (uint8_t i=0; i<compass.get_count(); i++) { for (uint8_t i=0; i<compass.get_count(); i++) {
compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f; compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f;
} }
// causing printing to happen as soon as throttle is lifted
} else { } else {
// calculate diff from compass base and scale with throttle // calculate diff from compass base and scale with throttle
@ -192,14 +190,13 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
// adjust the motor compensation to negate the impact // adjust the motor compensation to negate the impact
motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f; motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
} }
updated = true; updated = true;
} else { } else {
// for each compass // for each compass
for (uint8_t i=0; i<compass.get_count(); i++) { for (uint8_t i=0; i<compass.get_count(); i++) {
// current based compensation if more than 3amps being drawn // current based compensation if more than 3amps being drawn
motor_impact_scaled[i] = motor_impact[i] / battery.current_amps(); motor_impact_scaled[i] = motor_impact[i] / battery.current_amps();
// adjust the motor compensation to negate the impact if drawing over 3amps // adjust the motor compensation to negate the impact if drawing over 3amps
if (battery.current_amps() >= 3.0f) { if (battery.current_amps() >= 3.0f) {
motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f; motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
@ -214,7 +211,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
// interference is impact@fullthrottle / mag field * 100 // interference is impact@fullthrottle / mag field * 100
interference_pct[i] = motor_compensation[i].length() / (float)arming.compass_magfield_expected() * 100.0f; interference_pct[i] = motor_compensation[i].length() / (float)arming.compass_magfield_expected() * 100.0f;
} }
}else{ } else {
for (uint8_t i=0; i<compass.get_count(); i++) { for (uint8_t i=0; i<compass.get_count(); i++) {
// interference is impact/amp * (max current seen / max throttle seen) / mag field * 100 // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)arming.compass_magfield_expected() * 100.0f; interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)arming.compass_magfield_expected() * 100.0f;
@ -224,8 +221,8 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
// record maximum throttle and current // record maximum throttle and current
throttle_pct_max = MAX(throttle_pct_max, throttle_pct); throttle_pct_max = MAX(throttle_pct_max, throttle_pct);
current_amps_max = MAX(current_amps_max, battery.current_amps()); current_amps_max = MAX(current_amps_max, battery.current_amps());
} }
if (AP_HAL::millis() - last_send_time > 500) { if (AP_HAL::millis() - last_send_time > 500) {
last_send_time = AP_HAL::millis(); last_send_time = AP_HAL::millis();
mavlink_msg_compassmot_status_send(chan, mavlink_msg_compassmot_status_send(chan,
@ -275,4 +272,3 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
#endif // FRAME_CONFIG != HELI_FRAME #endif // FRAME_CONFIG != HELI_FRAME
} }