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

Rover: Add loiter behaviour parameter

Browse Source
This commit is contained in:
Raouf 2018-08-07 13:22:51 +09:00 committed by Randy Mackay
parent 8308646c5e
commit f0185cb79b
3 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
APMrover2/Parameters.cpp
View File

@ -564,6 +564,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO("FRAME_TYPE", 24, ParametersG2, frame_type, 0), AP_GROUPINFO("FRAME_TYPE", 24, ParametersG2, frame_type, 0),
// @Param: LOIT_TYPE
// @DisplayName: Loiter type
// @Description: Loiter behaviour when around next to a taget point
// @Values: 0:Reverse to target point,1:Always face bow to target point
// @User: Standard
AP_GROUPINFO("LOIT_TYPE", 25, ParametersG2, loit_type, 0),
AP_GROUPEND AP_GROUPEND
}; };

3
APMrover2/Parameters.h
View File

@ -354,6 +354,9 @@ public:
// frame type for vehicle (used for vectored motor vehicles and custom motor configs) // frame type for vehicle (used for vectored motor vehicles and custom motor configs)
AP_Int8 frame_type; AP_Int8 frame_type;
// loiter type
AP_Int8 loit_type;
}; };
extern const AP_Param::Info var_info[]; extern const AP_Param::Info var_info[];

2
APMrover2/mode_loiter.cpp
View File

@ -36,7 +36,7 @@ void ModeLoiter::update()
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination); _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor); _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it // if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000) { if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000); _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor); _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed; _desired_speed = -_desired_speed;