Skip to content

Commit

Permalink
Rover: enabled sending waypoints from a companion computer to ardupil…
Browse files Browse the repository at this point in the history
…ot for copter and rover

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
  • Loading branch information
haarshitgarg authored and rmackay9 committed May 2, 2024
1 parent a13639d commit 7701be8
Show file tree
Hide file tree
Showing 4 changed files with 22 additions and 4 deletions.
6 changes: 6 additions & 0 deletions Rover/AP_ExternalControl_Rover.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,12 @@ bool AP_ExternalControl_Rover::set_linear_velocity_and_yaw_rate(const Vector3f &
return true;
}

bool AP_ExternalControl_Rover::set_global_position(const Location& loc)
{
// set_target_location only checks if the rover is in guided mode or not
return rover.set_target_location(loc);
}

bool AP_ExternalControl_Rover::ready_for_external_control()
{
return rover.control_mode->in_guided_mode() && rover.arming.is_armed();
Expand Down
9 changes: 8 additions & 1 deletion Rover/AP_ExternalControl_Rover.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,21 @@

#if AP_EXTERNAL_CONTROL_ENABLED

class AP_ExternalControl_Rover : public AP_ExternalControl {
class AP_ExternalControl_Rover : public AP_ExternalControl
{
public:
/*
Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw.
Velocity is in earth frame, NED [m/s].
Yaw is in earth frame, NED [rad/s].
*/
bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)override WARN_IF_UNUSED;

/*
Sets the global position for loiter point
*/
bool set_global_position(const Location& loc) override WARN_IF_UNUSED;

private:
/*
Return true if Rover is ready to handle external control data.
Expand Down
6 changes: 4 additions & 2 deletions Rover/Rover.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -157,8 +157,8 @@ Rover::Rover(void) :
{
}

#if AP_SCRIPTING_ENABLED
// set target location (for use by scripting)
#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
// set target location (for use by external control and scripting)
bool Rover::set_target_location(const Location& target_loc)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
Expand All @@ -168,7 +168,9 @@ bool Rover::set_target_location(const Location& target_loc)

return mode_guided.set_desired_location(target_loc);
}
#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED

#if AP_SCRIPTING_ENABLED
// set target velocity (for use by scripting)
bool Rover::set_target_velocity_NED(const Vector3f& vel_ned)
{
Expand Down
5 changes: 4 additions & 1 deletion Rover/Rover.h
Original file line number Diff line number Diff line change
Expand Up @@ -269,8 +269,11 @@ class Rover : public AP_Vehicle {
cruise_learn_t cruise_learn;

// Rover.cpp
#if AP_SCRIPTING_ENABLED
#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
bool set_target_location(const Location& target_loc) override;
#endif

#if AP_SCRIPTING_ENABLED
bool set_target_velocity_NED(const Vector3f& vel_ned) override;
bool set_steering_and_throttle(float steering, float throttle) override;
bool get_steering_and_throttle(float& steering, float& throttle) override;
Expand Down

0 comments on commit 7701be8

Please sign in to comment.