diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 2aba32c67f..6a8507a1c8 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -135,6 +136,7 @@ private: int _global_pos_sub; /**< global position subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _vehicle_land_detected_sub; /**< vehicle land detected subscription */ + int _battery_status_sub; /**< battery status subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ @@ -156,6 +158,7 @@ private: struct vehicle_global_position_s _global_pos; /**< global position */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_land_detected_s _vehicle_land_detected; /**< vehicle land detected */ + struct battery_status_s _battery_status; /**< battery status */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ @@ -218,6 +221,8 @@ private: int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ + int bat_scale_en; /**< Battery scaling enabled */ + } _parameters; /**< local copies of interesting parameters */ struct { @@ -269,6 +274,8 @@ private: param_t vtol_type; + param_t bat_scale_en; + } _parameter_handles; /**< handles for interesting parameters */ // Rotation matrix and euler angles to extract from control state @@ -329,6 +336,11 @@ private: */ void vehicle_land_detected_poll(); + /** + * Check for battery status updates. + */ + void battery_status_poll(); + /** * Shim for calling task_main from task_create. */ @@ -453,6 +465,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.vtol_type = param_find("VT_TYPE"); + _parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN"); + /* fetch initial parameter values */ parameters_update(); } @@ -544,6 +558,8 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); + param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.p_tc); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -683,6 +699,18 @@ FixedwingAttitudeControl::vehicle_land_detected_poll() } } +void +FixedwingAttitudeControl::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); + } +} + void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -704,6 +732,7 @@ FixedwingAttitudeControl::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); parameters_update(); @@ -714,6 +743,7 @@ FixedwingAttitudeControl::task_main() vehicle_manual_poll(); vehicle_status_poll(); vehicle_land_detected_poll(); + battery_status_poll(); /* wakeup source */ px4_pollfd_struct_t fds[2]; @@ -1114,6 +1144,12 @@ FixedwingAttitudeControl::task_main() _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; + /* scale effort by battery status */ + if (_parameters.bat_scale_en && _battery_status.scale > 0.0f) { + _actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale; + } + + if (!PX4_ISFINITE(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index ebc6547c96..40a015ba49 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -561,3 +561,17 @@ PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f); + +/** + * Whether to scale throttle 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 fixed wing + * should constantly behave as if it was fully charged with reduced max thrust + * at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0);