Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: add dataflash logging

This commit is contained in:
Randy Mackay 2015-12-27 16:05:14 +09:00
parent 2829dc67ac
commit ecf01732d9
11 changed files with 224 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
AntennaTracker/AntennaTracker.cpp
View File

@ -40,6 +40,8 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
SCHED_TASK(gcs_data_stream_send, 50, 3000),
SCHED_TASK(compass_accumulate, 50, 1500),
SCHED_TASK(barometer_accumulate, 50, 900),
SCHED_TASK(ten_hz_logging_loop, 10, 300),
SCHED_TASK(dataflash_periodic, 50, 300),
SCHED_TASK(update_notify, 50, 100),
SCHED_TASK(check_usb_mux, 10, 300),
SCHED_TASK(gcs_retry_deferred, 50, 1000),
@ -83,6 +85,11 @@ void Tracker::loop()
scheduler.run(19900UL);
}
void Tracker::dataflash_periodic(void)
{
DataFlash.periodic_tasks();
}
void Tracker::one_second_loop()
{
// send a heartbeat
@ -107,6 +114,23 @@ void Tracker::one_second_loop()
}
}
void Tracker::ten_hz_logging_loop()
{
if (should_log(MASK_LOG_IMU)) {
DataFlash.Log_Write_IMU(ins);
DataFlash.Log_Write_IMUDT(ins);
}
if (should_log(MASK_LOG_ATTITUDE)) {
Log_Write_Attitude();
}
if (should_log(MASK_LOG_RCIN)) {
DataFlash.Log_Write_RCIN();
}
if (should_log(MASK_LOG_RCOUT)) {
DataFlash.Log_Write_RCOUT();
}
}
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
Tracker::Tracker(void)

29
AntennaTracker/GCS_Mavlink.cpp
View File

@ -5,9 +5,6 @@
// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS)
// use this to prevent recursion during sensor init
static bool in_mavlink_delay;
/*
* !!NOTE!!
*
@ -429,7 +426,7 @@ GCS_MAVLINK::data_stream_send(void)
}
}
if (in_mavlink_delay) {
if (tracker.in_mavlink_delay) {
// don't send any other stream types while in the delay callback
return;
}
@ -886,6 +883,26 @@ mission_failed:
break;
}
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
case MAVLINK_MSG_ID_LOG_ERASE:
tracker.in_log_download = true;
/* no break */
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
if (!tracker.in_mavlink_delay) {
handle_log_message(msg, tracker.DataFlash);
}
break;
case MAVLINK_MSG_ID_LOG_REQUEST_END:
tracker.in_log_download = false;
if (!tracker.in_mavlink_delay) {
handle_log_message(msg, tracker.DataFlash);
}
break;
case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
tracker.DataFlash.remote_log_block_status_msg(chan, msg);
break;
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg, tracker.gps);
break;
@ -913,7 +930,7 @@ void Tracker::mavlink_delay_cb()
static uint32_t last_1hz, last_50hz, last_5s;
if (!gcs[0].initialised) return;
in_mavlink_delay = true;
tracker.in_mavlink_delay = true;
uint32_t tnow = AP_HAL::millis();
if (tnow - last_1hz > 1000) {
@ -931,7 +948,7 @@ void Tracker::mavlink_delay_cb()
last_5s = tnow;
gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM");
}
in_mavlink_delay = false;
tracker.in_mavlink_delay = false;
}
/*

83
AntennaTracker/Log.cpp Normal file
View File

@ -0,0 +1,83 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Tracker.h"
#if LOGGING_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash log memory
// Write an attitude packet
void Tracker::Log_Write_Attitude()
{
Vector3f targets;
targets.y = nav_status.pitch;
targets.z = wrap_360_cd_float(nav_status.bearing);
DataFlash.Log_Write_Attitude(ahrs, targets);
DataFlash.Log_Write_EKF(ahrs,false);
DataFlash.Log_Write_AHRS2(ahrs);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
sitl.Log_Write_SIMSTATE(&DataFlash);
#endif
DataFlash.Log_Write_POS(ahrs);
}
void Tracker::Log_Write_Baro(void)
{
DataFlash.Log_Write_Baro(barometer);
}
const struct LogStructure Tracker::log_structure[] = {
LOG_COMMON_STRUCTURES,
};
void Tracker::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by DataFlash
DataFlash.Log_Write_Mode(control_mode);
}
// start a new log
void Tracker::start_logging()
{
if (g.log_bitmask != 0) {
if (!logging_started) {
logging_started = true;
DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&tracker, &Tracker::Log_Write_Vehicle_Startup_Messages, void));
DataFlash.StartNewLog();
}
// enable writes
DataFlash.EnableWrites(true);
}
}
void Tracker::log_init(void)
{
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
if (!DataFlash.CardInserted()) {
gcs_send_text(MAV_SEVERITY_WARNING, "No dataflash card inserted");
g.log_bitmask.set(0);
} else if (DataFlash.NeedPrep()) {
gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system");
DataFlash.Prep();
gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system");
for (uint8_t i=0; i<num_gcs; i++) {
gcs[i].reset_cli_timeout();
}
}
if (g.log_bitmask != 0) {
start_logging();
}
}
#else // LOGGING_ENABLED
void Tracker::Log_Write_Attitude(void) {}
void Tracker::Log_Write_Startup() {}
void Tracker::Log_Write_Baro(void) {}
void Tracker::start_logging() {}
void Tracker::log_init(void) {}
#endif // LOGGING_ENABLED

8
AntennaTracker/Parameters.cpp
View File

@ -226,6 +226,14 @@ const AP_Param::Info Tracker::var_info[] = {
// @Path: GCS_Mavlink.cpp
GOBJECTN(gcs[3], gcs3, "SR3_", GCS_MAVLINK),
// @Param: LOG_BITMASK
// @DisplayName: Log bitmask
// @Description: 4 byte bitmap of log types to enable
// @Values: 63:Default,0:Disabled
// @Bitmask: 0:ATTITUDE,1:GPS,2:RCIN,3:IMU,4:RCOUT,5:COMPASS
// @User: Standard
GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK),
// @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
GOBJECT(ins, "INS_", AP_InertialSensor),

3
AntennaTracker/Parameters.h
View File

@ -93,6 +93,7 @@ public:
k_param_distance_min,
k_param_sysid_target, // 138
k_param_gcs3, // stream rates for fourth MAVLink port
k_param_log_bitmask, // 140
//
// 150: Telemetry control
@ -148,6 +149,8 @@ public:
//
AP_Int8 command_total; // 1 if HOME is set
AP_Int32 log_bitmask;
// PID controllers
PID pidPitch2Srv;
PID pidYaw2Srv;

17
AntennaTracker/Tracker.h
View File

@ -100,6 +100,11 @@ private:
bool usb_connected = false;
// has a log download started?
bool in_log_download = false;
bool logging_started = false;
DataFlash_Class DataFlash{FIRMWARE_STRING};
AP_GPS gps;
AP_Baro barometer;
@ -180,10 +185,16 @@ private:
int8_t slew_dir = 0;
uint32_t slew_start_ms = 0;
// use this to prevent recursion during sensor init
bool in_mavlink_delay = false;
static const AP_Scheduler::Task scheduler_tasks[];
static const AP_Param::Info var_info[];
static const struct LogStructure log_structure[];
void dataflash_periodic(void);
void one_second_loop();
void ten_hz_logging_loop();
void send_heartbeat(mavlink_channel_t chan);
void send_attitude(mavlink_channel_t chan);
void send_location(mavlink_channel_t chan);
@ -243,6 +254,12 @@ private:
void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
void init_capabilities(void);
void compass_cal_update();
void Log_Write_Attitude();
void Log_Write_Baro(void);
void Log_Write_Vehicle_Startup_Messages();
void start_logging();
void log_init(void);
bool should_log(uint32_t mask);
public:
void mavlink_snoop(const mavlink_message_t* msg);

26
AntennaTracker/config.h
View File

@ -57,4 +57,30 @@
# define DISTANCE_MIN_DEFAULT 5.0f // do not track targets within 5 meters
#endif
//
// Dataflash logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
// Default logging bitmask
#ifndef DEFAULT_LOG_BITMASK
# define DEFAULT_LOG_BITMASK \
MASK_LOG_ATTITUDE | \
MASK_LOG_GPS | \
MASK_LOG_RCIN | \
MASK_LOG_IMU | \
MASK_LOG_RCOUT | \
MASK_LOG_COMPASS
#endif
/*
build a firmware version string.
GIT_VERSION comes from Makefile builds
*/
#ifndef GIT_VERSION
#define FIRMWARE_STRING THISFIRMWARE
#else
#define FIRMWARE_STRING THISFIRMWARE " (" GIT_VERSION ")"
#endif

9
AntennaTracker/defines.h
View File

@ -26,5 +26,14 @@ enum ServoType {
SERVO_TYPE_CR=2
};
// Logging parameters
#define MASK_LOG_ATTITUDE (1<<0)
#define MASK_LOG_GPS (1<<1)
#define MASK_LOG_RCIN (1<<2)
#define MASK_LOG_IMU (1<<3)
#define MASK_LOG_RCOUT (1<<4)
#define MASK_LOG_COMPASS (1<<5)
#define MASK_LOG_ANY 0xFFFF
#endif // _DEFINES_H

11
AntennaTracker/sensors.cpp
View File

@ -13,6 +13,9 @@ void Tracker::init_barometer(void)
void Tracker::update_barometer(void)
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
}
@ -33,6 +36,9 @@ void Tracker::update_compass(void)
if (g.compass_enabled && compass.read()) {
ahrs.set_compass(&compass);
compass.learn_offsets();
if (should_log(MASK_LOG_COMPASS)) {
DataFlash.Log_Write_Compass(compass);
}
} else {
ahrs.set_compass(NULL);
}
@ -103,6 +109,11 @@ void Tracker::update_GPS(void)
ground_start_count = 0;
}
}
// log GPS data
if (should_log(MASK_LOG_GPS)) {
DataFlash.Log_Write_GPS(gps, 0, current_loc.alt);
}
}
}

15
AntennaTracker/system.cpp
View File

@ -62,6 +62,10 @@ void Tracker::init_tracker()
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
if (g.compass_enabled==true) {
if (!compass.init() || !compass.read()) {
hal.console->println("Compass initialisation failed!");
@ -238,3 +242,14 @@ void Tracker::check_usb_mux(void)
// the user has switched to/from the telemetry port
usb_connected = usb_check;
}
/*
should we log a message type now?
*/
bool Tracker::should_log(uint32_t mask)
{
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
return false;
}
return true;
}

5
AntennaTracker/tracking.cpp
View File

@ -121,6 +121,11 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f;
vehicle.last_update_us = AP_HAL::micros();
vehicle.last_update_ms = AP_HAL::millis();
// log vehicle as GPS2
if (should_log(MASK_LOG_GPS)) {
DataFlash.Log_Write_GPS(gps, 1, vehicle.location.alt);
}
}