CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/Replay/DataFlashFileReader.cpp
Views: 1798
1
#include "DataFlashFileReader.h"

2
#include <AP_Filesystem/AP_Filesystem.h>

3


4
#include <fcntl.h>

5
#include <string.h>

6
#include <sys/types.h>

7
#include <stdio.h>

8
#include <unistd.h>

9
#include <time.h>

10
#include <cinttypes>

11


12
#ifndef PRIu64

13
#define PRIu64 "llu"

14
#endif

15


16
AP_LoggerFileReader::AP_LoggerFileReader()

17
{}

18


19
AP_LoggerFileReader::~AP_LoggerFileReader()

20
{

21
    ::printf("Replay counts: %" PRIu64 " bytes  %u entries\n", bytes_read, message_count);

22
}

23


24
bool AP_LoggerFileReader::open_log(const char *logfile)

25
{

26
    fd = AP::FS().open(logfile, O_RDONLY);

27
    if (fd == -1) {

28
        return false;

29
    }

30
    return true;

31
}

32


33
ssize_t AP_LoggerFileReader::read_input(void *buffer, const size_t count)

34
{

35
    uint64_t ret = AP::FS().read(fd, buffer, count);

36
    bytes_read += ret;

37
    return ret;

38
}

39


40
void AP_LoggerFileReader::format_type(uint16_t type, char dest[5])

41
{

42
    const struct log_Format &f = formats[type];

43
    memset(dest,0,5);

44
    if (f.length == 0) {

45
        return;

46
    }

47
    strncpy(dest, f.name, 4);

48
}

49
void AP_LoggerFileReader::get_packet_counts(uint64_t dest[])

50
{

51
    memcpy(dest, packet_counts, sizeof(packet_counts));

52
}

53


54
bool AP_LoggerFileReader::update()

55
{

56
    uint8_t hdr[3];

57
    if (read_input(hdr, 3) != 3) {

58
        return false;

59
    }

60
    if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {

61
        printf("bad log header\n");

62
        return false;

63
    }

64


65
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS

66
    // running on stm32 is slow enough it is nice to see progress

67
    if (message_count % 500 == 0) {

68
        ::printf("line %u pkt 0x%02x t=%u\n", message_count, hdr[2], AP_HAL::millis());

69
    }

70
#endif

71
    packet_counts[hdr[2]]++;

72


73
    if (hdr[2] == LOG_FORMAT_MSG) {

74
        struct log_Format f;

75
        memcpy(&f, hdr, 3);

76
        if (read_input(&f.type, sizeof(f)-3) != sizeof(f)-3) {

77
            return false;

78
        }

79
        memcpy(&formats[f.type], &f, sizeof(formats[f.type]));

80


81
        message_count++;

82
        return handle_log_format_msg(f);

83
    }

84


85
    const struct log_Format &f = formats[hdr[2]];

86
    if (f.length == 0) {

87
        // can't just throw these away as the format specifies the

88
        // number of bytes in the message

89
        ::printf("No format defined for type (%d)\n", hdr[2]);

90
        exit(1);

91
    }

92


93
    uint8_t msg[f.length];

94


95
    memcpy(msg, hdr, 3);

96
    if (read_input(&msg[3], f.length-3) != f.length-3) {

97
        return false;

98
    }

99


100
    message_count++;

101
    return handle_msg(f, msg);

102
}

103


104