forked from Archive/PX4-Autopilot
Scale throttle outputs by battery level. Fixes #4751
This commit is contained in:
Dennis Shtatnov
Lorenz Meier
parent
a94c8d7812
commit
a634c14582
|
|
@ -4,6 +4,7 @@ float32 current_a # Battery current in amperes, -1 if unknown
|
|||
float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown
|
||||
float32 discharged_mah # Discharged amount in mAh, -1 if unknown
|
||||
float32 remaining # From 1 to 0, -1 if unknown
|
||||
float32 scale # Power scaling factor with >= 1, or -1 if unknown
|
||||
int32 cell_count # Number of cells
|
||||
bool connected # Wether or not a battery is connected
|
||||
#bool is_powering_off # Power off event imminent indication, false if unknown
|
||||
|
|
|
|||
|
|
@ -80,6 +80,7 @@
|
|||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <uORB/topics/mc_att_ctrl_status.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
|
@ -141,6 +142,7 @@ private:
|
|||
int _armed_sub; /**< arming status subscription */
|
||||
int _vehicle_status_sub; /**< vehicle status subscription */
|
||||
int _motor_limits_sub; /**< motor limits subscription */
|
||||
int _battery_status_sub; /**< battery status subscription */
|
||||
|
||||
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
|
||||
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
|
||||
|
|
@ -161,6 +163,7 @@ private:
|
|||
struct vehicle_status_s _vehicle_status; /**< vehicle status */
|
||||
struct multirotor_motor_limits_s _motor_limits; /**< motor limits */
|
||||
struct mc_att_ctrl_status_s _controller_status; /**< controller status */
|
||||
struct battery_status_s _battery_status; /**< battery status */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
perf_counter_t _controller_latency_perf;
|
||||
|
|
@ -209,6 +212,8 @@ private:
|
|||
param_t vtol_opt_recovery_enabled;
|
||||
param_t vtol_wv_yaw_rate_scale;
|
||||
|
||||
param_t bat_scale_en;
|
||||
|
||||
} _params_handles; /**< handles for interesting parameters */
|
||||
|
||||
struct {
|
||||
|
|
@ -233,6 +238,8 @@ private:
|
|||
int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
|
||||
bool vtol_opt_recovery_enabled;
|
||||
float vtol_wv_yaw_rate_scale; /**< Scale value [0, 1] for yaw rate setpoint */
|
||||
|
||||
int bat_scale_en;
|
||||
} _params;
|
||||
|
||||
TailsitterRecovery *_ts_opt_recovery; /**< Computes optimal rates for tailsitter recovery */
|
||||
|
|
@ -292,6 +299,11 @@ private:
|
|||
*/
|
||||
void vehicle_motor_limits_poll();
|
||||
|
||||
/**
|
||||
* Check for battery status updates.
|
||||
*/
|
||||
void battery_status_poll();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
|
|
@ -365,7 +377,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
_params.rattitude_thres = 1.0f;
|
||||
_params.vtol_opt_recovery_enabled = false;
|
||||
_params.vtol_wv_yaw_rate_scale = 1.0f;
|
||||
|
||||
_params.bat_scale_en = 0;
|
||||
|
||||
_rates_prev.zero();
|
||||
_rates_sp.zero();
|
||||
|
|
@ -407,7 +419,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
_params_handles.pitch_tc = param_find("MC_PITCH_TC");
|
||||
_params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN");
|
||||
_params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL");
|
||||
|
||||
_params_handles.bat_scale_en = param_find("MC_BAT_SCALE_EN");
|
||||
|
||||
|
||||
|
||||
|
|
@ -538,6 +550,8 @@ MulticopterAttitudeControl::parameters_update()
|
|||
|
||||
param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale);
|
||||
|
||||
param_get(_params_handles.bat_scale_en, &_params.bat_scale_en);
|
||||
|
||||
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
|
||||
|
||||
return OK;
|
||||
|
|
@ -656,6 +670,18 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll()
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::battery_status_poll()
|
||||
{
|
||||
/* check if there is a new message */
|
||||
bool updated;
|
||||
orb_check(_battery_status_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Attitude controller.
|
||||
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
|
||||
|
|
@ -834,6 +860,7 @@ MulticopterAttitudeControl::task_main()
|
|||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
|
||||
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
|
||||
/* initialize parameters cache */
|
||||
parameters_update();
|
||||
|
|
@ -888,6 +915,7 @@ MulticopterAttitudeControl::task_main()
|
|||
vehicle_manual_poll();
|
||||
vehicle_status_poll();
|
||||
vehicle_motor_limits_poll();
|
||||
battery_status_poll();
|
||||
|
||||
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
|
||||
* or roll (yaw can rotate 360 in normal att control). If both are true don't
|
||||
|
|
@ -980,6 +1008,13 @@ MulticopterAttitudeControl::task_main()
|
|||
_actuators.timestamp = hrt_absolute_time();
|
||||
_actuators.timestamp_sample = _ctrl_state.timestamp;
|
||||
|
||||
/* scale effort by battery status */
|
||||
if (_params.bat_scale_en && _battery_status.scale > 0.0f) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
_actuators.control[i] *= _battery_status.scale;
|
||||
}
|
||||
}
|
||||
|
||||
_controller_status.roll_rate_integ = _rates_int(0);
|
||||
_controller_status.pitch_rate_integ = _rates_int(1);
|
||||
_controller_status.yaw_rate_integ = _rates_int(2);
|
||||
|
|
|
|||
|
|
@ -403,3 +403,19 @@ PARAM_DEFINE_FLOAT(MC_TPA_BREAK, 1.0f);
|
|||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_TPA_SLOPE, 1.0f);
|
||||
|
||||
|
||||
/**
|
||||
* Whether to scale outputs by battery power level
|
||||
*
|
||||
* This compensates for voltage drop of the battery over time by attempting to
|
||||
* normalize performance across the operating range of the battery. The copter
|
||||
* should constantly behave as if it was fully charged with reduced max acceleration
|
||||
* at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery,
|
||||
* it will still be 0.5 at 60% battery.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
|
||||
|
|
|
|||
|
|
@ -1956,6 +1956,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
log_msg.body.log_BATT.current_filtered = buf.battery.current_filtered_a;
|
||||
log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
|
||||
log_msg.body.log_BATT.remaining = buf.battery.remaining;
|
||||
log_msg.body.log_BATT.scale = buf.battery.scale;
|
||||
log_msg.body.log_BATT.warning = buf.battery.warning;
|
||||
LOGBUFFER_WRITE_AND_COUNT(BATT);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -293,6 +293,7 @@ struct log_BATT_s {
|
|||
float current_filtered;
|
||||
float discharged;
|
||||
float remaining;
|
||||
float scale;
|
||||
uint8_t warning;
|
||||
};
|
||||
|
||||
|
|
@ -678,7 +679,7 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
|
||||
LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
|
||||
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
|
||||
LOG_FORMAT(BATT, "ffffffB", "V,VFilt,C,CFilt,Discharged,Remaining,Warning"),
|
||||
LOG_FORMAT(BATT, "fffffffB", "V,VFilt,C,CFilt,Discharged,Remaining,Scale,Warning"),
|
||||
LOG_FORMAT(DIST, "BBBff", "Id,Type,Orientation,Distance,Covariance"),
|
||||
LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@ Battery::Battery() :
|
|||
_param_n_cells(this, "N_CELLS"),
|
||||
_param_capacity(this, "CAPACITY"),
|
||||
_param_v_load_drop(this, "V_LOAD_DROP"),
|
||||
_param_r_internal(this, "R_INTERNAL"),
|
||||
_param_low_thr(this, "LOW_THR"),
|
||||
_param_crit_thr(this, "CRIT_THR"),
|
||||
_voltage_filtered_v(-1.0f),
|
||||
|
|
@ -55,6 +56,7 @@ Battery::Battery() :
|
|||
_remaining_voltage(1.0f),
|
||||
_remaining_capacity(1.0f),
|
||||
_remaining(1.0f),
|
||||
_scale(1.0f),
|
||||
_warning(battery_status_s::BATTERY_WARNING_NONE),
|
||||
_last_timestamp(0)
|
||||
{
|
||||
|
|
@ -72,16 +74,22 @@ Battery::reset(battery_status_s *battery_status)
|
|||
memset(battery_status, 0, sizeof(*battery_status));
|
||||
battery_status->current_a = -1.0f;
|
||||
battery_status->remaining = 1.0f;
|
||||
battery_status->scale = 1.0f;
|
||||
battery_status->cell_count = _param_n_cells.get();
|
||||
// TODO: check if it is sane to reset warning to NONE
|
||||
battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
|
||||
battery_status->connected = false;
|
||||
}
|
||||
|
||||
// TODO: Distinguish between terminal battery voltage and real battery voltage
|
||||
|
||||
void
|
||||
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized,
|
||||
bool armed, battery_status_s *battery_status)
|
||||
{
|
||||
|
||||
// what we should do is
|
||||
// TODO: Add in voltage_term_v
|
||||
reset(battery_status);
|
||||
battery_status->timestamp = timestamp;
|
||||
filterVoltage(voltage_v);
|
||||
|
|
@ -89,10 +97,12 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
|
|||
sumDischarged(timestamp, current_a);
|
||||
estimateRemaining(voltage_v, throttle_normalized, armed);
|
||||
determineWarning();
|
||||
computeScale();
|
||||
|
||||
if (_voltage_filtered_v > 2.1f) {
|
||||
battery_status->voltage_v = voltage_v;
|
||||
battery_status->voltage_filtered_v = _voltage_filtered_v;
|
||||
battery_status->scale = _scale;
|
||||
battery_status->current_a = current_a;
|
||||
battery_status->current_filtered_a = _current_filtered_a;
|
||||
battery_status->discharged_mah = _discharged_mah;
|
||||
|
|
@ -212,3 +222,16 @@ Battery::determineWarning()
|
|||
_warning = battery_status_s::BATTERY_WARNING_LOW;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Battery::computeScale()
|
||||
{
|
||||
_scale = (_param_v_full.get() * _param_n_cells.get()) / _voltage_filtered_v;
|
||||
|
||||
if (_scale > 1.3f) { // Allow at most 30% compensation
|
||||
_scale = 1.3f;
|
||||
|
||||
} else if (!PX4_ISFINITE(_scale) || _scale < 1.0f) { // Shouldn't ever be more than the power at full battery
|
||||
_scale = 1.0f;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -96,12 +96,14 @@ private:
|
|||
void sumDischarged(hrt_abstime timestamp, float current_a);
|
||||
void estimateRemaining(float voltage_v, float throttle_normalized, bool armed);
|
||||
void determineWarning();
|
||||
void computeScale();
|
||||
|
||||
control::BlockParamFloat _param_v_empty;
|
||||
control::BlockParamFloat _param_v_full;
|
||||
control::BlockParamInt _param_n_cells;
|
||||
control::BlockParamFloat _param_capacity;
|
||||
control::BlockParamFloat _param_v_load_drop;
|
||||
control::BlockParamFloat _param_r_internal;
|
||||
control::BlockParamFloat _param_low_thr;
|
||||
control::BlockParamFloat _param_crit_thr;
|
||||
|
||||
|
|
@ -111,7 +113,7 @@ private:
|
|||
float _remaining_voltage; ///< normalized battery charge level remaining based on voltage
|
||||
float _remaining_capacity; ///< normalized battery charge level remaining based on capacity
|
||||
float _remaining; ///< normalized battery charge level, selected based on config param
|
||||
float _scale;
|
||||
uint8_t _warning;
|
||||
hrt_abstime _last_timestamp;
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue