#include #include #include #include #include #include #include #include "Telem.h" #include "SerialLog.h" #include "packetTypes.h" #include "net_wmm.h" #include "../mavlink/c_library/common/mavlink.h" #include "SLIP.h" #include "util.h" #include "link_packet.h" Telem::Telem(string ipaddr, int port, unsigned log_gap_us, unsigned log_delay_max, string &log_delay_filename) : PacketHandler(ipaddr, port, 0xe0, PKT_ID_MAVLINK), _log_gap_us(log_gap_us), _log_delay_max(log_delay_max), _log_delay_filename(log_delay_filename), _fp_delay(NULL), _delay_cnt(0) { pkt_delay_open(); } /*********************************************************************** Method: Telem data handler Description: Transmits telem data from UDP to STM32 via serial ***********************************************************************/ void Telem::downHandler(int ser_fd, int verbosity) { LinkPacket packet; socklen_t addrlen = sizeof(_sock); int recvlen; char msg[1024]; int encodedLen; SLIPEncoder *slipEnc = new SLIPEncoder(msg, sizeof(msg)); static uint64_t last_stm_recv_us = 0; // Attempt to receive data. This should be good since we got here from // a select() recvlen = recvfrom(_sock_fd, &packet, sizeof(packet), 0, (struct sockaddr *)&_sock, &addrlen); packet.stm_recv_us = clock_gettime_us(CLOCK_MONOTONIC); // Log any gaps in telemetry >= _log_gap_us if (last_stm_recv_us != 0) { unsigned gap_us = packet.stm_recv_us - last_stm_recv_us; if (gap_us >= _log_gap_us) syslog(LOG_INFO, "telem gap %u msec", (gap_us + 500) / 1000); } last_stm_recv_us = packet.stm_recv_us; pkt_delay_write(packet); #ifdef INCLUDE_SERIAL_LOG extern SerialLog *serialLog; #endif char *b; uint8_t *p_payload = packet.payload; while (p_payload < ((uint8_t *)&packet + recvlen)) { /* Make sure this is a mavlink packet */ if (p_payload[0] != 0xFE) { syslog(LOG_ERR, "pkt: Got a bad mavlink packet in the LinkPacket"); break; } // Since this should always be mavlink, make sure its not a dataflash log type // Check index 5 for the message type if (p_payload[5] == MAVLINK_MSG_ID_LOG_DATA || p_payload[5] == MAVLINK_MSG_ID_LOG_DATA_LEN || p_payload[5] == MAVLINK_MSG_ID_LOG_DATA_CRC) goto move_on; /* Use the existing packet, even though we'll lose some data. * Stick the id type before the start of the mavlink message.*/ b = (char *)(p_payload - 1); b[0] = _pktID; encodedLen = slipEnc->encode(b, (p_payload[1] + 8) + 1); #ifdef INCLUDE_SERIAL_LOG // Note that dataflash downlink is not logged, // since it is not sent to the STM32 serialLog->log_packet(b, encodedLen, SerialLog::PKTFLG_DOWN); #endif // INCLUDE_SERIAL_LOG if (write(ser_fd, msg, encodedLen) != encodedLen) syslog(LOG_ERR, "pkt: error writing to serial port"); move_on: p_payload += p_payload[1] + 8; } } void Telem::pkt_delay_open(void) { // packet time delay log enabled by setting _log_delay_filename != "" if (_log_delay_filename != "") { // If the file exists, this truncates it and starts over ("w", not "a"). // The edge case to avoid is if the rename below fails for whatever // reason, this file does not grow forever (eating RAM). _fp_delay = fopen(_log_delay_filename.c_str(), "w"); if (_fp_delay != NULL) { char time_buf[40]; fprintf(_fp_delay, "\n"); fprintf(_fp_delay, "starting: %s\n", clock_gettime_str_r(CLOCK_REALTIME, time_buf)); fprintf(_fp_delay, "seq,read_len,blocked,tf_recv,tf_send,tc_recv,tc_send,stm_recv\n"); fflush(_fp_delay); } } } // Telem::pkt_delay_open void Telem::pkt_delay_write(LinkPacket &packet) { if (_fp_delay == NULL) return; fprintf(_fp_delay, "%u,%u,%u,%llu,%llu,%llu,%llu,%llu\n", packet.seq, packet.data1, packet.data2, packet.tf_recv_us, packet.tf_send_us, packet.tc_recv_us, packet.tc_send_us, packet.stm_recv_us); _delay_cnt++; if (_delay_cnt < _log_delay_max) { fflush(_fp_delay); return; } // move file.csv to file.csv.1 (losing the old .1 if it exists), // and open a new file.csv fclose(_fp_delay); _fp_delay = NULL; _delay_cnt = 0; string fn_new = _log_delay_filename + ".1"; // if the rename fails, we lose the .1 file and start over with the new one (void)rename(_log_delay_filename.c_str(), fn_new.c_str()); pkt_delay_open(); } // Telem::pkt_delay_write