2. 程式人生 > >Ardupilot飛控log程式碼學習（3.6版本飛控韌體）

Ardupilot飛控log程式碼學習（3.6版本飛控韌體）

阿新 • • 發佈：2018-11-11

目錄

文章目錄

摘要

本節主要分析ardupilot3.6版本的log韌體程式碼，歡迎批評指正！！！

1.官網資料學習

新增一個新的日誌訊息（Adding a new Log Message）

DataFlash日誌儲存在飛行控制器的DataFlash記憶體中，可以在飛行後下載。這些日誌提供了關於每次飛行的詳細資訊，這可能非常重要，尤其是在試圖診斷出故障原因時

這個頁面解釋瞭如何編寫一個新的dataflash日誌訊息。

簡單的方法（The Easy Way）

用：ardupilot/libraries/DataFlash/DataFlash.h 的DataFlash_Class:instance()->Log_Write函式寫訊息

void Log_Write(const char *name, const char *labels, const char *fmt, ...);
void Log_Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

在Compass_learn.cpp中可以找到正在使用的頂層函式的一個例子：

    if (sample_available) 
    {
        DataFlash_Class::instance()->Log_Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
                                               AP_HAL::micros64(),
                                               (double)best_offsets.x,
                                               (double)best_offsets.y,
                                               (double)best_offsets.z,
                                               (double)best_error,
                                               (double)best_yaw_deg,
                                               (double)worst_error,
                                               num_samples);
    }
  • 第一個引數是訊息名稱。這應該是4個字元或更少，並且是唯一的。
  • 第二個引數是一個逗號分隔的欄位名列表
  • 第三個引數是一個格式字串，每個字母表示對應欄位的格式。每個字母的含義可以在ardupilot/libraries/DataFlash/LogStructure.h中找到。(意思就是格式程式碼的簡稱，比如整形16位，就用a來表示。)

我們分析下："QffffffI"是什麼意思？Q:uint64_t,可以看出AP_HAL::micros64()就是這種型別：uint64_t micros64()，因此Q表示一種型別的簡寫；f：float表示浮點型別，(double)best_offsets.x,(double)best_offsets.y, (double)best_offsets.z,(double)best_error, (double)best_yaw_deg,
(double)worst_error,num_samples)本來都是浮點型別，到這裡知道什麼意思了吧。

/*
二進位制日誌訊息格式字串中的格式字元:Format characters in the format string for binary log messages
  a   : int16_t[32]
  b   : int8_t
  B   : uint8_t
  h   : int16_t
  H   : uint16_t
  i   : int32_t
  I   : uint32_t
  f   : float
  d   : double
  n   : char[4]
  N   : char[16]
  Z   : char[64]
  c   : int16_t * 100
  C   : uint16_t * 100
  e   : int32_t * 100
  E   : uint32_t * 100
  L   : int32_t latitude/longitude
  M   : uint8_t flight mode
  q   : int64_t
  Q   : uint64_t
 */
  • 剩下的引數是要記錄的實際值。您可能注意到，在上面的示例中，有些欄位的格式是float (f)，但轉換為(double)，這是正確的，並且是避免編譯器警告所必需的
                                               AP_HAL::micros64(),      //第二個引數是一個逗號分隔的欄位名列表
                                               (double)best_offsets.x,  //第三個引數是一個格式字串，每個字母表示對應欄位的格式。
                                               (double)best_offsets.y,
                                               (double)best_offsets.z,
                                               (double)best_error,
                                               (double)best_yaw_deg,
                                               (double)worst_error,
                                               num_samples);

第二個Log_Write()函式與第一個函式相同，只是它接受另外兩個字串引數“units”和“mults”。 與“format”引數類似，這些引數中的每個字元指定以下欄位的單位（即“d”代表度數）或乘數（即“2”代表* 100，“B”代表* 0.01）。 這些幫助圖形工具在向用戶顯示時正確縮放輸出

例如，下面是一個“測試”日誌訊息，它輸出當前的系統時間和高度。

DataFlash_Class::instance()->Log_Write("TEST", "TimeUS,Alt",
                                       "sm", // units: seconds, meters,這個是單位s和米
                                       "FB", // mult: 1e-6, 1e-2，表示的數量級
                                       "Qf", // format: uint64_t, float ，資料型別，和上面的講解一樣
                                       AP_HAL::micros64(), //顯示的時間單位
                                       (double)alt_in_cm);  //顯示的高度資訊
  • 時間以秒為單位（“s”），乘數為“F”表示該值應除以1百萬，格式為“Q”表示它輸出為64位無符號整數。
  • 海拔高度單位為米（“m”），乘數為“B”表示該值應除以100（從釐米轉換為米）和格式“f”，因為該值為浮點數。

上面的要是不明白，可以具體看下面的表示

//這裡所有的單元都應該是基本單元。這意味著電池容量在這裡是“amp*.”。請保持名稱與Tools/autotest/param_metadata/param.py:33一致。

const struct UnitStructure log_Units[] = {
    { '-', "" },              // no units e.g. Pi, or a string
    { '?', "UNKNOWN" },       // Units which haven't been worked out yet....
    { 'A', "A" },             // Ampere
    { 'd', "deg" },           // of the angular variety, -180 to 180
    { 'b', "B" },             // bytes
    { 'k', "deg/s" },         // degrees per second. Degrees are NOT SI, but is some situations more user-friendly than radians
    { 'D', "deglatitude" },   // degrees of latitude
    { 'e', "deg/s/s" },       // degrees per second per second. Degrees are NOT SI, but is some situations more user-friendly than radians
    { 'E', "rad/s" },         // radians per second
    { 'G', "Gauss" },         // Gauss is not an SI unit, but 1 tesla = 10000 gauss so a simple replacement is not possible here
    { 'h', "degheading" },    // 0.? to 359.?
    { 'i', "A.s" },           // Ampere second
    { 'J', "W.s" },           // Joule (Watt second)
    // { 'l', "l" },          // litres
    { 'L', "rad/s/s" },       // radians per second per second
    { 'm', "m" },             // metres
    { 'n', "m/s" },           // metres per second
    // { 'N', "N" },          // Newton
    { 'o', "m/s/s" },         // metres per second per second
    { 'O', "degC" },          // degrees Celsius. Not SI, but Kelvin is too cumbersome for most users
    { '%', "%" },             // percent
    { 'S', "satellites" },    // number of satellites
    { 's', "s" },             // seconds
    { 'q', "rpm" },           // rounds per minute. Not SI, but sometimes more intuitive than Hertz
    { 'r', "rad" },           // radians
    { 'U', "deglongitude" },  // degrees of longitude
    { 'u', "ppm" },           // pulses per minute
    { 'U', "us" },            // pulse width modulation in microseconds
    { 'v', "V" },             // Volt
    { 'P', "Pa" },            // Pascal
    { 'w', "Ohm" },           // Ohm
    { 'z', "Hz" }             // Hertz
};

// this multiplier information applies to the raw value present in the
// log.  Any adjustment implied by the format field (e.g. the "centi"
// in "centidegrees" is *IGNORED* for the purposes of scaling.
// Essentially "format" simply tells you the C-type, and format-type h
// (int16_t) is equivalent to format-type c (int16_t*100)
// tl;dr a GCS shouldn't/mustn't infer any scaling from the unit name

const struct MultiplierStructure log_Multipliers[] = {
    { '-', 0 },       // no multiplier e.g. a string
    { '?', 1 },       // multipliers which haven't been worked out yet....
// <leave a gap here, just in case....>
    { '2', 1e2 },
    { '1', 1e1 },
    { '0', 1e0 },
    { 'A', 1e-1 },
    { 'B', 1e-2 },
    { 'C', 1e-3 },
    { 'D', 1e-4 },
    { 'E', 1e-5 },
    { 'F', 1e-6 },
    { 'G', 1e-7 },
// <leave a gap here, just in case....>
    { '!', 3.6 }, // (ampere*second => milliampere*hour) and (km/h => m/s)
    { '/', 3600 }, // (ampere*second => ampere*hour)
};

複雜的方法（The Harder Way）

對於通常使用的訊息，特別是以相對高的速率(50hz或更高)輸出的訊息，可以使用稍微更高效的日誌記錄方法。。


    struct PACKED log_Test {
    LOG_PACKET_HEADER;
    uint64_t time_us;   //時間
    float a_value;    //測量值
}


//下面這個是自己重新拷貝的，跟上面的一樣
struct PACKED log_Control_Tuning {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    float    throttle_in;
    float    angle_boost;
    float    throttle_out;
    float    throttle_hover;
    float    desired_alt;
    float    inav_alt;
    int32_t  baro_alt;
    float    desired_rangefinder_alt;
    int16_t  rangefinder_alt;
    float    terr_alt;
    int16_t  target_climb_rate;
    int16_t  climb_rate;
};
//寫一個控制曲線包----- Write a control tuning packet
void Copter::Log_Write_Control_Tuning()
{
    // get terrain altitude
    float terr_alt = 0.0f;
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
    if (!terrain.height_above_terrain(terr_alt, true)) {
        terr_alt = DataFlash.quiet_nan();
    }
#endif
 
    float _target_rangefinder_alt;
    if (target_rangefinder_alt_used) {
        _target_rangefinder_alt = target_rangefinder_alt * 0.01f; // cm->m
    } else {
        _target_rangefinder_alt = DataFlash.quiet_nan();
    }
    struct log_Control_Tuning pkt = {
        LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
        time_us             : AP_HAL::micros64(),
        throttle_in         : attitude_control->get_throttle_in(),
        angle_boost         : attitude_control->angle_boost(),
        throttle_out        : motors->get_throttle(),
        throttle_hover      : motors->get_throttle_hover(),
        desired_alt         : pos_control->get_alt_target() / 100.0f,
        inav_alt            : inertial_nav.get_altitude() / 100.0f,
        baro_alt            : baro_alt,
        desired_rangefinder_alt : _target_rangefinder_alt,
        rangefinder_alt     : rangefinder_state.alt_cm,
        terr_alt            : terr_alt,
        target_climb_rate   : (int16_t)pos_control->get_vel_target_z(),
        climb_rate          : climb_rate
    };
    DataFlash.WriteBlock(&pkt, sizeof(pkt));
}

void Copter::Log_Write_Control_Tuning() {}
void Copter::Log_Write_Performance() {}
void Copter::Log_Write_Attitude(void) {}
void Copter::Log_Write_EKF_POS() {}
void Copter::Log_Write_MotBatt() {}
void Copter::Log_Write_Event(uint8_t id) {}
void Copter::Log_Write_Data(uint8_t id, int32_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint32_t value) {}
void Copter::Log_Write_Data(uint8_t id, int16_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint16_t value) {}
void Copter::Log_Write_Data(uint8_t id, float value) {}
void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
void Copter::Log_Sensor_Health() {}
void Copter::Log_Write_Precland() {}
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
void Copter::Log_Write_Vehicle_Startup_Messages() {}
//新增自己的測試程式碼
 void Copter::Log_Write_Test()
 //新增自己的測試程式碼
  • 向模型程式碼或DataFlash庫新增一個名為Log_Write_< something-or-other>的新方法，它填充結構，然後呼叫DataFlash/WriteBlock()
   void Copter::Log_Write_Test()
{
    struct log_Test pkt = 
    {
        LOG_PACKET_HEADER_INIT(LOG_TEST_MSG),
        time_us  : AP_HAL::micros64(),
        a_value  : 1234
    };
    DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
  • 在您希望記錄值時，從排程器或程式碼中的其他地方呼叫此新函式。

2.ardupilot的資料快閃記憶體（The DataFlash library）

**ardupilot的資料快閃記憶體日誌記錄在Mincard上，最後儲存格式為.bin檔案

自動駕駛儀需要的另一種儲存方式就是飛行日誌儲存，ardupilot基於DataFlash library庫來實現。這個庫的名字看上去有些怪怪的，實際上這個庫最開始是為APM1的DataFlash晶片設計的，它原本是一個硬體驅動庫，後來慢慢演變為一個通用日誌系統，這個可以在程式碼中找到蛛絲馬跡（這些都是以前的痕跡，不是最好的程式碼實現方式）。

現在，DataFlash API主要用於實現日誌儲存模型設計。它允許您自定義日誌訊息的資料結構。例如GPS訊息，用於記錄GPS感測器的日誌資料。它能夠非常有效儲存這些資料，它同時也對其他庫提供相應的APIs，用來進行日誌回傳、下載。

如果您最近下載日誌時已經在ArduPilot看到了“*.bin”檔案，那麼您會看到ArduPilot用於儲存日誌訊息的格式。這個時“自我描述”，意思是地面站可以計算出日誌檔案中訊息的格式，而不必有一些共同的方案。在每個日誌檔案的前端是一組FMT訊息，它們具有眾所周知的格式，並且描述按照訊息的格式。

去看看庫/DATAFLIS/Stask/DATAFLASHYTest/DATAFLASHASTest.CPP。您將在頂部看到一個小表，它定義了我們將要編寫的日誌訊息，在本例中是一個“TEST”訊息，其中包含4個無符號16位整數和兩個帶符號32位整數（這就是“HHHHii”的意思）。它還給出了這6個變數的名稱（巧妙標記的V1到V4和L1和L2）。
在loop()函式中，你會看到這樣一個相當奇怪的呼叫：

DataFlash是用來儲存日誌的。日誌有固定的格式，固定的日誌頭和日誌包頭

DataFlash.get_log_boundaries(log_num, start, end);

DataFlash API隱藏了底層如何儲存log檔案的細節。另外，對於Pixhawk or Linux這樣的支援 Posix IO的系統，日誌檔案是儲存在microSD卡的“LOGS”目錄中的。使用者可以直接抽出SD卡，直接拷貝到電腦中。

在像APM2這樣的板上並不是那麼簡單。APM2在資料流晶片上有4M位元組的儲存，可以通過SPI介面訪問。介面本身是面向頁面的，因此需要填充一個512位元組（或者可能是528位元組！）頁，然後告訴晶片將該頁複製到持久儲存，同時填充下一頁。在這個DataFlash上執行隨機IO是不好的——它是為需要連續寫入的程式碼設計的，這在日誌記錄時發生。與自動駕駛儀喜歡記錄的資料量相比，4M位元組的大小實際上不是很大，因此當資料填充時，我們還需要處理包裝。

所有這些複雜性都隱藏在一個API後面，該API提出了“日誌編號”的概念，這只是一組在自動駕駛儀的一次飛行中寫入的位元組。APM1和APM2上的DataFlash實現在每個頁面的前端使用少量標記位元組來表示正在寫入哪個日誌編號。這些日誌號對應於當用戶要求檢索其日誌時下載的日誌號。

3.ardupilot的日誌格式

DataFlash是用來儲存日誌的。日誌有固定的格式，固定的日誌頭和日誌包頭

1.日誌頭格式

// 用於定義日誌格式的結構----structure used to define logging format
struct LogStructure {
    uint8_t msg_type;
    uint8_t msg_len;
    const char name[5];
    const char format[16];
    const char labels[64];
};
//所有車輛型別常見的日誌結構-----log structures common to all vehicle types
struct PACKED log_Format 
{
    LOG_PACKET_HEADER;
    uint8_t type;
    uint8_t length;
    char name[4];
    char format[16];
    char labels[64];
};

2.日誌包頭

#define LOG_PACKET_HEADER	       uint8_t head1, head2, msgid; //日誌包頭
#define LOG_PACKET_HEADER_INIT(id) head1 : HEAD_BYTE1, head2 : HEAD_BYTE2, msgid : id
#define LOG_PACKET_HEADER_LEN 3 // bytes required for LOG_PACKET_HEADER

// once the logging code is all converted we will remove these from
// this header
#define HEAD_BYTE1  0xA3    // Decimal 163
#define HEAD_BYTE2  0x95    // Decimal 149

3.日誌舉例

libraries/DataFlash/examples/DataFlash_test/DataFlash_test.cpp

struct PACKED log_Test {
    LOG_PACKET_HEADER;
    uint16_t v1, v2, v3, v4;
    int32_t  l1, l2;
};
static const struct LogStructure log_structure[] = {
    LOG_COMMON_STRUCTURES,
    { LOG_TEST_MSG, sizeof(log_Test),       
    "TEST", "HHHHii",        "V1,V2,V3,V4,L1,L2" }
};

日誌是以檔案的形式儲存到microSD card，可以直接拔出SD卡拷貝到PC
每個頁首都有“日誌檔案的編號”和“日誌檔案的頁號”。當用戶下載日誌時，非常有用

void DataFlashTest::setup(void)
{
    dataflash.Init(log_structure, ARRAY_SIZE(log_structure));
    dataflash.set_vehicle_armed(true);

    hal.console->printf("Dataflash Log Test 1.0\n");

    // Test
    hal.scheduler->delay(20);
    dataflash.ShowDeviceInfo(hal.console);

    if (dataflash.NeedPrep()) {
        hal.console->printf("Preparing dataflash...\n");
        dataflash.Prep();
    }

    // We start to write some info (sequentialy) starting from page 1
    // This is similar to what we will do...
    dataflash.StartUnstartedLogging();
    log_num = dataflash.find_last_log();
    hal.console->printf("Using log number %u\n", log_num);
    hal.console->printf("After testing perform erase before using DataFlash for logging!\n");
    hal.console->printf("\n");
    hal.console->printf("Writing to flash... wait...\n");

    uint32_t total_micros = 0;
    uint16_t i;

    for (i = 0; i < NUM_PACKETS; i++) {
        uint32_t start = AP_HAL::micros();
        // note that we use g++ style initialisers to make larger
        // structures easier to follow        
        struct log_Test pkt = {
            LOG_PACKET_HEADER_INIT(LOG_TEST_MSG),
            v1    : (uint16_t)(2000 + i),
            v2    : (uint16_t)(2001 + i),
            v3    : (uint16_t)(2002 + i),
            v4    : (uint16_t)(2003 + i),
            l1    : (int32_t)(i * 5000),
            l2    : (int32_t)(i * 16268)
        };
        dataflash.WriteBlock(&pkt, sizeof(pkt));
        total_micros += AP_HAL::micros() - start;
        hal.scheduler->delay(20);
    }

    hal.console->printf("Average write time %.1f usec/byte\n", 
                       (double)total_micros/((double)i*sizeof(struct log_Test)));

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
    dataflash.flush();
#endif

    hal.scheduler->delay(100);
}

void DataFlashTest::loop(void)
{
    uint16_t start, end;

    hal.console->printf("Start read of log %u\n", log_num);

    dataflash.get_log_boundaries(log_num, start, end); 
    dataflash.LogReadProcess(log_num, start, end, 
                             FUNCTOR_BIND_MEMBER(&DataFlashTest::print_mode, void, AP_HAL::BetterStream *, uint8_t),//print_mode,
                             hal.console);
    hal.console->printf("\nTest complete.  Test will repeat in 20 seconds\n");
    hal.scheduler->delay(20000);
}

4.日誌儲存相關的函式

使用者需要設定page大小512 byte，當寫滿一頁以後告訴晶片複製一頁
日誌本身是通過DataFlash_File.cpp寫到SD卡，還提供了DataFlash_Empty.cpp的塊裝置讀寫介面，下面是應用鐵電的讀寫例項

緩衝區buffer

static uint8_t buffer[2][DF_PAGE_SIZE]; 

void DataFlash_Flash::Init(const struct LogStructure *structure, uint8_t num_types)
{
    DataFlash_Backend::Init(structure, num_types);
    if (flash_fd == 0) {
        flash_fd = open(MTD_LOG_FILE, O_RDWR, 0777);
        if (flash_fd == -1) {
            printf("DataFlash_Flash init failed\n");
        }
    }

    df_PageSize = DF_PAGE_SIZE; //頁大小
    df_NumPages = DF_NUM_PAGES - 1; //頁數量
}

讀取flash資料到buffer

void DataFlash_Flash::PageToBuffer(unsigned char BufferNum, uint16_t PageAdr)
{
    PageAdr -= 1;

    uint16_t ofs = PageAdr * DF_PAGE_SIZE;

    memset(buffer[BufferNum], 0, DF_PAGE_SIZE);

    if (lseek(flash_fd, ofs, SEEK_SET) != ofs) {
        printf("PageToBuffer lseek err.\n");
        return;
    }
    if (read(flash_fd, buffer[BufferNum], DF_PAGE_SIZE) != DF_PAGE_SIZE)
    {
        printf("PageToBuffer read err.\n");
        return;
    }
}

記錄需要寫入flash的頁地址和緩衝區編號

void DataFlash_Flash::BufferToPage (unsigned char BufferNum, uint16_t PageAdr, unsigned char wait)
{
PageAdr -= 1;
uint16_t ofs = PageAdr * DF_PAGE_SIZE;

if(flash_fd < 0) return;

if (lseek(flash_fd, ofs, SEEK_SET) != ofs) {
    printf("BufferToPage lseek err.\n");
    return;
}

if (::write(flash_fd, &buffer[BufferNum], DF_PAGE_SIZE) != DF_PAGE_SIZE)
{
    printf("BufferToPage write err.\n");
    return;
}

}

**資料寫入緩衝區**

void DataFlash_Flash::BlockWrite(uint8_t BufferNum, uint16_t IntPageAdr,
const void *pHeader, uint8_t hdr_size,
const void *pBuffer, uint16_t size)
{
if (!_writes_enabled) {
return;
}

memset(&buffer[BufferNum][IntPageAdr], 0, size+hdr_size);

if (hdr_size) {
    memcpy(&buffer[BufferNum][IntPageAdr],
           pHeader,
           hdr_size);
}

memcpy(&buffer[BufferNum][IntPageAdr+hdr_size],
       pBuffer,
       size);

}

**從緩衝區讀取資料**

bool DataFlash_Flash::BlockRead(uint8_t BufferNum, uint16_t IntPageAdr, void *pBuffer, uint16_t size)
{
memset(pBuffer, 0, size);
memcpy(pBuffer, &buffer[BufferNum][IntPageAdr], size);

return true;

}

4.ardupilot的日誌學習

1.初始化過程

void Copter::setup()
{
    //從引數表中載入預設引數----------Load the default values of variables listed in var_info[]s
    AP_Param::setup_sketch_defaults();

    //初始化儲存的多旋翼佈局-----------setup storage layout for copter
    StorageManager::set_layout_copter();
    //感測器初始化，註冊
    init_ardupilot();

    //初始化整個主loop任務排程-------initialise the main loop scheduler
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}

需要注意的地方：
用於定義日誌格式的結構

struct LogStructure {
    uint8_t msg_type; //訊息型別
    uint8_t msg_len; //訊息長度
    const char *name; //訊息名字
    const char *format; //資料格式型別
    const char *labels; //名字標籤
    const char *units; //單位
    const char *multipliers; //數量級
};

const struct LogStructure Copter::log_structure[] =
{
    LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
    { LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
      "ATUN", "QBBfffffff",       "TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt", "s--ddd---o", "F--BBB---0" },
    { LOG_AUTOTUNEDETAILS_MSG, sizeof(log_AutoTuneDetails),
      "ATDE", "Qff",          "TimeUS,Angle,Rate", "sdk", "FBB" },
#endif
    { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
      "PTUN", "QBfHHH",          "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" },



#if OPTFLOW == ENABLED
    { LOG_OPTFLOW_MSG, sizeof(log_Optflow),
      "OF",   "QBffff",   "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" },
#endif
    { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
      "CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" },

	 //型別，長度，名字，格式，標籤，單位，乘法
    { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
      "MOTB", "Qffff",  "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
    { LOG_EVENT_MSG, sizeof(log_Event),         
      "EV",   "QB",           "TimeUS,Id", "s-", "F-" },
    { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),         
      "D16",   "QBh",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),         
      "DU16",  "QBH",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t),         
      "D32",   "QBi",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t),         
      "DU32",  "QBI",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),         
      "DFLT",  "QBf",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_ERROR_MSG, sizeof(log_Error),         
      "ERR",   "QBB",         "TimeUS,Subsys,ECode", "s--", "F--" },
#if FRAME_CONFIG == HELI_FRAME
    { LOG_HELI_MSG, sizeof(log_Heli),
      "HELI",  "Qff",         "TimeUS,DRRPM,ERRPM", "s--", "F--" },
#endif
#if PRECISION_LANDING == ENABLED
    { LOG_PRECLAND_MSG, sizeof(log_Precland),
      "PL",    "QBBfffffffIIB",    "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasUS,EKFOutl,Est", "s--ddmmddms--","F--00BB00BC--" },
#endif
    { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
      "GUID",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
	  { LOG_CONTROL_TUNING1_MSG, sizeof(log_Control_Tuning1),
	 	      "CTUN1", "Qffffffefcfhh", "TimeUS1,ThI1,ABst1,ThO1,ThH1,DAlt1,Alt1,BAlt1,DSAlt1,SAlt1,TAlt1,DCRt1,CRt1", "s----mmmmmmnn", "F----00B0BBBA" },
};


void Copter::init_ardupilot()
{
        .....................
  #if LOGGING_ENABLED == ENABLED
    log_init();  //快閃記憶體日誌初始化
  #endif
    .....................
}



void Copter::log_init(void)
{
    DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));  //初始化Dataflash
}

void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_types)
{
    gcs().send_text(MAV_SEVERITY_INFO, "Preparing log system");
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    validate_structures(structures, num_types);
    dump_structures(structures, num_types);
#endif
    if (_next_backend == DATAFLASH_MAX_BACKENDS) 
    {
        AP_HAL::panic("Too many backends");
        return;
    }
    _num_types = num_types;
    _structures = structures;

#if defined(HAL_BOARD_LOG_DIRECTORY)
 #if HAL_OS_POSIX_IO || HAL_OS_FATFS_IO
    if (_params.backend_types == DATAFLASH_BACKEND_FILE ||
        _params.backend_types == DATAFLASH_BACKEND_BOTH) {
        DFMessageWriter_DFLogStart *message_writer =
            new DFMessageWriter_DFLogStart();
        if (message_writer != nullptr)  {
            backends[_next_backend] = new DataFlash_File(*this,
                                                         message_writer,
                                                         HAL_BOARD_LOG_DIRECTORY);
        }
        if (backends[_next_backend] == nullptr) {
            hal.console->printf("Unable to open DataFlash_File");
        } else {
            _next_backend++;
        }
    }
 #elif CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT 

    if (_params.backend_types == DATAFLASH_BACKEND_FILE ||
        _params.backend_types == DATAFLASH_BACKEND_BOTH) {

        DFMessageWriter_DFLogStart *message_writer =
            new DFMessageWriter_DFLogStart();
        if (message_writer != nullptr)  {

  #if defined(BOARD_SDCARD_NAME) || defined(BOARD_DATAFLASH_FATFS)
            backends[_next_backend] = new DataFlash_File(*this, message_writer, HAL_BOARD_LOG_DIRECTORY);
  #else
            backends[_next_backend] = new DataFlash_Revo(*this, message_writer); // restore dataflash logs
  #endif
        }

        if (backends[_next_backend] == nullptr) {
            printf("Unable to open DataFlash_Revo");
        } else {
            _next_backend++;
        }
    }
 #endif
#endif // HAL_BOARD_LOG_DIRECTORY

#if DATAFLASH_MAVLINK_SUPPORT
    if (_params.backend_types == DATAFLASH_BACKEND_MAVLINK ||
        _params.backend_types == DATAFLASH_BACKEND_BOTH) {
        if (_next_backend == DATAFLASH_MAX_BACKENDS) {
            AP_HAL::panic("Too many backends");
            return;
        }
        DFMessageWriter_DFLogStart *message_writer =
            new DFMessageWriter_DFLogStart();
        if (message_writer != nullptr)  {
            backends[_next_backend] = new DataFlash_MAVLink(*this,
                                                            message_writer);
        }
        if (backends[_next_backend] == nullptr) {
            hal.console->printf("Unable to open DataFlash_MAVLink");
        } else {
            _next_backend++;
        }
    }
#endif

    for (uint8_t i=0; i<_next_backend; i++) 
    {
        backends[i]->Init()；//後端初始化
      }

    Prep();

    EnableWrites(true);

    gcs().send_text(MAV_SEVERITY_INFO, "Prepared log system");
}

2.更新過程

由於ardupilot預設記錄log到SD卡，需要解鎖才能進行，如果想不解鎖就進行記錄，需要修改引數設定LOG_DISARMED
在這裡插入圖片描述

SCHED_TASK(update_altitude,       10,    100), //更新高度資訊


void Copter::update_altitude()
{
    //讀取氣壓高度值--------read in baro altitude
    read_barometer();

    //寫高度資訊到dataflash logs中---------write altitude info to dataflash logs
    if (should_log(MASK_LOG_CTUN))
    {
        Log_Write_Control_Tuning();
    }
}

void Copter::Log_Write_Control_Tuning()
{
    //獲取地形高度----get terrain altitude
    float terr_alt = 0.0f;
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
    if (!terrain.height_above_terrain(terr_alt, true)) 
    {
        terr_alt = DataFlash.quiet_nan();
    }
#endif

    float _target_rangefinder_alt;
    if (target_rangefinder_alt_used) 
    {
        _target_rangefinder_alt = target_rangefinder_alt * 0.01f; // cm->m
    } else 
    {
        _target_rangefinder_alt = DataFlash.quiet_nan();
    }

    struct log_Control_Tuning pkt =
    {
        LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),  //LOG_CONTROL_TUNING_MSG=0x04
        time_us             : AP_HAL::micros64(),                      //執行時間
        throttle_in         : attitude_control->get_throttle_in(),     //throttle_in為油門輸入，0到1000的整數值。
        angle_boost         : attitude_control->angle_boost(),         //用於傾斜補償的油門增加量
        throttle_out        : motors->get_throttle(),                 //throttle_out油門輸出，0到1000的float數值。
        throttle_hover      : motors->get_throttle_hover(),           //評估油門值需要的油門滑動條範圍，修改這個值可以修正搖桿的中立點
        desired_alt         : pos_control->get_alt_target() / 100.0f, //desired_alt目標高度， _pos_target.z的值除以100，單位為米。
        inav_alt            : inertial_nav.get_altitude() / 100.0f,   //飛機目前高度，_relpos_cm.z的值除以100，單位為米。
        baro_alt            : baro_alt,                               // 氣壓計的高度，單位為釐米
        desired_rangefinder_alt : (int16_t)target_rangefinder_alt,     //目標測距儀高度
        rangefinder_alt     : rangefinder_state.alt_cm,                 //測距儀高度
        terr_alt            : terr_alt,                                 //地形高度
        target_climb_rate   : (int16_t)pos_control->get_vel_target_z(), //目標爬升率
        climb_rate          : climb_rate  //climb_rate為當前的爬升速率，此值在inertia.cpp中的read_inertial_altitude函式中更新，實際上就是讀取的inertial_nav的_velocity_cm.z的值
    };
    DataFlash.WriteBlock(&pkt, sizeof(pkt));  //把包寫進flash中
}

這裡寫圖片描述

5.ardupilot新增一條自己的引數，並顯示到地面站上

1.引數初始化

void Copter::setup()
{
    //從引數表中載入預設引數----------Load the default values of variables listed in var_info[]s
    AP_Param::setup_sketch_defaults();

    //初始化儲存的多旋翼佈局-----------setup storage layout for copter
    StorageManager::set_layout_copter();
    //感測器初始化，註冊
    /******************需要修改的地方**************************/
     /******************需要修改的地方**************************/
      /******************需要修改的地方**************************/
        init_ardupilot(); 
       /******************需要修改的地方**************************/
     /******************需要修改的地方**************************/
      /******************需要修改的地方**************************/
    //初始化整個主loop任務排程-------initialise the main loop scheduler
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}


#if LOGGING_ENABLED == ENABLED
    log_init();  //快閃記憶體日誌初始化,進入修改
#endif


void Copter::log_init(void)
{
    DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
}

const struct LogStructure Copter::log_structure[] =
{
    LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
    { LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
      "ATUN", "QBBfffffff",       "TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt", "s--ddd---o", "F--BBB---0" },
    { LOG_AUTOTUNEDETAILS_MSG, sizeof(log_AutoTuneDetails),
      "ATDE", "Qff",          "TimeUS,Angle,Rate", "sdk", "FBB" },
#endif
    { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
      "PTUN", "QBfHHH",          "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" },



#if OPTFLOW == ENABLED
    { LOG_OPTFLOW_MSG, sizeof(log_Optflow),
      "OF",   "QBffff",   "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" },
#endif
    { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
      "CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" },

	 //型別，長度，名字，格式，標籤，單位，乘法
    { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
      "MOTB", "Qffff",  "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
    { LOG_EVENT_MSG, sizeof(log_Event),         
      "EV",   "QB",           "TimeUS,Id", "s-", "F-" },
    { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),         
      "D16",   "QBh",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),         
      "DU16",  "QBH",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t),         
      "D32",   "QBi",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t),         
      "DU32",  "QBI",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),         
      "DFLT",  "QBf",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_ERROR_MSG, sizeof(log_Error),         
      "ERR",   "QBB",         "TimeUS,Subsys,ECode", "s--", "F--" },
#if FRAME_CONFIG == HELI_FRAME
    { LOG_HELI_MSG, sizeof(log_Heli),
      "HELI",  "Qff",         "TimeUS,DRRPM,ERRPM", "s--", "F--" },
#endif
#if PRECISION_LANDING == ENABLED
    { LOG_PRECLAND_MSG, sizeof(log_Precland),
      "PL",    "QBBfffffffIIB",    "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasUS,EKFOutl,Est", "s--ddmmddms--","F--00BB00BC--" },
#endif
    { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
      "GUID",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
       /******************需要修改的地方**************************/
     /******************需要修改的地方**************************/
      /******************需要修改的地方**************************/
	  { LOG_CONTROL_CT1_MSG,sizeof(log_Control_CT1),
	 	      "CT1", "Q", "TimeUS", "s", "F" },//這裡一定要注意書寫方法，特別是CT1,你不能隨便寫這個長度，長度不能超過四個字母，我這裡折騰很久，需要注意
};

新增log列舉變數的地方

enum LoggingParameters {
     TYPE_AIRSTART_MSG,
     TYPE_GROUNDSTART_MSG,
     LOG_CONTROL_TUNING_MSG,
     LOG_OPTFLOW_MSG,
     LOG_EVENT_MSG,
     LOG_ERROR_MSG,
     LOG_DATA_INT16_MSG,
     LOG_DATA_UINT16_MSG,
     LOG_DATA_INT32_MSG,
     LOG_DATA_UINT32_MSG,
     LOG_DATA_FLOAT_MSG,
     LOG_AUTOTUNE_MSG,
     LOG_AUTOTUNEDETAILS_MSG,
     LOG_MOTBATT_MSG,
     LOG_PARAMTUNE_MSG,
     LOG_HELI_MSG,
     LOG_PRECLAND_MSG,
     LOG_GUIDEDTARGET_MSG,
	 LOG_CONTROL_CT1_MSG,
};

定義結構體

struct PACKED log_Control_CT1 {
    LOG_PACKET_HEADER;
    uint64_t time_us;

};

2.更新寫入過程

SCHED_TASK(update_mylog,       10,    100),    //更新測試引數

需要注意需要在Copter.h中宣告update_mylog()這個函式

  void ten_hz_logging_loop();
    void twentyfive_hz_logging();
    void three_hz_loop();
    void one_hz_loop();
    void update_GPS(void);
    void init_simple_bearing();
    void update_simple_mode(void);
    void update_super_simple_bearing(bool force_update);
    void read_AHRS(void);
    void update_altitude();
    void update_mylog();//宣告函式

函式實現

void Copter::update_mylog()
{

    if (should_log(MASK_LOG_CT1)) 
    {
    	Log_Write_Control_CT1();

    }

}

其中：需要注意
（1）這裡要定義

 #define MASK_LOG_CT1        (1<<20)
#define MASK_LOG_ANY                    0x1FFFFF

（2） if (should_log(MASK_LOG_CT1))這裡需要設定的引數特別重要，不然if函式是進不去的

在這裡插入圖片描述
(3)函式體
宣告Copter.h

  void Log_Write_Control_Tuning();
    void Log_Write_Control_CT1(); //新增引數
    void Log_Write_Performance();

實現函式

void Copter::Log_Write_Control_CT1()
{

    struct log_Control_CT1 pkt =
    {
        LOG_PACKET_HEADER_INIT(LOG_CONTROL_CT1_MSG),
        time_us             : AP_HAL::micros64()
    };
    DataFlash.WriteBlock(&pkt, sizeof(pkt));
}

3.顯示在地面站

在這裡插入圖片描述

相關推薦

Ardupilot飛控log程式碼學習（3.6版本飛控韌體）

目錄 文章目錄 目錄 摘要 1.官網資料學習 新增一個新的日誌訊息（Adding a new Log Message） 簡單的方法（The Easy Way） 複雜的方法（The Harder Way）

win64 Python下安裝PIL出錯解決2.7版本 （3.6版本可以使用）

tis code win 選擇 pill could not 出錯 win64 3.6 轉自：http://blog.csdn.net/lhh31/article/details/51979293 1、軟件版本 首先我先安裝了 python 2.7 pip是 8.1.2

redis 叢集（3.0版本開始支援叢集功能）

叢集簡介 Redis 叢集是一個可以在多個 Redis 節點之間進行資料共享的設施（installation）。 Redis 叢集不支援那些需要同時處理多個鍵的 Redis 命令， 因為執行這些命令需要在多個 Redis 節點之間移動資料， 並且在高負載的情況下， 這

1小時教你把javaweb部署到騰訊雲上詳細教程（centOS6.6版本,jdk1.7+tomcat8+mysql）

最近想要把自己寫的web專案部署到騰訊雲上進行訪問，但是騰訊雲也不出一個官方的配置教程，像我們這種剛入門的新手完全懵逼。在百度上面搜了很久都沒有詳細的教程，都是在過程中出現各種各樣的問題，所以現在我就把我自己對騰訊雲的配置過程一步步的寫下來，希望大家能夠少走一些彎路。 其實

LSD-SLAM深入學習（3）-程式碼解析

前言      在LSD-SLAM深入學習（2)中我們已經對演算法進行分析，此處假設讀者對於ros的基本操作都已經很熟悉，而且已經編寫了一定量的的程式碼，我們直接上乾貨。此處分析的程式如下 main_live_odometry.cpp,這個可以實時讀取視訊的主函式 主函式 XIni

基於Qt的OpenGL可編程管線學習（3）- 使用Instanced方式繪制

qt opengl shader instanced 繪制多個重復的模型時，使用Instanced方式繪制可以大大加快顯然速度。繪制效果如下圖所示：1、Vertex Shader中定義如下：attribute vec3 pos; attribute vec2 coord; attribute

OAuth2.0學習（3-1）服務端實現

other cti info ase service packages artifacts ace ews 開源 http://oltu.apache.org/ 其他 http://www.oschina.net/project/tag/307/oauth?lang=19&

OSChinaclient源代碼學習（3）--輪詢機制的實現

man data 本地變量 其它 失敗 cbo cancel seh ref 主要以OSChina Androidclient源代碼中Notice的輪詢機制進行解讀。 一、基礎知識 一般IM（即使通訊）的實現有兩種方式：推送和輪詢，推送就是se

PYTHON設計模式學習（3）：Singleton pattern

blog ora 參考 擁有 沒有 _屬性 ces cti ttr 參考了其他的博客：http://ghostfromheaven.iteye.com/blog/1562618 #-*- encoding=utf-8 -*- print ‘--------------

tcpdump 學習（3）：MySQL Query

說明 server gen rollback 命令 復雜 pro 關於 col 在MySQL線上環境我們一般只打開了binary log，slow log，有時我們需要查看general log呢？因為該log記錄所有的請求，打開該日誌肯定給磁盤造成很大壓力，IO能力有所下

Django學習（3）模板定制

ase 我們 頻繁 ping upload shang star 2.0 asc 　　在Django學習（一）一首情詩中，views.py中HTML被直接硬編碼在代碼之中，雖然這樣便於解釋視圖是如何工作的，但直接將HTML硬編碼到視圖卻不算一個好主意。因為： 對頁面設計進

機器學習基石筆記11——機器可以怎樣學習（3）

1.2 div esc 怎樣 學習 clas regress 1.3 1.4 Lecture 11： Linear Models for Classification 11.1 Linear Models for Binary Classification 11.2 S

java入門學習（3）—循環，選擇，基礎算法，API概念

思想 冒泡 方法 就是 最大的 接口 兩個 循環控制 得到 1、順序結構：也就是順著程序的前後關系，依次執行。2、選擇分支：利用if..else , / switch(){case [ 這個必須是常量]：}; / if..else if….. ….else..等語句讓程序在

web前端學習（3）：認識HTML基本標簽

鼠標 首頁 一份 格式 所在地 有著 同時 網上 今天 本章主旨： 　　介紹常用的文本相關標簽，如<h>,<p>,<a>;簡介常見的HTML標簽屬性，如title屬性,href屬性,id/class屬性等；重點掌握<a>標簽。

Python爬蟲學習（3）

collect nbsp pri div time urlparse links ews 是否 在慕課網學習並創建了一個簡單的爬蟲包，爬取百度百科相關詞條信息 程序中會用到第三方解析包（BeautifulSoup4），Windows環境下安裝命令：pip install B

java===java基礎學習（3）---數據類型轉換，運算符級別，枚舉類型

test [] sof 進度 如果 nbsp math 否則 枚舉 數據類型轉換： 有的時候，程序需要將數據類型，比如 int + float ，結果是float， 這裏的int就被轉換為float類型，屬於合法轉換。 Java中的合法轉換如下圖： 紅色表示無信息丟失的轉

perl學習（3）模式匹配與正則表達式

.... 表達式 正則表達 pos 匹配模式 模式匹配 $2 多個 info 一：模式匹配中的特殊字符 1：　　點號 . 　　　 匹配任何單個字符(換行符\n除外) 2：　　反斜線 \ 　　　　轉義字符，用於特殊符號前，使其失去特殊字符的作用變成普通字符 3：　　+ 　　

Python3玩轉兒 機器學習（3）

分享圖片 多項式 pca 圖像 可視化 情況 異常 ... 學習 機器學習算法可以分為： 監督學習 非監督學習 半監督學習 增強學習 監督學習：給機器的訓練數據擁有“標記”或者“答案”，例如： 我們需要告訴機器左邊的畫面是一只狗，而右邊的照片是一只貓。同理對於MNIS

完全用Deepin Linux娛樂、工作、學習（3）-- 無線網卡及藍牙設備驅動篇

系統/運維 Linux 娛樂 1、異常說明 在前面木子已經講到了關於顯卡驅動及CPU微碼補丁更新，今天木子要講的是關於適配藍牙及無線網卡設備的驅動問題，它不僅僅適用於新驅動的安裝，也適用於驅動異常的修復。默認情況下木子的筆記本安裝Deepin Linux後是自帶驅動的，只是在折騰的過

Spring學習（3）：Spring概述（轉載）

效率 調度 jpa 源代碼 一個 維護 html www hiberna 1. Spring是什麽？ 　　Spring是一個開源的輕量級Java SE（Java 標準版本）/Java EE（Java 企業版本）開發應用框架，其目的是用於簡化企業級應用程序開發。 　　在面向對