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

FlightTaskAutoLine: move altitude above ground computaiton to method

Browse Source
This commit is contained in:
Dennis Mannhart 2018-04-27 09:28:32 +02:00 committed by Lorenz Meier
parent 3ea0a53192
commit c637ccb65f
2 changed files with 18 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
View File

@ -55,21 +55,11 @@ bool FlightTaskAutoLine::update()
// always reset constraints because they might change depending on the type
_setDefaultConstraints();
_updateAltitudeAboveGround();
bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position;
bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position;
// Altitude above ground is by default just the negation of the current local position in D-direction.
_alt_above_ground = -_position(2);
if (PX4_ISFINITE(_dist_to_bottom)) {
// We have a valid distance to ground measurement
_alt_above_ground = _dist_to_bottom;
} else if (_sub_home_position->get().valid_alt) {
// if home position is set, then altitude above ground is relative to the home position
_alt_above_ground = -_position(2) + _sub_home_position->get().z;
}
// 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state.
if (follow_line && !follow_line_prev) {
_reset();
@ -521,6 +511,21 @@ float FlightTaskAutoLine::_getVelocityFromAngle(const float angle)
return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed);
}
void FlightTaskAutoLine::_updateAltitudeAboveGround()
{
// Altitude above ground is by default just the negation of the current local position in D-direction.
_alt_above_ground = -_position(2);
if (PX4_ISFINITE(_dist_to_bottom)) {
// We have a valid distance to ground measurement
_alt_above_ground = _dist_to_bottom;
} else if (_sub_home_position->get().valid_alt) {
// if home position is set, then altitude above ground is relative to the home position
_alt_above_ground = -_position(2) + _sub_home_position->get().z;
}
}
void FlightTaskAutoLine::updateParams()
{
FlightTaskAuto::updateParams();

1
src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
View File

@ -86,6 +86,7 @@ protected:
void _updateInternalWaypoints(); /* Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
void _generateSetpoints(); /**< Generate velocity and position setpoint for following line. */
void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */
void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */
void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */
void updateParams() override; /**< See ModuleParam class */