ardupilot/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.cpp

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
 * Example of DataFlash library.
 * originally based on code by Jordi MuÒoz and Jose Julio
 */

// Libraries
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL_AVR/AP_HAL_AVR.h>
#include <AP_HAL_SITL/AP_HAL_SITL.h>
#include <AP_HAL_Linux/AP_HAL_Linux.h>
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_PX4/AP_HAL_PX4.h>

#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Progmem/AP_Progmem.h>
#include <AP_Math/AP_Math.h>
#include <AP_Compass/AP_Compass.h>
#include <Filter/Filter.h>
#include <AP_Declination/AP_Declination.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_ADC/AP_ADC.h>
#include <AP_ADC_AnalogSource/AP_ADC_AnalogSource.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_GPS/AP_GPS.h>
#include <DataFlash/DataFlash.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Mission/AP_Mission.h>
#include <StorageManager/StorageManager.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_BattMonitor/AP_BattMonitor.h>


const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;

#define LOG_TEST_MSG 1

struct PACKED log_Test {
    LOG_PACKET_HEADER;
    uint16_t v1, v2, v3, v4;
    int32_t  l1, l2;
};

static const struct LogStructure log_structure[] PROGMEM = {
    LOG_COMMON_STRUCTURES,
    { LOG_TEST_MSG, sizeof(log_Test),       
    "TEST", "HHHHii",        "V1,V2,V3,V4,L1,L2" }
};

#define NUM_PACKETS 500

static uint16_t log_num;

class DataFlashTest {
public:
    void setup();
    void loop();

private:

    DataFlash_Class dataflash{PSTR("DF Test 0.1")};
    void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
};

static DataFlashTest dataflashtest;

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

    hal.console->println("Dataflash Log Test 1.0");

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

    if (dataflash.NeedErase()) {
        hal.console->println("Erasing...");
        dataflash.EraseAll();
    }

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

    uint32_t total_micros = 0;
    uint16_t i;

    for (i = 0; i < NUM_PACKETS; i++) {
        uint32_t start = hal.scheduler->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 += hal.scheduler->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);
}

void DataFlashTest::print_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
    port->printf_P(PSTR("Mode(%u)"), (unsigned)mode);
}

/*
  compatibility with old pde style build
 */
void setup(void);
void loop(void);

void setup()
{
    dataflashtest.setup();
}


void loop()
{
    dataflashtest.loop();
}

AP_HAL_MAIN();