diff --git a/AntennaTracker/AntennaTracker.cpp b/AntennaTracker/AntennaTracker.cpp index 9d972d761b..5e10930bc6 100644 --- a/AntennaTracker/AntennaTracker.cpp +++ b/AntennaTracker/AntennaTracker.cpp @@ -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) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 9474dbb6ca..7bd6fde1a4 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -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; } /* diff --git a/AntennaTracker/Log.cpp b/AntennaTracker/Log.cpp new file mode 100644 index 0000000000..89c718d4f3 --- /dev/null +++ b/AntennaTracker/Log.cpp @@ -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; iprintln("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; +} diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp index b629667ff8..87a0d4acdf 100644 --- a/AntennaTracker/tracking.cpp +++ b/AntennaTracker/tracking.cpp @@ -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); + } }