AP_RangeFinder: Add LUA interface to access Range Finder state

libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp

@@ -53,30 +53,39 @@ bool AP_RangeFinder_Backend::has_data() const {
 }
 
 // update status based on distance measurement
-void AP_RangeFinder_Backend::update_status()
+void AP_RangeFinder_Backend::update_status(RangeFinder::RangeFinder_State &state_arg) const
 {
     // check distance
-    if (state.distance_m > max_distance_cm() * 0.01f) {
+    if (state_arg.distance_m > max_distance_cm() * 0.01f) {
-        set_status(RangeFinder::Status::OutOfRangeHigh);
+        set_status(state_arg, RangeFinder::Status::OutOfRangeHigh);
-    } else if (state.distance_m < min_distance_cm() * 0.01f) {
+    } else if (state_arg.distance_m < min_distance_cm() * 0.01f) {
-        set_status(RangeFinder::Status::OutOfRangeLow);
+        set_status(state_arg, RangeFinder::Status::OutOfRangeLow);
     } else {
-        set_status(RangeFinder::Status::Good);
+        set_status(state_arg, RangeFinder::Status::Good);
     }
 }
 
 // set status and update valid count
-void AP_RangeFinder_Backend::set_status(RangeFinder::Status _status)
+void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_State &state_arg, RangeFinder::Status _status)
 {
-    state.status = _status;
+    state_arg.status = _status;
 
     // update valid count
     if (_status == RangeFinder::Status::Good) {
-        if (state.range_valid_count < 10) {
+        if (state_arg.range_valid_count < 10) {
-            state.range_valid_count++;
+            state_arg.range_valid_count++;
         }
     } else {
-        state.range_valid_count = 0;
+        state_arg.range_valid_count = 0;
     }
 }
 
+#if AP_SCRIPTING_ENABLED
+// get a copy of state structure
+void AP_RangeFinder_Backend::get_state(RangeFinder::RangeFinder_State &state_arg)
+{
+    WITH_SEMAPHORE(_sem);
+    state_arg = state;
+}
+#endif
+

libraries/AP_RangeFinder/AP_RangeFinder_Backend.h

@@ -36,9 +36,11 @@ public:
     virtual void handle_msg(const mavlink_message_t &msg) { return; }
 
 #if AP_SCRIPTING_ENABLED
-    // Returns false if scripting backing hasn't been setup
+    void get_state(RangeFinder::RangeFinder_State &state_arg);
-    // Get distance from lua script
+
-    virtual bool handle_script_msg(float dist_m) { return false; }
+    // Returns false if scripting backing hasn't been setup.
+    virtual bool handle_script_msg(float dist_m) { return false; } // legacy interface
+    virtual bool handle_script_msg(const RangeFinder::RangeFinder_State &state_arg) { return false; }
 #endif
 
 #if HAL_MSP_RANGEFINDER_ENABLED
@@ -80,10 +82,12 @@ public:
 protected:
 
     // update status based on distance measurement
-    void update_status();
+    void update_status(RangeFinder::RangeFinder_State &state_arg) const;
+    void update_status() { update_status(state); }
 
     // set status and update valid_count
-    void set_status(RangeFinder::Status status);
+    static void set_status(RangeFinder::RangeFinder_State &state_arg, RangeFinder::Status status);
+    void set_status(RangeFinder::Status status) { set_status(state, status); }
 
     RangeFinder::RangeFinder_State &state;
     AP_RangeFinder_Params &params;

libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp

@@ -24,31 +24,49 @@
 AP_RangeFinder_Lua::AP_RangeFinder_Lua(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
     AP_RangeFinder_Backend(_state, _params)
 {
-    set_status(RangeFinder::Status::NoData);
 }
 
 
-// Set the distance based on a Lua Script
+// Process range finder data from a lua driver. The state structure needs to be completely
-bool AP_RangeFinder_Lua::handle_script_msg(float dist_m)
+// filled in by the lua script. The data passed to this method is copied to a pending_state
+// structure. The update() method periodically copies data from pending_state to state. The get_state()
+// method returns data from state.
+bool AP_RangeFinder_Lua::handle_script_msg(const RangeFinder::RangeFinder_State &state_arg)
 {
-    state.last_reading_ms = AP_HAL::millis();
+    WITH_SEMAPHORE(_sem);
-    _distance_m = dist_m;
+    _state_pending = state_arg;
     return true;
 }
 
+// Process range finder data from a lua driver - legacy interface. This method takes
+// a distance measurement and fills in the pending state structure. In this legacy mode
+// the lua script only passes in a distance measurement and does not manage the rest
+// of the fields in the state structure.
+bool AP_RangeFinder_Lua::handle_script_msg(float dist_m) {
 
-// update the state of the sensor
+    const uint32_t now = AP_HAL::millis();
+
+    WITH_SEMAPHORE(_sem);
+
+    // Time out on incoming data; if we don't get new data in 500ms, dump it
+    if (now - _state_pending.last_reading_ms > AP_RANGEFINDER_LUA_TIMEOUT_MS) {
+        set_status(_state_pending, RangeFinder::Status::NoData);
+    } else {
+        _state_pending.last_reading_ms = now;
+        _state_pending.distance_m = dist_m;
+        _state_pending.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
+        _state_pending.voltage_mv = 0;
+        update_status(_state_pending);
+    }
+
+    return true;
+}
+
+// Update the state of the sensor
 void AP_RangeFinder_Lua::update(void)
 {
-    //Time out on incoming data; if we don't get new
+    WITH_SEMAPHORE(_sem);
-    //data in 500ms, dump it
+    state = _state_pending;
-    if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_LUA_TIMEOUT_MS) {
-        set_status(RangeFinder::Status::NoData);
-        state.distance_m = 0.0f;
-    } else {
-        state.distance_m = _distance_m;
-        update_status();
-    }
 }
 
 #endif  // AP_RANGEFINDER_LUA_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_Lua.h

@@ -21,6 +21,7 @@ public:
 
     // Get update from Lua script
     bool handle_script_msg(float dist_m) override;
+    bool handle_script_msg(const RangeFinder::RangeFinder_State &state_arg) override;
 
     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
         return MAV_DISTANCE_SENSOR_UNKNOWN;
@@ -28,7 +29,7 @@ public:
 
 private:
 
-    float _distance_m;   // stored data from lua script:
+    RangeFinder::RangeFinder_State _state_pending = {};
 };
 
 #endif  // AP_RANGEFINDER_LUA_ENABLED