From 6603be080fd8c07430d692ed6cb4d9e5f9d00bfb Mon Sep 17 00:00:00 2001 From: zbr Date: Mon, 22 Mar 2021 18:50:57 +0800 Subject: [PATCH] add underwater sonar data,view,log --- APMrover2/Rover.cpp | 2 +- APMrover2/Underwater_Sonar.cpp | 138 +++++++++++++++++++++++++++++ APMrover2/Underwater_Sonar.h | 42 +++++++++ APMrover2/version.h | 4 +- libraries/AP_Logger/AP_Logger.h | 2 + libraries/AP_Logger/LogFile.cpp | 20 +++++ libraries/AP_Logger/LogStructure.h | 18 +++- 7 files changed, 221 insertions(+), 5 deletions(-) create mode 100644 APMrover2/Underwater_Sonar.cpp create mode 100644 APMrover2/Underwater_Sonar.h diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index cd4071fc40..15250d023f 100644 --- a/APMrover2/Rover.cpp +++ b/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"); } diff --git a/APMrover2/Underwater_Sonar.cpp b/APMrover2/Underwater_Sonar.cpp new file mode 100644 index 0000000000..e863d79ade --- /dev/null +++ b/APMrover2/Underwater_Sonar.cpp @@ -0,0 +1,138 @@ +#include "Underwater_Sonar.h" + +#include + +#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(); + } + +} diff --git a/APMrover2/Underwater_Sonar.h b/APMrover2/Underwater_Sonar.h new file mode 100644 index 0000000000..f0a72fe6f8 --- /dev/null +++ b/APMrover2/Underwater_Sonar.h @@ -0,0 +1,42 @@ +#ifndef __AIRMAR_P66__ +#define __AIRMAR_P66__ + +#include +#include +#include +#include + + +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 diff --git a/APMrover2/version.h b/APMrover2/version.h index 1208beb4a0..d5f13078e3 100644 --- a/APMrover2/version.h +++ b/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 diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 603097f0ac..64ca83a857 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/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 ¤t_loc, uint64_t timestamp_us=0); void Write_Camera(const Location ¤t_loc, uint64_t timestamp_us=0); + void Write_Underwater_Sonar(const Location ¤t_loc); + void Write_Trigger(const Location ¤t_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); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 46a63892ed..49d3a03fbb 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -13,6 +13,8 @@ #include #include +#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 ¤t_l WriteCriticalBlock(&pkt, sizeof(pkt)); } +void AP_Logger::Write_Underwater_Sonar(const Location ¤t_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 ¤t_loc, uint64_t timestamp_us) { diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index d3ecd18388..f0277326d7 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/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,8 +1429,10 @@ 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 \ { LOG_IMU2_MSG, sizeof(log_IMU), \ @@ -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,