fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw_att_control only store battery_status scale

Browse Source
This commit is contained in:
Daniel Agar 2018-02-16 20:23:47 -05:00
parent 94b8e4a2bf
commit 7fa858ad84
2 changed files with 18 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -325,18 +325,6 @@ 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::run()
{
/*
@ -360,7 +348,6 @@ void FixedwingAttitudeControl::run()
vehicle_manual_poll();
vehicle_status_poll();
vehicle_land_detected_poll();
battery_status_poll();
/* wakeup source */
px4_pollfd_struct_t fds[1];
@ -478,7 +465,6 @@ void FixedwingAttitudeControl::run()
global_pos_poll();
vehicle_status_poll();
vehicle_land_detected_poll();
battery_status_poll();
// the position controller will not emit attitude setpoints in some modes
// we need to make sure that this flag is reset
@ -715,9 +701,23 @@ void FixedwingAttitudeControl::run()
&& !_vehicle_status.engine_failure) ? throttle_sp : 0.0f;
/* scale effort by battery status */
if (_parameters.bat_scale_en && _battery_status.scale > 0.0f &&
if (_parameters.bat_scale_en &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale;
bool updated = false;
orb_check(_battery_status_sub, &updated);
if (updated) {
battery_status_s battery_status = {};
if (orb_copy(ORB_ID(battery_status), _battery_status_sub, &battery_status) == PX4_OK) {
if (battery_status.scale > 0.0f) {
_battery_scale = battery_status.scale;
}
}
}
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
} else {

8
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -116,7 +116,6 @@ private:
actuator_controls_s _actuators {}; /**< actuator control inputs */
actuator_controls_s _actuators_airframe {}; /**< actuator control inputs */
battery_status_s _battery_status {}; /**< battery status */
manual_control_setpoint_s _manual {}; /**< r/c channel data */
vehicle_attitude_s _att {}; /**< vehicle attitude setpoint */
vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */
@ -136,6 +135,8 @@ private:
bool _landed{true};
float _battery_scale{1.0f};
struct {
float p_tc;
float p_p;
@ -301,9 +302,4 @@ private:
*/
void vehicle_land_detected_poll();
/**
* Check for battery status updates.
*/
void battery_status_poll();
};