Rover: Guided mode supports direct steering and throttle control

This commit is contained in:
Randy Mackay 2020-06-15 16:29:00 +09:00
parent 73508564f7
commit 35f0402b47
4 changed files with 68 additions and 1 deletions

View File

@ -167,6 +167,19 @@ bool Rover::set_target_velocity_NED(const Vector3f& vel_ned)
return true; return true;
} }
// set steering and throttle (-1 to +1) (for use by scripting)
bool Rover::set_steering_and_throttle(float steering, float throttle)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!control_mode->in_guided_mode()) {
return false;
}
// set steering and throttle
mode_guided.set_steering_and_throttle(steering, throttle);
return true;
}
#if STATS_ENABLED == ENABLED #if STATS_ENABLED == ENABLED
/* /*
update AP_Stats update AP_Stats

View File

@ -278,6 +278,7 @@ private:
// Rover.cpp // Rover.cpp
bool set_target_location(const Location& target_loc) override; bool set_target_location(const Location& target_loc) override;
bool set_target_velocity_NED(const Vector3f& vel_ned) override; bool set_target_velocity_NED(const Vector3f& vel_ned) override;
bool set_steering_and_throttle(float steering, float throttle) override;
void stats_update(); void stats_update();
void ahrs_update(); void ahrs_update();
void gcs_failsafe_check(void); void gcs_failsafe_check(void);

View File

@ -387,6 +387,9 @@ public:
void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed); void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed);
void set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed); void set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed);
// set steering and throttle (-1 to +1). Only called from scripts
void set_steering_and_throttle(float steering, float throttle);
// vehicle start loiter // vehicle start loiter
bool start_loiter(); bool start_loiter();
@ -402,7 +405,8 @@ protected:
Guided_WP, Guided_WP,
Guided_HeadingAndSpeed, Guided_HeadingAndSpeed,
Guided_TurnRateAndSpeed, Guided_TurnRateAndSpeed,
Guided_Loiter Guided_Loiter,
Guided_SteeringAndThrottle
}; };
bool _enter() override; bool _enter() override;
@ -416,6 +420,12 @@ protected:
bool sent_notification; // used to send one time notification to ground station bool sent_notification; // used to send one time notification to ground station
float _desired_speed; // desired speed used only in HeadingAndSpeed submode float _desired_speed; // desired speed used only in HeadingAndSpeed submode
// direct steering and throttle control
bool _have_strthr; // true if we have a valid direct steering and throttle inputs
uint32_t _strthr_time_ms; // system time last call to set_steering_and_throttle was made (used for timeout)
float _strthr_steering; // direct steering input in the range -1 to +1
float _strthr_throttle; // direct throttle input in the range -1 to +1
// limits // limits
struct { struct {
uint32_t timeout_ms;// timeout from the time that guided is invoked uint32_t timeout_ms;// timeout from the time that guided is invoked

View File

@ -105,6 +105,30 @@ void ModeGuided::update()
break; break;
} }
case Guided_SteeringAndThrottle:
{
// handle timeout
if (_have_strthr && (AP_HAL::millis() - _strthr_time_ms) > 3000) {
_have_strthr = false;
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
}
if (_have_strthr) {
// pass latest steering and throttle directly to motors library
g2.motors.set_steering(_strthr_steering * 4500.0f, false);
g2.motors.set_throttle(_strthr_throttle * 100.0f);
} else {
// loiter or stop vehicle
if (rover.is_boat()) {
if (!start_loiter()) {
stop_vehicle();
}
} else {
stop_vehicle();
}
}
break;
}
default: default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break; break;
@ -122,6 +146,8 @@ float ModeGuided::get_distance_to_destination() const
return 0.0f; return 0.0f;
case Guided_Loiter: case Guided_Loiter:
return rover.mode_loiter.get_distance_to_destination(); return rover.mode_loiter.get_distance_to_destination();
case Guided_SteeringAndThrottle:
return 0.0f;
} }
// we should never reach here but just in case, return 0 // we should never reach here but just in case, return 0
@ -137,6 +163,7 @@ bool ModeGuided::reached_destination() const
case Guided_HeadingAndSpeed: case Guided_HeadingAndSpeed:
case Guided_TurnRateAndSpeed: case Guided_TurnRateAndSpeed:
case Guided_Loiter: case Guided_Loiter:
case Guided_SteeringAndThrottle:
return true; return true;
} }
@ -160,6 +187,9 @@ bool ModeGuided::set_desired_speed(float speed)
return false; return false;
case Guided_Loiter: case Guided_Loiter:
return rover.mode_loiter.set_desired_speed(speed); return rover.mode_loiter.set_desired_speed(speed);
case Guided_SteeringAndThrottle:
// no speed control
return false;
} }
return false; return false;
} }
@ -181,6 +211,9 @@ bool ModeGuided::get_desired_location(Location& destination) const
case Guided_Loiter: case Guided_Loiter:
// get destination from loiter // get destination from loiter
return rover.mode_loiter.get_desired_location(destination); return rover.mode_loiter.get_desired_location(destination);
case Guided_SteeringAndThrottle:
// no desired location in this submode
break;
} }
// should never get here but just in case // should never get here but just in case
@ -246,6 +279,16 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
} }
// set steering and throttle (both in the range -1 to +1)
void ModeGuided::set_steering_and_throttle(float steering, float throttle)
{
_guided_mode = ModeGuided::Guided_SteeringAndThrottle;
_strthr_time_ms = AP_HAL::millis();
_strthr_steering = constrain_float(steering, -1.0f, 1.0f);
_strthr_throttle = constrain_float(throttle, -1.0f, 1.0f);
_have_strthr = true;
}
bool ModeGuided::start_loiter() bool ModeGuided::start_loiter()
{ {
if (rover.mode_loiter.enter()) { if (rover.mode_loiter.enter()) {