add underwater sonar data,view,log

APMrover2/Rover.cpp

@@ -297,7 +297,7 @@ void Rover::one_second_loop(void)
     // send latest param values to wp_nav
     g2.wp_nav.set_turn_params(g.turn_max_g, g2.turn_radius, g2.motors.have_skid_steering());
 
-    underwater_sonar.update(serial_manager);
+    underwater_sonar.update(serial_manager,current_loc);
     // gcs().send_text(MAV_SEVERITY_CRITICAL, "test");
 
 }

APMrover2/Underwater_Sonar.cpp

@@ -0,0 +1,138 @@
+#include "Underwater_Sonar.h"
+
+#include <AP_Logger/AP_Logger.h>
+
+#define MY_USE_DEBUG 1
+#if MY_USE_DEBUG
+
+# define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_CRITICAL,fmt, ## args)
+#else 
+//  
+ # define Debug(fmt, args ...) 
+#endif
+
+Underwater_Sonar::Underwater_Sonar(void):
+    ucCRCHi(0),
+    ucCRCLo(0),
+    iIndex(0)
+{
+    _singleton = this;
+}
+
+bool Underwater_Sonar::init(const AP_SerialManager& serial_manager)
+{
+	_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Underwater_Sonar,0);
+	if (_uart != nullptr) {
+	        _uart->begin(4800U);
+
+            return true;
+	        // _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
+	}
+    return false;    
+}
+
+bool Underwater_Sonar::read(void)
+{
+    if (_uart == nullptr) {
+        return false;
+    }
+    // Debug("read");
+    bool ret = false;
+
+
+    uint32_t nbytes = _uart->available();
+    uint32_t data_len = nbytes;
+    while(nbytes-->0)
+    {
+        uint8_t temp = _uart->read();
+        sonar_data[data_len-nbytes] = temp;
+        // Debug("read %d:%d",nbytes,temp);
+        if(_decode(temp)){
+
+            // write(&temp,1);
+            for(int j = 0; j < 8; j++){
+                // radar_data = _sonar[j];
+                // memcpy(*radar_data,_sonar[j],sizeof(_sonar[j]));
+                radar_data[j] = atof(_term[j]);
+                
+                // cout << j << " : " << radar_data[j] << endl;
+            }
+            // data_index +=1;
+            //     Debug("%d: %d,%.2f,%d,%d,%d,%.2f,%.d,%d ",data_index,(int)radar_data[0],radar_data[1],(int)radar_data[2],(int)radar_data[3], \
+            //                         (int)radar_data[4],radar_data[5],(int)radar_data[6],(int)radar_data[7]);
+            ret = true;
+        }
+    }
+    // Debug("read %s",sonar_data);
+    return ret;
+}
+
+bool Underwater_Sonar::_decode(char c)
+{
+    bool valid_sentence = false;
+    // _term[_term_num][_term_offset];  
+    switch (c) {
+        case '\r':
+        case '\n':
+            _term_offset = 0;
+            _term_num = 0;
+            valid_sentence = true;
+            return valid_sentence;
+        case ' ':
+            _term_num+=1;
+            _term_offset = 0;
+            return valid_sentence;
+
+    }
+    if (_term_offset < sizeof(_term[_term_num]) - 1)
+        _term[_term_num][_term_offset++] = c;
+    return valid_sentence;
+}
+
+
+void Underwater_Sonar::write(const uint8_t *data, uint16_t len)
+{
+
+    if (_uart == nullptr) {
+        return;
+    }
+    if (_uart->txspace() > len) {
+        _uart->write(data, len);
+    } else {
+        // Debug("FLOW: Not enough TXSPACE");
+    }
+}
+
+void Underwater_Sonar::update(const AP_SerialManager& serial_manager, const struct Location &_loc){
+    if (_uart == nullptr) {
+        _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Underwater_Sonar,0);
+        if (_uart != nullptr) {
+                _uart->begin(4800U);
+            Debug("---------Underwater_Sonar: ok\n");
+        }
+    }
+    if(read()){
+        AP_Logger *logger = AP_Logger::get_singleton();
+        if (logger == nullptr) {
+            Debug("log error");
+            return;
+        }
+        logger->Write_Underwater_Sonar(_loc);
+        
+        Debug("%d: %d,%.2f,%d,%d,%d,%.2f,%.d,%d ",data_index,(int)radar_data[0],radar_data[1],(int)radar_data[2],(int)radar_data[3], \
+                            (int)radar_data[4],radar_data[5],(int)radar_data[6],(int)radar_data[7]);
+        
+    }
+
+}
+// singleton instance
+Underwater_Sonar *Underwater_Sonar::_singleton;
+
+namespace AP {
+
+    Underwater_Sonar &underwater_sonar()
+    {
+        return *Underwater_Sonar::get_singleton();
+    }
+
+}

APMrover2/Underwater_Sonar.h

@@ -0,0 +1,42 @@
+#ifndef __AIRMAR_P66__
+#define __AIRMAR_P66__
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <GCS_MAVLink/GCS.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+
+
+class Underwater_Sonar
+{
+public:
+    Underwater_Sonar();
+
+    // get singleton instance
+    static Underwater_Sonar *get_singleton() { return _singleton; }
+
+	bool init(const AP_SerialManager& serial_manager);
+
+    bool read(void);
+    void update(const AP_SerialManager& serial_manager, const struct Location &_loc);
+    void write(const uint8_t *data, uint16_t len);
+    bool _decode(char c);
+    
+    float radar_data[8];
+private:
+    static Underwater_Sonar *_singleton;
+    AP_HAL::UARTDriver  *_uart;
+
+    uint8_t     ucCRCHi;
+    uint8_t     ucCRCLo;
+    int         iIndex;
+    uint8_t     sonar_data[25];
+    char _term[8][6];                                                     ///< buffer for the current term within the current sentence
+    uint8_t _term_offset;                                       ///< character offset with the term being received
+    uint8_t _term_num;
+    uint32_t  data_index;
+};
+namespace AP {
+    Underwater_Sonar &underwater_sonar();
+};
+#endif

APMrover2/version.h

@@ -6,10 +6,10 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZR_Roveraa V4.0.0"
+#define THISFIRMWARE "ZR_Boat V4.0.1"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,0,0,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 4,0,1,FIRMWARE_VERSION_TYPE_DEV
 
 #define FW_MAJOR 4
 #define FW_MINOR 0

libraries/AP_Logger/AP_Logger.h

@@ -253,6 +253,8 @@ public:
     void Write_MessageF(const char *fmt, ...);
     void Write_CameraInfo(enum LogMessages msg, const Location &current_loc, uint64_t timestamp_us=0);
     void Write_Camera(const Location &current_loc, uint64_t timestamp_us=0);
+    void Write_Underwater_Sonar(const Location &current_loc);
+    
     void Write_Trigger(const Location &current_loc);
     void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t temperature, uint16_t current_tot);
     void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct);

libraries/AP_Logger/LogFile.cpp

@@ -13,6 +13,8 @@
 #include <AP_RSSI/AP_RSSI.h>
 #include <AP_GPS/AP_GPS.h>
 
+#include <../APMrover2/Underwater_Sonar.h>
+
 #include "AP_Logger.h"
 #include "AP_Logger_File.h"
 #include "AP_Logger_MAVLink.h"
@@ -590,6 +592,24 @@ void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location &current_l
     WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
+void AP_Logger::Write_Underwater_Sonar(const Location &current_loc)
+{
+
+    const Underwater_Sonar sonar = AP::underwater_sonar();
+    struct log_Underwater_Sonar pkt{
+        LOG_PACKET_HEADER_INIT(LOG_UNDERWATER_SONAR_MSG),
+        time_us         : AP_HAL::micros64(),
+        latitude    : current_loc.lat,
+        longitude   : current_loc.lng,
+        altitude    : current_loc.alt,
+    };
+
+        for (uint8_t i = 0; i < 8; i++) {
+            pkt.radar_data[i] = sonar.radar_data[i] ;
+        }
+    WriteBlock(&pkt, sizeof(pkt));
+}
+
 // Write a Camera packet
 void AP_Logger::Write_Camera(const Location &current_loc, uint64_t timestamp_us)
 {

libraries/AP_Logger/LogStructure.h

@@ -730,6 +730,16 @@ struct PACKED log_Camera {
     uint16_t yaw;
 };
 
+
+struct PACKED log_Underwater_Sonar {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    int32_t  latitude;
+    int32_t  longitude;
+    int32_t  altitude;
+    float radar_data[8];
+};
+
 struct PACKED log_Attitude {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -1419,7 +1429,9 @@ struct PACKED log_Arm_Disarm {
     { LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \
       "OABR","QBHHfLLLL","TimeUS,Active,DesYaw,Yaw,Mar,DLat,DLng,OALat,OALng", "sbddmDUDU", "F----GGGG" }, \
     { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
-      "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }
+      "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \
+    { LOG_UNDERWATER_SONAR_MSG, sizeof(log_Underwater_Sonar), \
+      "UDSR", "QLLeffffffff","TimeUS,Lat,Lng,Alt,C1,C2,C3,C4,C5,C6,C7,C8", "s--DUm--------", "F-----------" }
     
 // messages for more advanced boards
 #define LOG_EXTRA_STRUCTURES \
@@ -1671,6 +1683,8 @@ enum LogMessages : uint8_t {
     LOG_XKV1_MSG,
     LOG_XKV2_MSG,
 
+    LOG_UNDERWATER_SONAR_MSG,
+
     LOG_FORMAT_MSG = 128, // this must remain #128
 
     LOG_PARAMETER_MSG,