Rover: remove Dodge avoidance

this has been replaced with BendyRuler OA Path Planning

APMrover2/Log.cpp

@@ -224,43 +224,6 @@ void Rover::Log_Write_Throttle()
     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)
 {
     logger.Write_RCIN();
@@ -290,8 +253,6 @@ const LogStructure Rover::log_structure[] = {
       "THR", "Qhffff", "TimeUS,ThrIn,ThrOut,DesSpeed,Speed,AccY", "s--nno", "F--000" },
     { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
       "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),
       "STER", "Qhfffff",   "TimeUS,SteerIn,SteerOut,DesLatAcc,LatAcc,DesTurnRate,TurnRate", "s--ookk", "F--0000" },
     { 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_Startup(uint8_t type) {}
 void Rover::Log_Write_Throttle() {}
-void Rover::Log_Write_Rangefinder() {}
 void Rover::Log_Write_RC(void) {}
 void Rover::Log_Write_Steering() {}
 void Rover::Log_Write_Vehicle_Startup_Messages() {}

APMrover2/Parameters.cpp

@@ -169,41 +169,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Advanced
     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
     // @DisplayName: Mode channel
     // @Description: RC Channel to use for driving mode control

APMrover2/Parameters.h

@@ -149,11 +149,11 @@ public:
         // obstacle control
         k_param_sonar_enabled = 190,  // deprecated, can be removed
         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_rangefinder_debounce,
+        k_param_rangefinder_debounce, // unused
         k_param_rangefinder,          // rangefinder object
 
         //
@@ -251,13 +251,6 @@ public:
     AP_Int8     fs_ekf_action;
     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
     //
     AP_Int8     mode_channel;

APMrover2/Rover.h

@@ -274,18 +274,6 @@ private:
     // true if we have a position estimate from AHRS
     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)
     uint32_t rangefinder_last_reading_ms;
 
@@ -415,7 +403,6 @@ private:
     void Log_Write_Startup(uint8_t type);
     void Log_Write_Steering();
     void Log_Write_Throttle();
-    void Log_Write_Rangefinder();
     void Log_Write_RC(void);
     void Log_Write_Vehicle_Startup_Messages();
     void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);

APMrover2/mode.cpp

@@ -408,13 +408,6 @@ void Mode::navigate_to_waypoint()
 // calculate steering output given a turn rate and speed
 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
     const float steering_out = attitude_control.get_steering_out_rate(turn_rate,
                                                                       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)
 {
-    // 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
     lat_accel = constrain_float(lat_accel, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS);
 

APMrover2/sensors.cpp

@@ -129,71 +129,7 @@ void Rover::accel_cal_update() {
 void Rover::read_rangefinders(void)
 {
     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();
-
-    // 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