PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <RunwayTakeoff.h>
Public Member Functions | |
RunwayTakeoff (ModuleParams *parent) | |
~RunwayTakeoff ()=default | |
void | init (float yaw, double current_lat, double current_lon) |
void | update (float airspeed, float alt_agl, double current_lat, double current_lon, orb_advert_t *mavlink_log_pub) |
RunwayTakeoffState | getState () |
bool | isInitialized () |
bool | runwayTakeoffEnabled () |
float | getMinAirspeedScaling () |
float | getInitYaw () |
bool | controlYaw () |
bool | climbout () |
float | getPitch (float tecsPitch) |
float | getRoll (float navigatorRoll) |
float | getYaw (float navigatorYaw) |
float | getThrottle (float tecsThrottle) |
bool | resetIntegrators () |
float | getMinPitch (float sp_min, float climbout_min, float min) |
float | getMaxPitch (float max) |
matrix::Vector2f | getStartWP () |
void | reset () |
Private Attributes | |
RunwayTakeoffState | _state |
state variables More... | |
bool | _initialized |
hrt_abstime | _initialized_time |
float | _init_yaw |
bool | _climbout |
matrix::Vector2f | _start_wp |
Definition at line 65 of file RunwayTakeoff.h.
runwaytakeoff::RunwayTakeoff::RunwayTakeoff | ( | ModuleParams * | parent | ) |
Definition at line 54 of file RunwayTakeoff.cpp.
|
default |
|
inline |
Definition at line 82 of file RunwayTakeoff.h.
References math::max(), math::min(), and lps22hb::reset().
Referenced by FixedwingPositionControl::control_takeoff().
bool runwaytakeoff::RunwayTakeoff::controlYaw | ( | ) |
Definition at line 130 of file RunwayTakeoff.cpp.
References _state, and runwaytakeoff::CLIMBOUT.
Referenced by FixedwingPositionControl::control_takeoff().
|
inline |
Definition at line 79 of file RunwayTakeoff.h.
float runwaytakeoff::RunwayTakeoff::getMaxPitch | ( | float | max | ) |
Definition at line 241 of file RunwayTakeoff.cpp.
References _state, runwaytakeoff::FLY, and math::max().
Referenced by FixedwingPositionControl::control_takeoff().
|
inline |
Definition at line 78 of file RunwayTakeoff.h.
Referenced by FixedwingPositionControl::control_takeoff().
float runwaytakeoff::RunwayTakeoff::getMinPitch | ( | float | sp_min, |
float | climbout_min, | ||
float | min | ||
) |
Definition at line 225 of file RunwayTakeoff.cpp.
References _state, runwaytakeoff::FLY, math::max(), and math::min().
Referenced by FixedwingPositionControl::control_takeoff().
float runwaytakeoff::RunwayTakeoff::getPitch | ( | float | tecsPitch | ) |
Definition at line 142 of file RunwayTakeoff.cpp.
References _state, runwaytakeoff::CLAMPED_TO_RUNWAY, and math::radians().
Referenced by FixedwingPositionControl::control_takeoff().
float runwaytakeoff::RunwayTakeoff::getRoll | ( | float | navigatorRoll | ) |
Definition at line 154 of file RunwayTakeoff.cpp.
References _state, runwaytakeoff::CLIMBOUT, math::constrain(), runwaytakeoff::FLY, and math::radians().
Referenced by FixedwingPositionControl::control_takeoff().
Vector2f runwaytakeoff::RunwayTakeoff::getStartWP | ( | ) |
Definition at line 256 of file RunwayTakeoff.cpp.
References _start_wp.
Referenced by FixedwingPositionControl::control_takeoff().
|
inline |
Definition at line 74 of file RunwayTakeoff.h.
float runwaytakeoff::RunwayTakeoff::getThrottle | ( | float | tecsThrottle | ) |
Definition at line 193 of file RunwayTakeoff.cpp.
References _initialized_time, _state, runwaytakeoff::CLAMPED_TO_RUNWAY, hrt_elapsed_time(), and runwaytakeoff::THROTTLE_RAMP.
Referenced by FixedwingPositionControl::control_position().
float runwaytakeoff::RunwayTakeoff::getYaw | ( | float | navigatorYaw | ) |
Definition at line 177 of file RunwayTakeoff.cpp.
References _init_yaw, _state, and runwaytakeoff::CLIMBOUT.
Referenced by FixedwingPositionControl::control_takeoff().
void runwaytakeoff::RunwayTakeoff::init | ( | float | yaw, |
double | current_lat, | ||
double | current_lon | ||
) |
Definition at line 64 of file RunwayTakeoff.cpp.
References _climbout, _init_yaw, _initialized, _initialized_time, _start_wp, _state, hrt_absolute_time(), and runwaytakeoff::THROTTLE_RAMP.
Referenced by FixedwingPositionControl::control_takeoff().
|
inline |
Definition at line 75 of file RunwayTakeoff.h.
Referenced by FixedwingPositionControl::control_takeoff().
void runwaytakeoff::RunwayTakeoff::reset | ( | void | ) |
Definition at line 261 of file RunwayTakeoff.cpp.
References _initialized, _state, and runwaytakeoff::THROTTLE_RAMP.
Referenced by FixedwingPositionControl::reset_takeoff_state().
bool runwaytakeoff::RunwayTakeoff::resetIntegrators | ( | ) |
Definition at line 212 of file RunwayTakeoff.cpp.
References _state, and runwaytakeoff::TAKEOFF.
Referenced by FixedwingPositionControl::control_takeoff().
|
inline |
Definition at line 77 of file RunwayTakeoff.h.
Referenced by FixedwingPositionControl::control_position(), and FixedwingPositionControl::control_takeoff().
void runwaytakeoff::RunwayTakeoff::update | ( | float | airspeed, |
float | alt_agl, | ||
double | current_lat, | ||
double | current_lon, | ||
orb_advert_t * | mavlink_log_pub | ||
) |
Definition at line 75 of file RunwayTakeoff.cpp.
References _climbout, _initialized_time, _start_wp, _state, runwaytakeoff::CLAMPED_TO_RUNWAY, runwaytakeoff::CLIMBOUT, runwaytakeoff::FLY, hrt_elapsed_time(), mavlink_log_info, runwaytakeoff::TAKEOFF, and runwaytakeoff::THROTTLE_RAMP.
Referenced by FixedwingPositionControl::control_takeoff().
|
private |
Definition at line 100 of file RunwayTakeoff.h.
|
private |
Definition at line 99 of file RunwayTakeoff.h.
|
private |
Definition at line 97 of file RunwayTakeoff.h.
|
private |
Definition at line 98 of file RunwayTakeoff.h.
Referenced by getThrottle(), init(), and update().
|
private |
Definition at line 101 of file RunwayTakeoff.h.
Referenced by getStartWP(), init(), and update().
|
private |
state variables
Definition at line 96 of file RunwayTakeoff.h.
Referenced by controlYaw(), getMaxPitch(), getMinPitch(), getPitch(), getRoll(), getThrottle(), getYaw(), init(), reset(), resetIntegrators(), and update().