Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Refactor battery current monitoring

This commit is contained in:
Michael du Breuil 2019-07-07 14:36:07 +00:00 committed by WickedShell
parent be25a703c0
commit b07d65c1f3
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/compassmot.cpp
View File

@ -75,8 +75,10 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
// disable cpu failsafe
failsafe_disable();
float current;
// default compensation type to use current if possible
if (battery.has_current()) {
if (battery.current_amps(current)) {
comp_type = AP_COMPASS_MOT_COMP_CURRENT;
} else {
comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
@ -157,6 +159,11 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
throttle_pct = (float)channel_throttle->get_control_in() / 1000.0f;
throttle_pct = constrain_float(throttle_pct,0.0f,1.0f);
if (!battery.current_amps(current)) {
current = 0;
}
current_amps_max = MAX(current_amps_max, current);
// if throttle is near zero, update base x,y,z values
if (throttle_pct <= 0.0f) {
for (uint8_t i=0; i<compass.get_count(); i++) {
@ -182,11 +189,9 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
} else {
// for each compass
for (uint8_t i=0; i<compass.get_count(); i++) {
// current based compensation if more than 3amps being drawn
motor_impact_scaled[i] = motor_impact[i] / battery.current_amps();
// adjust the motor compensation to negate the impact if drawing over 3amps
if (battery.current_amps() >= 3.0f) {
if (current >= 3.0f) {
motor_impact_scaled[i] = motor_impact[i] / current;
motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
updated = true;
}
@ -206,16 +211,15 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
}
}
// record maximum throttle and current
// record maximum throttle
throttle_pct_max = MAX(throttle_pct_max, throttle_pct);
current_amps_max = MAX(current_amps_max, battery.current_amps());
}
if (AP_HAL::millis() - last_send_time > 500) {
last_send_time = AP_HAL::millis();
mavlink_msg_compassmot_status_send(gcs_chan.get_chan(),
channel_throttle->get_control_in(),
battery.current_amps(),
current,
interference_pct[compass.get_primary()],
motor_compensation[compass.get_primary()].x,
motor_compensation[compass.get_primary()].y,