Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-25 10:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add TERRAIN_MARGIN parameter

Browse Source
This commit is contained in:
Randy Mackay 2021-07-09 14:18:28 +09:00
parent e78fcb834e
commit 17b4f5914c
5 changed files with 25 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -884,6 +884,7 @@ private:
// terrain.cpp // terrain.cpp
void terrain_update(); void terrain_update();
void terrain_logging(); void terrain_logging();
float get_terrain_margin() const;
// tuning.cpp // tuning.cpp
void tuning(); void tuning();

10
ArduCopter/Parameters.cpp
View File

@ -1079,6 +1079,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0), AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0),
#endif #endif
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
// @Param: TERRAIN_MARGIN
// @DisplayName: Terrain following altitude margin
// @Description: Terrain following altitude margin. Vehicle will stop if distance from target altitude is larger than this margin (in meters)
// @Units: m
// @Range: 0.1 100
// @User: Advanced
AP_GROUPINFO("TERRAIN_MARGIN", 47, ParametersG2, terrain_margin, 10.0),
#endif
AP_GROUPEND AP_GROUPEND
}; };

3
ArduCopter/Parameters.h
View File

@ -653,6 +653,9 @@ public:
AP_Float guided_timeout; AP_Float guided_timeout;
#endif #endif
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
AP_Float terrain_margin;
#endif
}; };
extern const AP_Param::Info var_info[]; extern const AP_Param::Info var_info[];

4
ArduCopter/mode_guided.cpp
View File

@ -551,11 +551,9 @@ void ModeGuided::pos_control_run()
guided_accel_target_cmss.zero(); guided_accel_target_cmss.zero();
guided_vel_target_cms.zero(); guided_vel_target_cms.zero();
// todo: Randy to convert to parameter TERRAIN_MARGIN (in m)
float TERRAIN_MARGIN = 10;
float pos_offset_z_buffer = 0.0; // Vertical buffer size in m float pos_offset_z_buffer = 0.0; // Vertical buffer size in m
if (guided_pos_terrain_alt) { if (guided_pos_terrain_alt) {
pos_offset_z_buffer = MIN(TERRAIN_MARGIN * 100.0, 0.5 * fabsf(guided_pos_target_cm.z)); pos_offset_z_buffer = MIN(copter.get_terrain_margin() * 100.0, 0.5 * fabsf(guided_pos_target_cm.z));
} }
pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset, pos_offset_z_buffer); pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset, pos_offset_z_buffer);

10
ArduCopter/terrain.cpp
View File

@ -26,3 +26,13 @@ void Copter::terrain_logging()
} }
#endif #endif
} }
// return terrain following altitude margin. Vehicle will stop if distance from target altitude is larger than this margin (in meters)
float Copter::get_terrain_margin() const
{
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
return MAX(g2.terrain_margin.get(), 0.1);
#else
return 10;
#endif
}