[WIP]Feature/pixy parser

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -11,6 +11,8 @@
 #include <AP_UAVCAN/AP_UAVCAN.h>
 #endif
 
+//param show PLND_type
+
 extern const AP_HAL::HAL& hal;
 
 const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
@@ -178,8 +180,7 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
     }
 }
 
-bool AC_PrecLand::target_acquired()
-{
+bool AC_PrecLand::target_acquired() {
     if ((AP_HAL::millis()-_last_update_ms) >= 2000) {
         _estimator_initialized = false;
         _target_acquired = false;
@@ -188,6 +189,17 @@ bool AC_PrecLand::target_acquired()
     return _target_acquired;
 }
 
+void AC_PrecLand::set_target_acquired(bool _target_acquired_value) {
+    if (_target_acquired_value == false) {
+        _target_acquired = _target_acquired_value;
+        _estimator_initialized = false;
+    }
+    else {
+        _target_acquired = _target_acquired_value;
+    }
+}
+
+
 bool AC_PrecLand::get_height_above_target_cm(int32_t& ret) {
 #if !HAL_WITH_UAVCAN
     return false;

libraries/AC_PrecLand/AC_PrecLand.h

@@ -77,6 +77,9 @@ class AC_PrecLand
     // returns true when the landing target has been detected
     bool target_acquired();
 
+    // sets the value of _target_acquired
+    void set_target_acquired(bool _target_acquired_value);
+
     // process a LANDING_TARGET mavlink message
     void handle_msg(mavlink_message_t* msg);
 

libraries/AC_PrecLand/AC_PrecLand_Backend.h

@@ -10,7 +10,7 @@ class AC_PrecLand_Backend
 {
 public:
     // Constructor
-    AC_PrecLand_Backend(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) :
+    AC_PrecLand_Backend(AC_PrecLand& frontend, AC_PrecLand::precland_state& state) :
         _frontend(frontend),
         _state(state) {}
 
@@ -43,6 +43,6 @@ class AC_PrecLand_Backend
     int8_t get_bus(void) const { return _frontend._bus.get(); }
 
 protected:
-    const AC_PrecLand&  _frontend;          // reference to precision landing front end
+    AC_PrecLand&  _frontend;          // reference to precision landing front end
     AC_PrecLand::precland_state &_state;    // reference to this instances state
 };

libraries/AC_PrecLand/AC_PrecLand_Companion.cpp

@@ -1,10 +1,11 @@
+
 #include <AP_HAL/AP_HAL.h>
 #include "AC_PrecLand_Companion.h"
 
 extern const AP_HAL::HAL& hal;
 
 // Constructor
-AC_PrecLand_Companion::AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
+AC_PrecLand_Companion::AC_PrecLand_Companion(AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
     : AC_PrecLand_Backend(frontend, state),
       _timestamp_us(0),
       _distance_to_target(0.0f),

libraries/AC_PrecLand/AC_PrecLand_Companion.h

@@ -1,3 +1,4 @@
+
 #pragma once
 
 #include <AP_Common/AP_Common.h>
@@ -13,7 +14,7 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend
 {
 public:
     // Constructor
-    AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
+    AC_PrecLand_Companion(AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
 
     // perform any required initialisation of backend
     void init() override;

libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp

@@ -1,23 +1,59 @@
+
 #include <AP_HAL/AP_HAL.h>
 #include "AC_PrecLand_IRLock.h"
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
 // Constructor
-AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
+AC_PrecLand_IRLock::AC_PrecLand_IRLock(AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
     : AC_PrecLand_Backend(frontend, state),
       irlock(),
       _have_los_meas(false),
-      _los_meas_time_ms(0)
+      _los_meas_time_ms(0),
+      _multiple_target_timestamp_log(),
+      _index(0)
 {
 }
 
 // init - perform initialisation of this backend
-void AC_PrecLand_IRLock::init()
+void AC_PrecLand_IRLock::init() 
 {
     irlock.init(get_bus());
 }
 
+//multiple_target_check - Checks if multiple targets detected for 500 ms continously. If yes, sets target_acquired=False which calls contingency landing
+void AC_PrecLand_IRLock::multiple_target_check() 
+{
+    if (_index == 24) {
+        _index = 0;
+    }
+    _multiple_target_timestamp_log[_index] = irlock.num_targets();
+    _index++;
+
+/*
+    printf("\n _multiple_target_timestamp_log: ");
+    for (size_t j=0; j<24; j++) {
+        printf("%u ", _multiple_target_timestamp_log[j]);
+    }
+*/
+    size_t count = 0;
+    for (size_t j=0; j<24; j++) {
+        if (_multiple_target_timestamp_log[j] > 1) {
+            count++;
+        }
+    }
+    uint16_t multiple_target_count_percentage = count*100/25; 
+//    printf("\nCount: %u  --  Multiple_target_count_percentage: %u", irlock.num_targets(), multiple_target_count_percentage);     
+    if (multiple_target_count_percentage >= 80) {
+//        printf("\n----------------------------------------------------------------------HIGH COUNT PERCENTAGE: %u  ------setting TARGET ACQUIRED FALSE -- %", multiple_target_count_percentage); 
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Multiple Targets Detected");
+        // Send sn alert telling multiple targets detected
+        _frontend.set_target_acquired(false); 
+    }
+}
+
+
 // update - give chance to driver to get updates from sensor
 void AC_PrecLand_IRLock::update()
 {
@@ -31,6 +67,7 @@ void AC_PrecLand_IRLock::update()
         irlock.get_unit_vector_body(_los_meas_body);
         _have_los_meas = true;
         _los_meas_time_ms = irlock.last_update_ms();
+        multiple_target_check(); 
     }
     _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000;
 }

libraries/AC_PrecLand/AC_PrecLand_IRLock.h

@@ -1,3 +1,4 @@
+
 #pragma once
 
 #include <AP_Common/AP_Common.h>
@@ -19,7 +20,7 @@ class AC_PrecLand_IRLock : public AC_PrecLand_Backend
 public:
 
     // Constructor
-    AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
+    AC_PrecLand_IRLock(AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
 
     // perform any required initialisation of backend
     void init() override;
@@ -37,10 +38,17 @@ class AC_PrecLand_IRLock : public AC_PrecLand_Backend
     // return true if there is a valid los measurement available
     bool have_los_meas() override;
 
+    //multiple_target_check - Checks if multiple targets detected for 500 ms continously.
+    //If yes, sets target_acquired=False which calls contingency landing
+    void multiple_target_check(); 
+
+
 private:
     AP_IRLock_I2C irlock;
 
     Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
     bool                _have_los_meas;         // true if there is a valid measurement from the camera
     uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
+    uint32_t            _multiple_target_timestamp_log[25];    // array of timestamps when multiple targets detected
+    size_t              _index;
 };