mirror of https://github.com/ArduPilot/ardupilot
Rover: Guided mode supports direct steering and throttle control
This commit is contained in:
parent
73508564f7
commit
35f0402b47
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
12
Rover/mode.h
12
Rover/mode.h
|
@ -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
|
||||||
|
|
|
@ -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()) {
|
||||||
|
|
Loading…
Reference in New Issue