aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: Implement yaw rate control for DDS

Browse Source 
* And add const to external control
* This method bypasses the loiter controller
This commit is contained in:
Ryan Friedman 2024-10-24 17:42:19 +09:00
parent 79f743983d
commit eb73a10cb8
4 changed files with 35 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/AP_ExternalControl_Plane.cpp
View File

@ -19,4 +19,14 @@ bool AP_ExternalControl_Plane::set_global_position(const Location& loc)
return plane.set_target_location(loc);
}
/*
Sets only the target yaw rate
Yaw is in earth frame, NED [rad/s]
*/
bool AP_ExternalControl_Plane::set_yaw_rate(const float yaw_rate_rads)
{
// TODO validate yaw rate is feasible for current dynamics
return plane.set_target_yaw_rate(yaw_rate_rads);
}
#endif // AP_EXTERNAL_CONTROL_ENABLED

6
ArduPlane/AP_ExternalControl_Plane.h
View File

@ -16,6 +16,12 @@ public:
Sets the target global position for a loiter point.
*/
bool set_global_position(const Location& loc) override WARN_IF_UNUSED;
/*
Sets only the target yaw rate
Yaw is in earth frame, NED [rad/s]
*/
bool set_yaw_rate(const float yaw_rate_rads) override WARN_IF_UNUSED;
};
#endif // AP_EXTERNAL_CONTROL_ENABLED

18
ArduPlane/Plane.cpp
View File

@ -884,6 +884,24 @@ bool Plane::set_target_location(const Location &target_loc)
plane.set_guided_WP(loc);
return true;
}
bool Plane::set_target_yaw_rate(const float yaw_rate)
{
if (plane.control_mode != &plane.mode_guided) {
// only accept yaw rate updates when in GUIDED mode
return false;
}
const auto direction_is_ccw = yaw_rate < 0;
if (!is_zero(yaw_rate)) {
auto const speed = plane.ahrs.groundspeed();
auto const radius = speed / yaw_rate;
plane.mode_guided.set_radius_and_direction(abs(radius), direction_is_ccw);
} else {
plane.mode_guided.set_radius_and_direction(0.0, true);
}
return true;
}
#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
#if AP_SCRIPTING_ENABLED

1
ArduPlane/Plane.h
View File

@ -1306,6 +1306,7 @@ public:
bool is_taking_off() const override;
#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
bool set_target_location(const Location& target_loc) override;
bool set_target_yaw_rate(const float yaw_rate_rads) override;
#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
#if AP_SCRIPTING_ENABLED
bool get_target_location(Location& target_loc) override;