Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: remove Dodge avoidance 

this has been replaced with BendyRuler OA Path Planning
This commit is contained in:
Randy Mackay 2019-08-03 15:02:17 +09:00
parent 7fbe7de984
commit 13aaf4375b
6 changed files with 4 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
APMrover2/Log.cpp
View File

@ -224,43 +224,6 @@ void Rover::Log_Write_Throttle()
logger.WriteBlock(&pkt, sizeof(pkt)); logger.WriteBlock(&pkt, sizeof(pkt));
} }
struct PACKED log_Rangefinder {
LOG_PACKET_HEADER;
uint64_t time_us;
float lateral_accel;
uint16_t rangefinder1_distance;
uint16_t rangefinder2_distance;
uint16_t detected_count;
int8_t turn_angle;
uint16_t turn_time;
uint16_t ground_speed;
int8_t throttle;
};
// Write a rangefinder packet
void Rover::Log_Write_Rangefinder()
{
uint16_t turn_time = 0;
if (!is_zero(obstacle.turn_angle)) {
turn_time = AP_HAL::millis() - obstacle.detected_time_ms;
}
AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0);
AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1);
struct log_Rangefinder pkt = {
LOG_PACKET_HEADER_INIT(LOG_RANGEFINDER_MSG),
time_us : AP_HAL::micros64(),
lateral_accel : g2.attitude_control.get_desired_lat_accel(),
rangefinder1_distance : s0 ? s0->distance_cm() : (uint16_t)0,
rangefinder2_distance : s1 ? s1->distance_cm() : (uint16_t)0,
detected_count : obstacle.detected_count,
turn_angle : static_cast<int8_t>(obstacle.turn_angle),
turn_time : turn_time,
ground_speed : static_cast<uint16_t>(fabsf(ground_speed * 100.0f)),
throttle : int8_t(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))
};
logger.WriteBlock(&pkt, sizeof(pkt));
}
void Rover::Log_Write_RC(void) void Rover::Log_Write_RC(void)
{ {
logger.Write_RCIN(); logger.Write_RCIN();
@ -290,8 +253,6 @@ const LogStructure Rover::log_structure[] = {
"THR", "Qhffff", "TimeUS,ThrIn,ThrOut,DesSpeed,Speed,AccY", "s--nno", "F--000" }, "THR", "Qhffff", "TimeUS,ThrIn,ThrOut,DesSpeed,Speed,AccY", "s--nno", "F--000" },
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning), { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
"NTUN", "QfHHHf", "TimeUS,WpDist,WpBrg,DesYaw,Yaw,XTrack", "smdddm", "F0BBB0" }, "NTUN", "QfHHHf", "TimeUS,WpDist,WpBrg,DesYaw,Yaw,XTrack", "smdddm", "F0BBB0" },
{ LOG_RANGEFINDER_MSG, sizeof(log_Rangefinder),
"RGFD", "QfHHHbHCb", "TimeUS,LatAcc,R1Dist,R2Dist,DCnt,TAng,TTim,Spd,Thr", "somm-hsm-", "F0BB-0CB-" },
{ LOG_STEERING_MSG, sizeof(log_Steering), { LOG_STEERING_MSG, sizeof(log_Steering),
"STER", "Qhfffff", "TimeUS,SteerIn,SteerOut,DesLatAcc,LatAcc,DesTurnRate,TurnRate", "s--ookk", "F--0000" }, "STER", "Qhfffff", "TimeUS,SteerIn,SteerOut,DesLatAcc,LatAcc,DesTurnRate,TurnRate", "s--ookk", "F--0000" },
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
@ -313,7 +274,6 @@ void Rover::Log_Write_Nav_Tuning() {}
void Rover::Log_Write_Sail() {} void Rover::Log_Write_Sail() {}
void Rover::Log_Write_Startup(uint8_t type) {} void Rover::Log_Write_Startup(uint8_t type) {}
void Rover::Log_Write_Throttle() {} void Rover::Log_Write_Throttle() {}
void Rover::Log_Write_Rangefinder() {}
void Rover::Log_Write_RC(void) {} void Rover::Log_Write_RC(void) {}
void Rover::Log_Write_Steering() {} void Rover::Log_Write_Steering() {}
void Rover::Log_Write_Vehicle_Startup_Messages() {} void Rover::Log_Write_Vehicle_Startup_Messages() {}

35
APMrover2/Parameters.cpp
View File

@ -169,41 +169,6 @@ const AP_Param::Info Rover::var_info[] = {
// @User: Advanced // @User: Advanced
GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", 0.8f), GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", 0.8f),
// @Param: RNGFND_TRIGGR_CM
// @DisplayName: Object avoidance trigger distance
// @Description: The distance from an obstacle in centimeters at which the rangefinder triggers a turn to avoid the obstacle
// @Units: cm
// @Range: 0 1000
// @Increment: 1
// @User: Standard
GSCALAR(rangefinder_trigger_cm, "RNGFND_TRIGGR_CM", 100),
// @Param: RNGFND_TURN_ANGL
// @DisplayName: Object avoidance turn aggressiveness and direction
// @Description: The aggressiveness and direction of turn to avoid an obstacle. Large positive or negative values (i.e. -450 or 450) cause turns up to the vehicle's maximum lateral acceleration (TURN_MAX_G) while values near zero cause gentle turns. Positive means to turn right, negative means turn left.
// @Units: deg
// @Range: -450 450
// @Increment: 1
// @User: Standard
GSCALAR(rangefinder_turn_angle, "RNGFND_TURN_ANGL", 45),
// @Param: RNGFND_TURN_TIME
// @DisplayName: Object avoidance turn time
// @Description: The amount of time in seconds to apply the RNGFND_TURN_ANGL after detecting an obstacle.
// @Units: s
// @Range: 0 100
// @Increment: 0.1
// @User: Standard
GSCALAR(rangefinder_turn_time, "RNGFND_TURN_TIME", 1.0f),
// @Param: RNGFND_DEBOUNCE
// @DisplayName: Object avoidance rangefinder debounce count
// @Description: The number of 50Hz rangefinder hits needed to trigger an obstacle avoidance event. If you get a lot of false rangefinder events then raise this number, but if you make it too large then it will cause lag in detecting obstacles, which could cause you go hit the obstacle.
// @Range: 1 100
// @Increment: 1
// @User: Standard
GSCALAR(rangefinder_debounce, "RNGFND_DEBOUNCE", 2),
// @Param: MODE_CH // @Param: MODE_CH
// @DisplayName: Mode channel // @DisplayName: Mode channel
// @Description: RC Channel to use for driving mode control // @Description: RC Channel to use for driving mode control

15
APMrover2/Parameters.h
View File

@ -149,11 +149,11 @@ public:
// obstacle control // obstacle control
k_param_sonar_enabled = 190, // deprecated, can be removed k_param_sonar_enabled = 190, // deprecated, can be removed
k_param_sonar_old, // unused k_param_sonar_old, // unused
k_param_rangefinder_trigger_cm, k_param_rangefinder_trigger_cm, // unused
k_param_rangefinder_turn_angle, k_param_rangefinder_turn_angle, // unused
k_param_rangefinder_turn_time, k_param_rangefinder_turn_time, // unused
k_param_sonar2_old, // unused k_param_sonar2_old, // unused
k_param_rangefinder_debounce, k_param_rangefinder_debounce, // unused
k_param_rangefinder, // rangefinder object k_param_rangefinder, // rangefinder object
// //
@ -251,13 +251,6 @@ public:
AP_Int8 fs_ekf_action; AP_Int8 fs_ekf_action;
AP_Float fs_ekf_thresh; AP_Float fs_ekf_thresh;
// obstacle avoidance control
AP_Int16 rangefinder_trigger_cm;
AP_Float rangefinder_turn_angle;
AP_Float rangefinder_turn_time;
AP_Int8 rangefinder_debounce;
// driving modes // driving modes
// //
AP_Int8 mode_channel; AP_Int8 mode_channel;

13
APMrover2/Rover.h
View File

@ -274,18 +274,6 @@ private:
// true if we have a position estimate from AHRS // true if we have a position estimate from AHRS
bool have_position; bool have_position;
// obstacle detection information
struct {
// have we detected an obstacle?
uint8_t detected_count;
float turn_angle;
uint16_t rangefinder1_distance_cm;
uint16_t rangefinder2_distance_cm;
// time when we last detected an obstacle, in milliseconds
uint32_t detected_time_ms;
} obstacle;
// range finder last update (used for DPTH logging) // range finder last update (used for DPTH logging)
uint32_t rangefinder_last_reading_ms; uint32_t rangefinder_last_reading_ms;
@ -415,7 +403,6 @@ private:
void Log_Write_Startup(uint8_t type); void Log_Write_Startup(uint8_t type);
void Log_Write_Steering(); void Log_Write_Steering();
void Log_Write_Throttle(); void Log_Write_Throttle();
void Log_Write_Rangefinder();
void Log_Write_RC(void); void Log_Write_RC(void);
void Log_Write_Vehicle_Startup_Messages(); void Log_Write_Vehicle_Startup_Messages();
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);

13
APMrover2/mode.cpp
View File

@ -408,13 +408,6 @@ void Mode::navigate_to_waypoint()
// calculate steering output given a turn rate and speed // calculate steering output given a turn rate and speed
void Mode::calc_steering_from_turn_rate(float turn_rate, float speed, bool reversed) void Mode::calc_steering_from_turn_rate(float turn_rate, float speed, bool reversed)
{ {
// add obstacle avoidance response to lateral acceleration target
// ToDo: replace this type of object avoidance with path planning
if (!reversed) {
const float lat_accel_adj = (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g;
turn_rate += attitude_control.get_turn_rate_from_lat_accel(lat_accel_adj, speed);
}
// calculate and send final steering command to motor library // calculate and send final steering command to motor library
const float steering_out = attitude_control.get_steering_out_rate(turn_rate, const float steering_out = attitude_control.get_steering_out_rate(turn_rate,
g2.motors.limit.steer_left, g2.motors.limit.steer_left,
@ -428,12 +421,6 @@ void Mode::calc_steering_from_turn_rate(float turn_rate, float speed, bool rever
*/ */
void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reversed) void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reversed)
{ {
// add obstacle avoidance response to lateral acceleration target
// ToDo: replace this type of object avoidance with path planning
if (!reversed) {
lat_accel += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g;
}
// constrain to max G force // constrain to max G force
lat_accel = constrain_float(lat_accel, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS); lat_accel = constrain_float(lat_accel, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS);

64
APMrover2/sensors.cpp
View File

@ -129,71 +129,7 @@ void Rover::accel_cal_update() {
void Rover::read_rangefinders(void) void Rover::read_rangefinders(void)
{ {
rangefinder.update(); rangefinder.update();
AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0);
AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1);
if (s0 == nullptr || s0->status() == RangeFinder::RangeFinder_NotConnected) {
// this makes it possible to disable rangefinder at runtime
return;
}
if (s1 != nullptr && s1->has_data()) {
// we have two rangefinders
obstacle.rangefinder1_distance_cm = s0->distance_cm();
obstacle.rangefinder2_distance_cm = s1->distance_cm();
if (obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(g.rangefinder_trigger_cm) &&
obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(obstacle.rangefinder2_distance_cm)) {
// we have an object on the left
if (obstacle.detected_count < 127) {
obstacle.detected_count++;
}
if (obstacle.detected_count == g.rangefinder_debounce) {
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder1 obstacle %u cm",
(unsigned int)obstacle.rangefinder1_distance_cm);
}
obstacle.detected_time_ms = AP_HAL::millis();
obstacle.turn_angle = g.rangefinder_turn_angle;
} else if (obstacle.rangefinder2_distance_cm < static_cast<uint16_t>(g.rangefinder_trigger_cm)) {
// we have an object on the right
if (obstacle.detected_count < 127) {
obstacle.detected_count++;
}
if (obstacle.detected_count == g.rangefinder_debounce) {
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder2 obstacle %u cm",
(unsigned int)obstacle.rangefinder2_distance_cm);
}
obstacle.detected_time_ms = AP_HAL::millis();
obstacle.turn_angle = -g.rangefinder_turn_angle;
}
} else {
// we have a single rangefinder
obstacle.rangefinder1_distance_cm = s0->distance_cm();
obstacle.rangefinder2_distance_cm = 0;
if (obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(g.rangefinder_trigger_cm)) {
// obstacle detected in front
if (obstacle.detected_count < 127) {
obstacle.detected_count++;
}
if (obstacle.detected_count == g.rangefinder_debounce) {
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder obstacle %u cm",
(unsigned int)obstacle.rangefinder1_distance_cm);
}
obstacle.detected_time_ms = AP_HAL::millis();
obstacle.turn_angle = g.rangefinder_turn_angle;
}
}
Log_Write_Rangefinder();
Log_Write_Depth(); Log_Write_Depth();
// no object detected - reset after the turn time
if (obstacle.detected_count >= g.rangefinder_debounce &&
AP_HAL::millis() > obstacle.detected_time_ms + g.rangefinder_turn_time*1000) {
gcs().send_text(MAV_SEVERITY_INFO, "Obstacle passed");
obstacle.detected_count = 0;
obstacle.turn_angle = 0;
}
} }
// initialise proximity sensor // initialise proximity sensor