/***************************************************************************** * Copyright (c) 2002-2020 MBARI * Monterey Bay Aquarium Research Institute, all rights reserved. ***************************************************************************** * @file Idt83pParser.c * @authors r. henthorn * @date 07/21/2020 * @brief Delta T packet parser utility functions and data definitions * * Project: LRAUV Obstacle Avoidance * Summary: See @brief *****************************************************************************/ /***************************************************************************** * Copyright Information: * Copyright 2002-2020 MBARI * Monterey Bay Aquarium Research Institute, all rights reserved. * * Terms of Use: * This program is free software; you can redistribute it and/or modify it * under the terms of the GNU General Public License as published by the Free * Software Foundation; either version 3 of the License, or (at your option) * any later version. You can access the GPLv3 license at * http://www.gnu.org/licenses/gpl-3.0.html * * This program is distributed in the hope that it will be useful, but WITHOUT * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for * more details (http://www.gnu.org/licenses/gpl-3.0.html). * * MBARI provides the documentation and software code "as is", with no * warranty, express or implied, as to the software, title, non-infringement * of third party rights, merchantability, or fitness for any particular * purpose, the accuracy of the code, or the performance or results which you * may obtain from its use. You assume the entire risk associated with use of * the code, and you agree to be responsible for the entire cost of repair or * servicing of the program with which you are using the code. * * In no event shall MBARI be liable for any damages,whether general, special, * incidental or consequential damages, arising out of your use of the * software, including, but not limited to,the loss or corruption of your data * or damages of any kind resulting from use of the software, any prohibited * use, or your inability to use the software. You agree to defend, indemnify * and hold harmless MBARI and its officers, directors, and employees against * any claim,loss,liability or expense,including attorneys' fees,resulting from * loss of or damage to property or the injury to or death of any person * arising out of the use of the software. * * The MBARI software is provided without obligation on the part of the * Monterey Bay Aquarium Research Institute to assist in its use, correction, * modification, or enhancement. * * MBARI assumes no responsibility or liability for any third party and/or * commercial software required for the database or applications. Licensee * agrees to obtain and maintain valid licenses for any additional third party * software required. *****************************************************************************/ /****************************** * Headers ******************************/ #include #include #include #include #include "Idt83pParser.h" #include "macrologger.h" /****************************** * Macros ******************************/ #define IDT_83P_TYPE "83P" #define IDT_SIZE_TYPE 3 // size of packet type (e.g., '83P') // 2-byte translator #define TO_UINT(upper,lower) ((upper)<<8 | (lower)) /****************************** * Functions ******************************/ static int8_t idtAscii2Month(const char* month); static uint8_t extractPacketType(Idt83pData* p83pData, const char* pIdtPacket); static void createTimestamp(Idt83pData* p83pData, const char* pIdtPacket); static void extractPingtime(Idt83pData* p83pData, const char* pIdtPacket); static void extractPingnumber(Idt83pData* p83pData, const char* pIdtPacket); static void extractAltitude(Idt83pData* p83pData, const char* pIdtPacket); static void extractInterval(Idt83pData* p83pData, const char* pIdtPacket); static void extractNbeams(Idt83pData* p83pData, const char* pIdtPacket); static void extractRanges(Idt83pData* p83pData, const char* pIdtPacket); static void extractIntensities(Idt83pData* p83pData, const char* pIdtPacket); // IDT 83P Packet parser // Parameters: // p83pData - pointer to users Idt83pData object // pIdtPacket - pointer to packet read from 83P source (e.g., beamformer) // packetSize - number of bytes read from 83P source (<=256+4*IDT_MAX_BEAMS) // Returns: // Value > 0 when packet is parsed successfully. // Value <=0 otherwise. See valid codes above int8_t parseIdt83p(Idt83pData* p83pData, const char *pIdtPacket, int16_t packetSize) { // check parameters if (NULL == p83pData) { LOG_ERROR("Idt83Parser::parseIdt83p: NULL parameter"); return IDT_OTHER_ERR; } // Idt83pData exists if (NULL == pIdtPacket) { LOG_ERROR("Idt83Parser::parseIdt83p: NULL parameter"); return (p83pData->valid = IDT_OTHER_ERR); } // Minimum packet size if ((packetSize > 0) && (packetSize < IDT_HDR_SIZE)) { LOG_ERROR("Idt83Parser::parseIdt83p: 83P packet too small %d", packetSize); return (p83pData->valid = IDT_SIZE_ERR); } // ******** check header for packet type if (extractPacketType(p83pData, pIdtPacket) <= 0) return p83pData->valid; // ******** number of beams and intensity indicator extractNbeams(p83pData, pIdtPacket); bool intensities = (p83pData->valid == IDT_83P_INTENSE); if (p83pData->nbeams > IDT_MAX_BEAMS) { LOG_ERROR("Idt83Parser::parseIdt83p: too many beams %d > %d", p83pData->nbeams, IDT_MAX_BEAMS); return (p83pData->valid = IDT_OTHER_ERR); } // calculate expected packet size uint16_t expected = IDT_HDR_SIZE + p83pData->nbeams*(intensities? IDT_B_BYTES+IDT_I_BYTES : IDT_B_BYTES); if (expected != packetSize) { LOG_ERROR("Idt83Parser::parseIdt83p: unexpected packet size %d", packetSize); return IDT_SIZE_ERR; } // Done with error checking, extract remaining info and data // ******** timestamp createTimestamp(p83pData, pIdtPacket); // ******** pingtime extractPingtime(p83pData, pIdtPacket); // ******** ping number extractPingnumber(p83pData, pIdtPacket); // ******** altitude extractAltitude(p83pData, pIdtPacket); // ******** interval extractInterval(p83pData, pIdtPacket); // ******** ranges extractRanges(p83pData, pIdtPacket); // ******** intensities extractIntensities(p83pData, pIdtPacket); return p83pData->valid; } // ascii to month number conversion // return integer range 0 - 11 when 3-letter month passed in, not case-sensitive // returns value < 0 when error occurs // "JAN"=0,"FEB","MAR","APR","MAY","JUN","JUL","AUG","SEP","OCT","NOV","DEC"=11 int8_t idtAscii2Month(const char* month) { const char* months[] = {"jan", "feb", "mar", "apr", "may", "jun", "jul", "aug", "sep", "oct", "nov", "dec"}; int8_t tm_mon; for (tm_mon = 0; tm_mon < IDT_NUM_MONTHS; tm_mon++) if (!strncasecmp(month, months[tm_mon], 3)) break; // match found // return month number or error if (tm_mon < IDT_NUM_MONTHS) return tm_mon; else return -1; } uint8_t extractPacketType(Idt83pData* p83pData, const char* pIdtPacket) { uint8_t ptype = IDT_NOT_83P; if (strncmp(IDT_83P_TYPE, pIdtPacket+IDT_LOC_TYPE, IDT_SIZE_TYPE)) { LOG_INFO("Idt83Parser::extractPacketType: non 83P packet:%c%c%c", pIdtPacket[0], pIdtPacket[1], pIdtPacket[2]); ptype = IDT_NOT_83P; } else if (pIdtPacket[IDT_LOC_INTENSE]) { ptype = IDT_83P_INTENSE; } else { ptype = IDT_83P; } return (p83pData->valid = ptype); } static void createTimestamp(Idt83pData* p83pData, const char* pIdtPacket) { struct timeval tv; gettimeofday(&tv, NULL); p83pData->timestamp = (double)tv.tv_sec + (double(tv.tv_usec)/1000000.); } static void extractPingtime(Idt83pData* p83pData, const char* pIdtPacket) { struct tm pingtm; pingtm.tm_isdst = -1; char month[4]; double msec; // scan date from packet sscanf(pIdtPacket+IDT_LOC_DATE, "%2d-%3s-%4d", &pingtm.tm_mday, month, &pingtm.tm_year); month[3] = '\0'; if ((pingtm.tm_mon = idtAscii2Month(month)) < 0) { LOG_ERROR("parseIdt83p::extractPingtime: malformed month: %s", month); pingtm.tm_mon = 0; } pingtm.tm_year -= 1900; // scan time from packet sscanf(pIdtPacket+IDT_LOC_TIME, "%2d:%2d:%2d", &pingtm.tm_hour, &pingtm.tm_min, &pingtm.tm_sec); sscanf(pIdtPacket+IDT_LOC_MSEC, "%4lf", &msec); // make the epoch seconds from pingtm struct time_t sec = mktime(&pingtm); p83pData->pingtime = (double)sec + msec; } static void extractPingnumber(Idt83pData* p83pData, const char* pIdtPacket) { p83pData->pingnumber = TO_UINT(pIdtPacket[IDT_LOC_PINGNUM],pIdtPacket[IDT_LOC_PINGNUM+1]); } static void extractAltitude(Idt83pData* p83pData, const char* pIdtPacket) { memcpy(&p83pData->altitude, pIdtPacket+IDT_LOC_ALTITUDE, sizeof(float)); } static void extractInterval(Idt83pData* p83pData, const char* pIdtPacket) { p83pData->interval = TO_UINT(pIdtPacket[IDT_LOC_INTERVAL],pIdtPacket[IDT_LOC_INTERVAL+1]); } static void extractNbeams(Idt83pData* p83pData, const char* pIdtPacket) { p83pData->nbeams = TO_UINT(pIdtPacket[IDT_LOC_NBEAMS],pIdtPacket[IDT_LOC_NBEAMS+1]); } static void extractRanges(Idt83pData* p83pData, const char* pIdtPacket) { // initialize all the data memset(p83pData->range, 0, IDT_MAX_BEAMS*sizeof(float)); uint16_t resolution_mm = TO_UINT(pIdtPacket[IDT_LOC_RESOLUTION],pIdtPacket[IDT_LOC_RESOLUTION+1]); uint16_t rangeOffset = IDT_HDR_SIZE; for (uint16_t b = 0; b < p83pData->nbeams && b < IDT_MAX_BEAMS; b++) { uint16_t thisRange = rangeOffset + (b * IDT_B_BYTES); p83pData->range[b] = ((float)resolution_mm/1000.0) * TO_UINT(pIdtPacket[thisRange], pIdtPacket[thisRange+1]); } } static void extractIntensities(Idt83pData* p83pData, const char* pIdtPacket) { // initialize all the data memset(p83pData->intense, 0, IDT_MAX_BEAMS*sizeof(uint16_t)); // Are intensity values included in the packet? uint16_t intensities = pIdtPacket[IDT_LOC_INTENSITY]; if (intensities) { uint16_t intenseOffset = IDT_HDR_SIZE + (p83pData->nbeams * IDT_B_BYTES); for (uint16_t b = 0; b < p83pData->nbeams && b < IDT_MAX_BEAMS; b++) { uint16_t thisIntense = intenseOffset + (b * IDT_I_BYTES); p83pData->intense[b] = TO_UINT(pIdtPacket[thisIntense], pIdtPacket[thisIntense+1]); } } }