Fix upflow data msg bugs.

Author: uavfly

libraries/AP_Common/AP_UpFlow_TOF.cpp

@@ -2,7 +2,17 @@
 
 static UPFLOW_TOF UPFLOW_TOF_DATA;
 
-UPFLOW_TOF* get_upflow_tof()
+/* UPFLOW_TOF* get_upflow_tof_ptr()
 {
     return &UPFLOW_TOF_DATA;
-}
+} */
+
+UPFLOW_TOF get_upflow_tof()
+{
+    return UPFLOW_TOF_DATA;
+}
+
+void set_upflow_tof( UPFLOW_TOF data )
+{
+    UPFLOW_TOF_DATA = data;
+}

libraries/AP_Common/AP_UpFlow_TOF.h

@@ -8,4 +8,6 @@ typedef struct{
     bool        if_opt_ok;
 }UPFLOW_TOF;
 
-UPFLOW_TOF* get_upflow_tof();
+//UPFLOW_TOF* get_upflow_tof_ptr();
+UPFLOW_TOF get_upflow_tof();
+void set_upflow_tof( UPFLOW_TOF data );

libraries/AP_OpticalFlow/AP_OpticalFlow_UPFLOW_LC30x.cpp

@@ -52,7 +52,8 @@
 #define UPFLOW_TIMEOUT_SEC         0.3f
 
 extern const AP_HAL::HAL& hal;
-static bool upflow_lc30x_message_sent = false;
+
+/* static bool upflow_lc30x_message_sent = false;
 
 // internal parameter configuration instructions
 static const uint8_t upflow_internal_para[4] = {0x96, 0x26, 0xbc, 0x50};
@@ -77,7 +78,7 @@ static const uint8_t upflow_sensor_cfg[]= {
     0xab, 0x16, 0xac, 0x3c, 0xad, 0xf0, 0xae, 0x57, 0xc6, 0xaa, 0xd2, 0x78, 0xd0, 0xb4, 0xd1, 0x00, 0xc8, 0x10, 0xc9, 0x12,
     0xd3, 0x09, 0xd4, 0x2a, 0xee, 0x4c, 0x7e, 0xfa, 0x74, 0xa7, 0x78, 0x4e, 0x60, 0xe7, 0x61, 0xc8, 0x6d, 0x70, 0x1e, 0x39,
     0x98, 0x1a
-};
+}; */
 
 
 // constructor
@@ -126,7 +127,7 @@ void AP_OpticalFlow_UPFLOW_LC30x::update(void)
     }
 
     // init upflow,only once
-    if (!upflow_lc30x_message_sent) {
+    /* if (!upflow_lc30x_message_sent) {
         
         uint8_t buf[30];
 		uint16_t len = sizeof(upflow_sensor_cfg);
@@ -154,7 +155,7 @@ void AP_OpticalFlow_UPFLOW_LC30x::update(void)
 		uart->write( buf, 1 );	
 
         upflow_lc30x_message_sent = true; 
-    }
+    } */
 
 
     // record gyro values as long as they are being used

libraries/AP_OpticalFlow/AP_OpticalFlow_UPFLOW_Tx.cpp

@@ -105,7 +105,7 @@ void AP_OpticalFlow_UPFLOW_Tx::update(void)
         return;
     }
 
-    static UPFLOW_TOF* UPFLOW_TOF_DATA = get_upflow_tof();
+    static UPFLOW_TOF UPFLOW_TOF_DATA = get_upflow_tof();
 
     // setup upflow link, only once
     if (!upflow_tx_message_sent) {
@@ -196,9 +196,9 @@ void AP_OpticalFlow_UPFLOW_Tx::update(void)
         _applyYaw(state.flowRate);
 
         //update tof
-        UPFLOW_TOF_DATA->if_opt_ok = true;
-        UPFLOW_TOF_DATA->ground_distance = updata.ground_distance;
-        UPFLOW_TOF_DATA->tof_valid = updata.tof_valid;
+        UPFLOW_TOF_DATA.if_opt_ok = true;
+        UPFLOW_TOF_DATA.ground_distance = updata.ground_distance;
+        UPFLOW_TOF_DATA.tof_valid = updata.tof_valid;
 
         //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UPFLOW_Tx: OK");
 
@@ -208,11 +208,14 @@ void AP_OpticalFlow_UPFLOW_Tx::update(void)
         state.bodyRate.zero();
 
         //update tof
-        UPFLOW_TOF_DATA->if_opt_ok = false;
-        UPFLOW_TOF_DATA->ground_distance = 0;
-        UPFLOW_TOF_DATA->tof_valid = 0;
+        UPFLOW_TOF_DATA.if_opt_ok = false;
+        UPFLOW_TOF_DATA.ground_distance = 0;
+        UPFLOW_TOF_DATA.tof_valid = 0;
     }
 
+    // update the rangefinder state
+    set_upflow_tof(UPFLOW_TOF_DATA);
+
     _update_frontend(state);
 
     // reset gyro sum

libraries/AP_RangeFinder/AP_RangeFinder_UPFLOW_TOF.cpp

@@ -46,11 +46,11 @@ bool AP_RangeFinder_UPFLOW_TOF::detect()
  */
 void AP_RangeFinder_UPFLOW_TOF::update(void)
 {
-    static UPFLOW_TOF* tof_data = get_upflow_tof();
+    static UPFLOW_TOF tof_data = get_upflow_tof();
     // check validity of the data
-    if( tof_data->if_opt_ok == true && tof_data->tof_valid != 0 ){
-        state.distance_m = tof_data->ground_distance * 0.001f;
-        tof_data->if_opt_ok = false;
+    if( tof_data.if_opt_ok == true && tof_data.tof_valid != 0 ){
+        state.distance_m = tof_data.ground_distance * 0.001f;
+        tof_data.if_opt_ok = false;
         e = 0;
         //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "tof: OK");
     }else{
@@ -61,6 +61,9 @@ void AP_RangeFinder_UPFLOW_TOF::update(void)
             //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "tof: err");
         }
     }
+
+    // update the rangefinder state
+    set_upflow_tof(tof_data);
 
     // update range_valid state based on distance measured
     update_status();