fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 19:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

create example vehicle type build configs for fmu-v2 and fmu-v5 (#10963)

Browse Source 
- update navigator precision landing to build without multicopter
This commit is contained in:
Daniel Agar
2019-02-05 19:53:54 -05:00
committed by GitHub
parent fd4caa849d
commit 1a4d31140e
10 changed files with 546 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/precland.cpp
+23 -6
View File
@@ -63,6 +63,10 @@ PrecLand::PrecLand(Navigator *navigator) :
MissionBlock(navigator),
ModuleParams(navigator)
{
_handle_param_acceleration_hor = param_find("MPC_ACC_HOR");
_handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE");
updateParams();
}
void
@@ -101,7 +105,6 @@ PrecLand::on_activation()
_last_slewrate_time = 0;
switch_to_state_start();
}
void
@@ -160,6 +163,20 @@ PrecLand::on_active()
}
void
PrecLand::updateParams()
{
ModuleParams::updateParams();
if (_handle_param_acceleration_hor != PARAM_INVALID) {
param_get(_handle_param_acceleration_hor, &_param_acceleration_hor);
}
if (_handle_param_xy_vel_cruise != PARAM_INVALID) {
param_get(_handle_param_xy_vel_cruise, &_param_xy_vel_cruise);
}
}
void
PrecLand::run_state_start()
{
@@ -545,21 +562,21 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
// limit the setpoint speed to the maximum cruise speed
matrix::Vector2f sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
if (sp_vel.length() > _param_xy_vel_cruise.get()) {
sp_vel = sp_vel.normalized() * _param_xy_vel_cruise.get();
if (sp_vel.length() > _param_xy_vel_cruise) {
sp_vel = sp_vel.normalized() * _param_xy_vel_cruise;
sp_curr = _sp_pev + sp_vel * dt;
}
// limit the setpoint acceleration to the maximum acceleration
matrix::Vector2f sp_acc = (sp_curr - _sp_pev * 2 + _sp_pev_prev) / (dt * dt); // acceleration of the setpoints
if (sp_acc.length() > _param_acceleration_hor.get()) {
sp_acc = sp_acc.normalized() * _param_acceleration_hor.get();
if (sp_acc.length() > _param_acceleration_hor) {
sp_acc = sp_acc.normalized() * _param_acceleration_hor;
sp_curr = _sp_pev * 2 - _sp_pev_prev + sp_acc * (dt * dt);
}
// limit the setpoint speed such that we can stop at the setpoint given the maximum acceleration/deceleration
float max_spd = sqrtf(_param_acceleration_hor.get() * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x,
float max_spd = sqrtf(_param_acceleration_hor * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x,
sp_y))).length());
sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
src/modules/navigator/precland.h
+10 -3
View File
@@ -77,6 +77,9 @@ public:
PrecLandMode get_mode() { return _mode; };
private:
void updateParams() override;
// run the control loop for each state
void run_state_start();
void run_state_horizontal_approach();
@@ -127,9 +130,13 @@ private:
(ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_final_approach_alt,
(ParamFloat<px4::params::PLD_SRCH_ALT>) _param_search_alt,
(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_search_timeout,
(ParamInt<px4::params::PLD_MAX_SRCH>) _param_max_searches,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_acceleration_hor,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_xy_vel_cruise
(ParamInt<px4::params::PLD_MAX_SRCH>) _param_max_searches
)
// non-navigator parameters
param_t _handle_param_acceleration_hor{PARAM_INVALID};
param_t _handle_param_xy_vel_cruise{PARAM_INVALID};
float _param_acceleration_hor{0.0f};
float _param_xy_vel_cruise{0.0f};
};