diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 351b31d035..a312b5feb8 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -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 diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b152f36953..2b0a9988f1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -80,6 +80,7 @@ #include #include #include +#include #include #include #include @@ -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); diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 5105abc275..53f0e9562b 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -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); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 530df1be1a..38c87529ae 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5db02e1d92..06c3174e56 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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"), diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index 0b7d17ad18..7a0784074a 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -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; + } +} diff --git a/src/modules/systemlib/battery.h b/src/modules/systemlib/battery.h index d352030d5d..9ebc6a6ee2 100644 --- a/src/modules/systemlib/battery.h +++ b/src/modules/systemlib/battery.h @@ -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; }; -