From 3ec90ae26677d6b0af2771f293959f67abc38027 Mon Sep 17 00:00:00 2001 From: zbr Date: Wed, 28 Jul 2021 18:22:32 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8D=95=E6=B3=A2=E6=9D=9F=E4=BF=AE=E6=94=B9?= =?UTF-8?q?=EF=BC=8Cuavcan=E6=A8=A1=E5=BC=8F=E5=92=8C=E4=B8=B2=E5=8F=A3?= =?UTF-8?q?=E6=A8=A1=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- APMrover2/Log.cpp | 11 ++- APMrover2/Rover.cpp | 4 +- APMrover2/Rover.h | 2 +- APMrover2/system.cpp | 2 +- libraries/AP_Logger/LogFile.cpp | 26 ++++--- libraries/AP_Logger/LogStructure.h | 19 +++-- .../AP_RangeFinder/AP_RangeFinder_NMEA.cpp | 69 +++++++------------ .../AP_RangeFinder/AP_RangeFinder_NMEA.h | 20 +++++- libraries/GCS_MAVLink/GCS_Common.cpp | 6 +- 9 files changed, 85 insertions(+), 74 deletions(-) diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 5c1615a3e2..426924bf4e 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -49,6 +49,15 @@ void Rover::Log_Write_Depth() if (!ahrs.get_position(loc)) { return; } + float deepthR = (float)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f); + static float last_deep; + static uint32_t last_1s; + uint32_t tnow = AP_HAL::millis(); + if (tnow - last_1s > 1000 || fabs(last_deep - deepthR) > 0.01) { + last_1s = tnow; + last_deep = deepthR; + logger.Write_Underwater_Sonar(loc); + } // check if new sensor reading has arrived uint32_t reading_ms = rangefinder.last_reading_ms(ROTATION_PITCH_270); @@ -62,7 +71,7 @@ void Rover::Log_Write_Depth() AP_HAL::micros64(), loc.lat, loc.lng, - (double)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f)); + deepthR); } // guided mode logging diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index 2147b7cd5b..a6dc3c2ca4 100644 --- a/APMrover2/Rover.cpp +++ b/APMrover2/Rover.cpp @@ -263,10 +263,10 @@ void Rover::update_logging2(void) // underwater_sonar.update(serial_manager,current_loc); uint16_t rc8_in = RC_Channels::rc_channel(CH_8)->get_radio_in(); - uint16_t rc9_in = RC_Channels::rc_channel(CH_9)->get_radio_in(); + // uint16_t rc9_in = RC_Channels::rc_channel(CH_9)->get_radio_in(); AP_Relay *_apm_relay = AP::relay(); rc8_in > 1500?_apm_relay->on(1):_apm_relay->off(1); - rc9_in > 1500?_apm_relay->on(2):_apm_relay->off(2); + // rc9_in > 1500?_apm_relay->on(2):_apm_relay->off(2); } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 7f2c9399fb..6d745365d6 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -441,7 +441,7 @@ private: static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, "_failsafe_priorities is missing the sentinel"); - Underwater_Sonar underwater_sonar; + // Underwater_Sonar underwater_sonar; public: void mavlink_delay_cb(); void failsafe_check(); diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 7dfac264d7..4225a97f67 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -163,7 +163,7 @@ void Rover::init_ardupilot() // flag that initialisation has completed initialised = true; - underwater_sonar.init(serial_manager); + // underwater_sonar.init(serial_manager); #if AP_PARAM_KEY_DUMP AP_Param::show_all(hal.console, true); #endif diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index cc3ddf1b79..1c8113268e 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -594,26 +594,30 @@ void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location ¤t_l void AP_Logger::Write_Underwater_Sonar(const Location ¤t_loc) { - int32_t altitude_gps; - const AP_GPS &gps = AP::gps(); - if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { - altitude_gps = gps.location().alt; - } else { - altitude_gps = 0; + + const RangeFinder *rangefinder = RangeFinder::get_singleton(); + AP_Baro &baro = AP::baro(); + // only log depth on boats with working downward facing range finders + if ( !rangefinder->has_data_orient(ROTATION_PITCH_270)) { + return; } + float deepthR = (float)(rangefinder->distance_cm_orient(ROTATION_PITCH_270) * 0.01f); + + const AP_GPS &gps = AP::gps(); - 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 : altitude_gps, + altitude : current_loc.alt, + temperature : baro.get_temperature(0), + deepth : deepthR, + gps_lat:gps.location().lat, + gps_lng:gps.location().lng, + gps_alt:gps.location().alt, }; - for (uint8_t i = 0; i < 8; i++) { - pkt.radar_data[i] = sonar.radar_data[i] ; - } WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index f0277326d7..7df5ef5747 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -732,12 +732,17 @@ struct PACKED log_Camera { 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]; + LOG_PACKET_HEADER; + uint64_t time_us; + int32_t latitude; + int32_t longitude; + int32_t altitude; + // float radar_data[8]; + float temperature; + float deepth; + int32_t gps_lat; + int32_t gps_lng; + int32_t gps_alt; }; struct PACKED log_Attitude { @@ -1431,7 +1436,7 @@ struct PACKED log_Arm_Disarm { { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \ "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-----------" } + "UDSR", "QLLeffLLe","TimeUS,Lat,Lng,Alt,Temp,Deep,GLat,GLng,GAlt", "sDUm--DUm", "FGGB--GGB" } // messages for more advanced boards #define LOG_EXTRA_STRUCTURES \ diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index fed8abe0c2..7175e54406 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -62,27 +62,22 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm) if (uart == nullptr) { return false; } + bool ret = false; - // read any available lines from the lidar - float sum = 0.0f; - uint16_t count = 0; int16_t nbytes = uart->available(); while (nbytes-- > 0) { - char c = uart->read(); + uint8_t c = uart->read(); if (decode(c)) { - sum += _distance_m; - count++; + ret = true; + _sonar_deep = radar_data[1]; + _sonar_voltage = radar_data[5]; } } - // return false on failure - if (count == 0) { - return false; - } // return average of all measurements - reading_cm = 100.0f * sum / count; - return true; + reading_cm = 100.0f * _sonar_deep; + return ret; } // add a single character to the buffer and attempt to decode @@ -90,43 +85,25 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm) bool AP_RangeFinder_NMEA::decode(char c) { switch (c) { - case ',': - // end of a term, add to checksum - _checksum ^= c; - FALLTHROUGH; - case '\r': - case '\n': - case '*': - { - // null terminate and decode latest term - _term[_term_offset] = 0; - bool valid_sentence = decode_latest_term(); - - // move onto next term - _term_number++; - _term_offset = 0; - _term_is_checksum = (c == '*'); - return valid_sentence; - } - - case '$': // sentence begin - _sentence_type = SONAR_UNKNOWN; - _term_number = 0; - _term_offset = 0; - _checksum = 0; - _term_is_checksum = false; - _distance_m = -1.0f; - return false; + // case '\r': + case '\n': + _term_offset = 0; + decode_step = 0; + memset(_term,0,sizeof(_term)); + return true; + case ' ': + if(decode_step < 8){ + radar_data[decode_step] = atof(_term); + _term_offset = 0; + decode_step += 1; + memset(_term,0,sizeof(_term)); + } + break; } - // ordinary characters are added to term - if (_term_offset < sizeof(_term) - 1) { + if (_term_offset < sizeof(_term) - 1) _term[_term_offset++] = c; - } - if (!_term_is_checksum) { - _checksum ^= c; - } - + return false; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index 743b7e8537..e96ea14605 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -63,11 +63,27 @@ private: AP_HAL::UARTDriver *uart = nullptr; // pointer to serial uart // message decoding related members - char _term[15]; // buffer for the current term within the current sentence - uint8_t _term_offset; // offset within the _term buffer where the next character should be placed + // char _term[15]; // buffer for the current term within the current sentence + // uint8_t _term_offset; // offset within the _term buffer where the next character should be placed uint8_t _term_number; // term index within the current sentence float _distance_m; // distance in meters parsed from a term, -1 if no distance uint8_t _checksum; // checksum accumulator bool _term_is_checksum; // current term is the checksum sentence_types _sentence_type; // the sentence type currently being processed + + + float radar_data[8]; + + uint8_t ucCRCHi; + uint8_t ucCRCLo; + int iIndex; + uint8_t sonar_data[25]; + char _term[10]; ///< 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; + float _sonar_deep; + float _sonar_voltage; + uint8_t decode_step; + }; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index dd24cc6cf7..3bb0c0531d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4311,9 +4311,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) case MSG_RANGEFINDER: { CHECK_PAYLOAD_SIZE(RANGEFINDER); - //send_rangefinder(); - Underwater_Sonar sonar = AP::underwater_sonar(); - sonar.send_sonar_mavlink(chan); + send_rangefinder(); + // Underwater_Sonar sonar = AP::underwater_sonar(); + // sonar.send_sonar_mavlink(chan); } break;