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

Plane: add guided radius to mavlink DO_REPOSITION

Browse Source
This commit is contained in:
Tom Pittenger 2022-10-10 14:58:30 -07:00 committed by Tom Pittenger
parent 938df192fa
commit 46bc30a40c
3 changed files with 21 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/GCS_Mavlink.cpp
View File

@ -725,6 +725,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
plane.set_guided_WP(requested_position);
// Loiter radius for planes. Positive radius in meters, direction is controlled by Yaw (param4) value, parsed above
if (!isnan(packet.param3) && packet.param3 > 0) {
plane.mode_guided.set_radius_and_direction(packet.param3, requested_position.loiter_ccw);
}
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;

5
ArduPlane/mode.h
View File

@ -224,9 +224,14 @@ public:
// handle a guided target request from GCS
bool handle_guided_request(Location target_loc) override;
void set_radius_and_direction(const float radius, const bool direction_is_ccw);
protected:
bool _enter() override;
private:
float active_radius_m;
};
class ModeCircle: public Mode

13
ArduPlane/mode_guided.cpp
View File

@ -20,6 +20,9 @@ bool ModeGuided::_enter()
}
#endif
// set guided radius to WP_LOITER_RAD on mode change.
active_radius_m = 0;
plane.set_guided_WP(loc);
return true;
}
@ -39,8 +42,7 @@ void ModeGuided::update()
void ModeGuided::navigate()
{
// Zero indicates to use WP_LOITER_RAD
plane.update_loiter(0);
plane.update_loiter(active_radius_m);
}
bool ModeGuided::handle_guided_request(Location target_loc)
@ -55,3 +57,10 @@ bool ModeGuided::handle_guided_request(Location target_loc)
return true;
}
void ModeGuided::set_radius_and_direction(const float radius, const bool direction_is_ccw)
{
// constrain to (uint16_t) range for update_loiter()
active_radius_m = constrain_int32(fabsf(radius), 0, UINT16_MAX);
plane.loiter.direction = direction_is_ccw ? -1 : 1;
}