Rover: add proximity library

APMrover2/APMrover2.cpp

@@ -53,6 +53,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(update_GPS_10Hz,        10,   2500),
     SCHED_TASK(update_alt,             10,   3400),
     SCHED_TASK(update_beacon,          50,     50),
+    SCHED_TASK(update_proximity,       50,     50),
     SCHED_TASK(update_visual_odom,     50,     50),
     SCHED_TASK(update_wheel_encoder,   20,     50),
     SCHED_TASK(update_compass,         10,   2000),
@@ -257,6 +258,7 @@ void Rover::update_logging1(void)
     if (should_log(MASK_LOG_THR)) {
         Log_Write_Throttle();
         Log_Write_Beacon();
+        Log_Write_Proximity();
     }
 
     if (should_log(MASK_LOG_NTUN)) {

APMrover2/AP_Arming.cpp

@@ -74,7 +74,8 @@ bool AP_Arming_Rover::pre_arm_checks(bool report)
 {
     return (AP_Arming::pre_arm_checks(report)
             & rover.g2.motors.pre_arm_check(report)
-            & fence_checks(report));
+            & fence_checks(report)
+            & proximity_check(report));
 }
 
 bool AP_Arming_Rover::fence_checks(bool report)
@@ -89,3 +90,34 @@ bool AP_Arming_Rover::fence_checks(bool report)
     }
     return true;
 }
+
+// check nothing is too close to vehicle
+bool AP_Arming_Rover::proximity_check(bool report)
+{
+    // return true immediately if no sensor present
+    if (rover.g2.proximity.get_status() == AP_Proximity::Proximity_NotConnected) {
+        return true;
+    }
+
+    // return false if proximity sensor unhealthy
+    if (rover.g2.proximity.get_status() < AP_Proximity::Proximity_Good) {
+        if (report) {
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: check proximity sensor");
+        }
+        return false;
+    }
+
+    // get closest object if we might use it for avoidance
+    float angle_deg, distance;
+    if (rover.g2.proximity.get_closest_object(angle_deg, distance)) {
+        // display error if something is within 60cm
+        if (distance <= 0.6f) {
+            if (report) {
+                gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Proximity %d deg, %4.2fm", static_cast<int32_t>(angle_deg), static_cast<double>(distance));
+            }
+            return false;
+        }
+    }
+
+    return true;
+}

APMrover2/AP_Arming.h

@@ -27,6 +27,7 @@ public:
 protected:
     enum HomeState home_status() const override;
     bool fence_checks(bool report);
+    bool proximity_check(bool report);
 
 private:
     const AC_Fence& _fence;

APMrover2/GCS_Mavlink.cpp

@@ -381,6 +381,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
         CHECK_PAYLOAD_SIZE(RANGEFINDER);
         rover.send_rangefinder(chan);
         send_distance_sensor(rover.rangefinder);
+        send_proximity(rover.g2.proximity);
         break;
 
     case MSG_RPM:
@@ -1386,6 +1387,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_DISTANCE_SENSOR:
         rover.rangefinder.handle_msg(msg);
+        rover.g2.proximity.handle_msg(msg);
         break;
 
     case MAVLINK_MSG_ID_VISION_POSITION_DELTA:

APMrover2/Log.cpp

@@ -339,6 +339,12 @@ void Rover::Log_Write_WheelEncoder()
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+// Write proximity sensor distances
+void Rover::Log_Write_Proximity()
+{
+    DataFlash.Log_Write_Proximity(g2.proximity);
+}
+
 // type and unit information can be found in
 // libraries/DataFlash/Logstructure.h; search for "log_Units" for
 // units and "Format characters" for field type information
@@ -398,5 +404,6 @@ void Rover::Log_Arm_Disarm() {}
 void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
 void Rover::Log_Write_Steering() {}
 void Rover::Log_Write_WheelEncoder() {}
+void Rover::Log_Write_Proximity() {}
 
 #endif  // LOGGING_ENABLED

APMrover2/Parameters.cpp

@@ -546,6 +546,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AC_Fence/AC_Fence.cpp
     AP_SUBGROUPINFO(fence, "FENCE_", 17, ParametersG2, AC_Fence),
 
+    // @Group: PRX
+    // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp
+    AP_SUBGROUPINFO(proximity, "PRX", 18, ParametersG2, AP_Proximity),
+
     AP_GROUPEND
 };
 
@@ -559,7 +563,8 @@ ParametersG2::ParametersG2(void)
     motors(rover.ServoRelayEvents),
     attitude_control(rover.ahrs),
     smart_rtl(rover.ahrs),
-    fence(rover.ahrs)
+    fence(rover.ahrs),
+    proximity(rover.serial_manager)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

APMrover2/Parameters.h

@@ -300,6 +300,7 @@ public:
     // advanced failsafe library
     AP_AdvancedFailsafe_Rover afs;
 #endif
+
     AP_Beacon beacon;
 
     // Visual Odometry camera
@@ -332,6 +333,9 @@ public:
 
     // fence library
     AC_Fence fence;
+
+    // proximity library
+    AP_Proximity proximity;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -77,6 +77,7 @@
 #include <RC_Channel/RC_Channel.h>                  // RC Channel Library
 #include <StorageManager/StorageManager.h>
 #include <AC_Fence/AC_Fence.h>
+#include <AP_Proximity/AP_Proximity.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include <SITL/SITL.h>
 #endif
@@ -519,6 +520,7 @@ private:
     void Log_Write_Home_And_Origin();
     void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
     void Log_Write_WheelEncoder();
+    void Log_Write_Proximity();
     void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
     void log_init(void);
     void Log_Write_Vehicle_Startup_Messages();
@@ -552,6 +554,8 @@ private:
     void accel_cal_update(void);
     void read_rangefinders(void);
     void button_update(void);
+    void init_proximity();
+    void update_proximity();
     void update_sensor_status_flags(void);
 
     // Steering.cpp

APMrover2/sensors.cpp

@@ -275,6 +275,19 @@ void Rover::button_update(void)
     button.update();
 }
 
+// initialise proximity sensor
+void Rover::init_proximity(void)
+{
+    g2.proximity.init();
+    g2.proximity.set_rangefinder(&rangefinder);
+}
+
+// update proximity sensor
+void Rover::update_proximity(void)
+{
+    g2.proximity.update();
+}
+
 // update error mask of sensors and subsystems. The mask
 // uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
 // then it indicates that the sensor or subsystem is present but
@@ -297,7 +310,9 @@ void Rover::update_sensor_status_flags(void)
     if (rover.DataFlash.logging_present()) {  // primary logging only (usually File)
         control_sensors_present |= MAV_SYS_STATUS_LOGGING;
     }
-
+    if (rover.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) {
+        control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
+    }
 
     // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually
     control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL &
@@ -357,7 +372,9 @@ void Rover::update_sensor_status_flags(void)
             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         }
     }
-
+    if (rover.g2.proximity.get_status() < AP_Proximity::Proximity_Good) {
+        control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION;
+    }
     if (rover.DataFlash.logging_failed()) {
         control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
     }

APMrover2/system.cpp

@@ -92,6 +92,9 @@ void Rover::init_ardupilot()
     // initialise rangefinder
     init_rangefinder();
 
+    // init proximity sensor
+    init_proximity();
+
     // init beacons used for non-gps position estimation
     init_beacon();
 

APMrover2/wscript

@@ -23,6 +23,7 @@ def build(bld):
             'AP_WheelEncoder',
             'AP_SmartRTL',
             'AC_Fence',
+            'AP_Proximity',
         ],
     )