mirror of https://github.com/ArduPilot/ardupilot
GLOBAL: rename DataFlash_Class to AP_Logger
This commit is contained in:
parent
0bf55ce3f4
commit
b47733142f
|
@ -83,7 +83,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
|
|||
SCHED_TASK(compass_save, 0.1, 200),
|
||||
SCHED_TASK(accel_cal_update, 10, 200),
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
SCHED_TASK_CLASS(DataFlash_Class, &rover.DataFlash, periodic_tasks, 50, 300),
|
||||
SCHED_TASK_CLASS(AP_Logger, &rover.logger, periodic_tasks, 50, 300),
|
||||
#endif
|
||||
SCHED_TASK_CLASS(AP_InertialSensor, &rover.ins, periodic, 400, 200),
|
||||
SCHED_TASK_CLASS(AP_Scheduler, &rover.scheduler, update_logging, 0.1, 200),
|
||||
|
@ -139,7 +139,7 @@ void Rover::update_soft_armed()
|
|||
{
|
||||
hal.util->set_soft_armed(arming.is_armed() &&
|
||||
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
||||
DataFlash.set_vehicle_armed(hal.util->get_soft_armed());
|
||||
logger.set_vehicle_armed(hal.util->get_soft_armed());
|
||||
}
|
||||
|
||||
// update AHRS system
|
||||
|
@ -177,7 +177,7 @@ void Rover::ahrs_update()
|
|||
}
|
||||
|
||||
if (should_log(MASK_LOG_IMU)) {
|
||||
DataFlash.Log_Write_IMU();
|
||||
logger.Log_Write_IMU();
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -204,7 +204,7 @@ void Rover::update_compass(void)
|
|||
ahrs.set_compass(&compass);
|
||||
// update offsets
|
||||
if (should_log(MASK_LOG_COMPASS)) {
|
||||
DataFlash.Log_Write_Compass();
|
||||
logger.Log_Write_Compass();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -221,7 +221,7 @@ void Rover::update_logging1(void)
|
|||
|
||||
if (should_log(MASK_LOG_THR)) {
|
||||
Log_Write_Throttle();
|
||||
DataFlash.Log_Write_Beacon(g2.beacon);
|
||||
logger.Log_Write_Beacon(g2.beacon);
|
||||
}
|
||||
|
||||
if (should_log(MASK_LOG_NTUN)) {
|
||||
|
@ -229,7 +229,7 @@ void Rover::update_logging1(void)
|
|||
}
|
||||
|
||||
if (should_log(MASK_LOG_RANGEFINDER)) {
|
||||
DataFlash.Log_Write_Proximity(g2.proximity);
|
||||
logger.Log_Write_Proximity(g2.proximity);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -248,7 +248,7 @@ void Rover::update_logging2(void)
|
|||
}
|
||||
|
||||
if (should_log(MASK_LOG_IMU)) {
|
||||
DataFlash.Log_Write_Vibration();
|
||||
logger.Log_Write_Vibration();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -180,7 +180,7 @@ void GCS_MAVLINK_Rover::send_rangefinder() const
|
|||
*/
|
||||
void Rover::send_pid_tuning(mavlink_channel_t chan)
|
||||
{
|
||||
const DataFlash_Class::PID_Info *pid_info;
|
||||
const AP_Logger::PID_Info *pid_info;
|
||||
// steering PID
|
||||
if (g.gcs_pid_mask & 1) {
|
||||
pid_info = &g2.attitude_control.get_steering_rate_pid().get_pid_info();
|
||||
|
@ -1130,7 +1130,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
case MAVLINK_MSG_ID_RADIO:
|
||||
case MAVLINK_MSG_ID_RADIO_STATUS:
|
||||
{
|
||||
handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM));
|
||||
handle_radio_status(msg, rover.logger, rover.should_log(MASK_LOG_PM));
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -1168,7 +1168,7 @@ void Rover::mavlink_delay_cb()
|
|||
}
|
||||
|
||||
// don't allow potentially expensive logging calls:
|
||||
DataFlash.EnableWrites(false);
|
||||
logger.EnableWrites(false);
|
||||
|
||||
const uint32_t tnow = millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
|
@ -1187,7 +1187,7 @@ void Rover::mavlink_delay_cb()
|
|||
gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
|
||||
}
|
||||
|
||||
DataFlash.EnableWrites(true);
|
||||
logger.EnableWrites(true);
|
||||
}
|
||||
|
||||
AP_AdvancedFailsafe *GCS_MAVLINK_Rover::get_advanced_failsafe() const
|
||||
|
|
|
@ -19,7 +19,7 @@ void Rover::Log_Write_Arm_Disarm()
|
|||
arm_state : arming.is_armed(),
|
||||
arm_checks : arming.get_enabled_checks()
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// Write an attitude packet
|
||||
|
@ -28,26 +28,26 @@ void Rover::Log_Write_Attitude()
|
|||
float desired_pitch_cd = degrees(g2.attitude_control.get_desired_pitch()) * 100.0f;
|
||||
const Vector3f targets(0.0f, desired_pitch_cd, 0.0f);
|
||||
|
||||
DataFlash.Log_Write_Attitude(ahrs, targets);
|
||||
logger.Log_Write_Attitude(ahrs, targets);
|
||||
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
DataFlash.Log_Write_EKF(ahrs);
|
||||
DataFlash.Log_Write_AHRS2(ahrs);
|
||||
logger.Log_Write_EKF(ahrs);
|
||||
logger.Log_Write_AHRS2(ahrs);
|
||||
#endif
|
||||
DataFlash.Log_Write_POS(ahrs);
|
||||
logger.Log_Write_POS(ahrs);
|
||||
|
||||
// log steering rate controller
|
||||
DataFlash.Log_Write_PID(LOG_PIDS_MSG, g2.attitude_control.get_steering_rate_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g2.attitude_control.get_throttle_speed_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDS_MSG, g2.attitude_control.get_steering_rate_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDA_MSG, g2.attitude_control.get_throttle_speed_pid().get_pid_info());
|
||||
|
||||
// log pitch control for balance bots
|
||||
if (is_balancebot()) {
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDP_MSG, g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info());
|
||||
}
|
||||
|
||||
// log heel to sail control for sailboats
|
||||
if (g2.motors.has_sail()) {
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info());
|
||||
}
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
sitl.Log_Write_SIMSTATE();
|
||||
|
@ -75,7 +75,7 @@ void Rover::Log_Write_Depth()
|
|||
}
|
||||
rangefinder_last_reading_ms = reading_ms;
|
||||
|
||||
DataFlash.Log_Write("DPTH", "TimeUS,Lat,Lng,Depth",
|
||||
logger.Log_Write("DPTH", "TimeUS,Lat,Lng,Depth",
|
||||
"sDUm", "FGG0", "QLLf",
|
||||
AP_HAL::micros64(),
|
||||
loc.lat,
|
||||
|
@ -99,7 +99,7 @@ void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
|
|||
sub_system : sub_system,
|
||||
error_code : error_code,
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// guided mode logging
|
||||
|
@ -129,7 +129,7 @@ void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_targ
|
|||
vel_target_y : vel_target.y,
|
||||
vel_target_z : vel_target.z
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Nav_Tuning {
|
||||
|
@ -154,7 +154,7 @@ void Rover::Log_Write_Nav_Tuning()
|
|||
yaw : (uint16_t)ahrs.yaw_sensor,
|
||||
xtrack_error : nav_controller->crosstrack_error()
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
void Rover::Log_Write_Sail()
|
||||
|
@ -165,17 +165,17 @@ void Rover::Log_Write_Sail()
|
|||
}
|
||||
|
||||
// get wind direction
|
||||
float wind_dir_abs = DataFlash.quiet_nanf();
|
||||
float wind_dir_rel = DataFlash.quiet_nanf();
|
||||
float wind_speed_true = DataFlash.quiet_nanf();
|
||||
float wind_speed_apparent = DataFlash.quiet_nanf();
|
||||
float wind_dir_abs = logger.quiet_nanf();
|
||||
float wind_dir_rel = logger.quiet_nanf();
|
||||
float wind_speed_true = logger.quiet_nanf();
|
||||
float wind_speed_apparent = logger.quiet_nanf();
|
||||
if (rover.g2.windvane.enabled()) {
|
||||
wind_dir_abs = degrees(g2.windvane.get_absolute_wind_direction_rad());
|
||||
wind_dir_rel = degrees(g2.windvane.get_apparent_wind_direction_rad());
|
||||
wind_speed_true = g2.windvane.get_true_wind_speed();
|
||||
wind_speed_apparent = g2.windvane.get_apparent_wind_speed();
|
||||
}
|
||||
DataFlash.Log_Write("SAIL", "TimeUS,WindDirAbs,WindDirApp,WindSpdTrue,WindSpdApp,SailOut,VMG",
|
||||
logger.Log_Write("SAIL", "TimeUS,WindDirAbs,WindDirApp,WindSpdTrue,WindSpdApp,SailOut,VMG",
|
||||
"shhnn%n", "F000000", "Qffffff",
|
||||
AP_HAL::micros64(),
|
||||
(double)wind_dir_abs,
|
||||
|
@ -212,13 +212,13 @@ void Rover::Log_Write_Startup(uint8_t type)
|
|||
startup_type : type,
|
||||
command_total : mode_auto.mission.num_commands()
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// Write a steering packet
|
||||
void Rover::Log_Write_Steering()
|
||||
{
|
||||
float lat_accel = DataFlash.quiet_nanf();
|
||||
float lat_accel = logger.quiet_nanf();
|
||||
g2.attitude_control.get_lat_accel(lat_accel);
|
||||
struct log_Steering pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
|
||||
|
@ -230,7 +230,7 @@ void Rover::Log_Write_Steering()
|
|||
desired_turn_rate : degrees(g2.attitude_control.get_desired_turn_rate()),
|
||||
turn_rate : degrees(ahrs.get_yaw_rate_earth())
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Throttle {
|
||||
|
@ -247,7 +247,7 @@ struct PACKED log_Throttle {
|
|||
void Rover::Log_Write_Throttle()
|
||||
{
|
||||
const Vector3f accel = ins.get_accel();
|
||||
float speed = DataFlash.quiet_nanf();
|
||||
float speed = logger.quiet_nanf();
|
||||
g2.attitude_control.get_forward_speed(speed);
|
||||
struct log_Throttle pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_THR_MSG),
|
||||
|
@ -258,7 +258,7 @@ void Rover::Log_Write_Throttle()
|
|||
speed : speed,
|
||||
accel_y : accel.y
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Rangefinder {
|
||||
|
@ -295,15 +295,15 @@ void Rover::Log_Write_Rangefinder()
|
|||
ground_speed : static_cast<uint16_t>(fabsf(ground_speed * 100.0f)),
|
||||
throttle : int8_t(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
void Rover::Log_Write_RC(void)
|
||||
{
|
||||
DataFlash.Log_Write_RCIN();
|
||||
DataFlash.Log_Write_RCOUT();
|
||||
logger.Log_Write_RCIN();
|
||||
logger.Log_Write_RCOUT();
|
||||
if (rssi.enabled()) {
|
||||
DataFlash.Log_Write_RSSI(rssi);
|
||||
logger.Log_Write_RSSI(rssi);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -336,20 +336,20 @@ void Rover::Log_Write_WheelEncoder()
|
|||
quality_1 : (uint8_t)constrain_float(g2.wheel_encoder.get_signal_quality(1), 0.0f, 100.0f),
|
||||
rpm_1 : wheel_encoder_rpm[1]
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
void Rover::Log_Write_Vehicle_Startup_Messages()
|
||||
{
|
||||
// only 200(?) bytes are guaranteed by DataFlash
|
||||
// only 200(?) bytes are guaranteed by AP_Logger
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
DataFlash.Log_Write_Mode(control_mode->mode_number(), control_mode_reason);
|
||||
logger.Log_Write_Mode(control_mode->mode_number(), control_mode_reason);
|
||||
ahrs.Log_Write_Home_And_Origin();
|
||||
gps.Write_DataFlash_Log_Startup_messages();
|
||||
gps.Write_AP_Logger_Log_Startup_messages();
|
||||
}
|
||||
|
||||
// type and unit information can be found in
|
||||
// libraries/DataFlash/Logstructure.h; search for "log_Units" for
|
||||
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
|
||||
// units and "Format characters" for field type information
|
||||
const LogStructure Rover::log_structure[] = {
|
||||
LOG_COMMON_STRUCTURES,
|
||||
|
@ -375,7 +375,7 @@ const LogStructure Rover::log_structure[] = {
|
|||
|
||||
void Rover::log_init(void)
|
||||
{
|
||||
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
logger.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
}
|
||||
|
||||
#else // LOGGING_ENABLED
|
||||
|
|
|
@ -377,8 +377,8 @@ const AP_Param::Info Rover::var_info[] = {
|
|||
GOBJECT(arming, "ARMING_", AP_Arming),
|
||||
|
||||
// @Group: LOG
|
||||
// @Path: ../libraries/DataFlash/DataFlash.cpp
|
||||
GOBJECT(DataFlash, "LOG", DataFlash_Class),
|
||||
// @Path: ../libraries/AP_Logger/AP_Logger.cpp
|
||||
GOBJECT(logger, "LOG", AP_Logger),
|
||||
|
||||
// @Group: BATT
|
||||
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
|
||||
|
|
|
@ -207,7 +207,7 @@ public:
|
|||
k_param_button,
|
||||
k_param_osd,
|
||||
|
||||
k_param_DataFlash = 253, // Logging Group
|
||||
k_param_logger = 253, // Logging Group
|
||||
|
||||
// 254,255: reserved
|
||||
};
|
||||
|
|
|
@ -28,7 +28,7 @@ Rover::Rover(void) :
|
|||
channel_throttle(nullptr),
|
||||
channel_aux(nullptr),
|
||||
channel_lateral(nullptr),
|
||||
DataFlash{g.log_bitmask},
|
||||
logger{g.log_bitmask},
|
||||
modes(&g.mode1),
|
||||
nav_controller(&L1_controller),
|
||||
control_mode(&mode_initializing),
|
||||
|
|
|
@ -66,7 +66,7 @@
|
|||
#include <AP_WheelEncoder/AP_WheelRateControl.h>
|
||||
#include <APM_Control/AR_AttitudeControl.h>
|
||||
#include <AP_SmartRTL/AP_SmartRTL.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <Filter/AverageFilter.h> // Mode Filter from Filter library
|
||||
#include <Filter/Butter.h> // Filter library - butterworth filter
|
||||
#include <Filter/Filter.h> // Filter library
|
||||
|
@ -163,7 +163,7 @@ private:
|
|||
RC_Channel *channel_aux;
|
||||
RC_Channel *channel_lateral;
|
||||
|
||||
DataFlash_Class DataFlash;
|
||||
AP_Logger logger;
|
||||
|
||||
// sensor drivers
|
||||
AP_GPS gps;
|
||||
|
|
|
@ -56,7 +56,7 @@ bool Rover::set_home(const Location& loc, bool lock)
|
|||
if (should_log(MASK_LOG_CMD)) {
|
||||
AP_Mission::Mission_Command temp_cmd;
|
||||
if (mode_auto.mission.read_cmd_from_storage(0, temp_cmd)) {
|
||||
DataFlash.Log_Write_Mission_Cmd(mode_auto.mission, temp_cmd);
|
||||
logger.Log_Write_Mission_Cmd(mode_auto.mission, temp_cmd);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -17,7 +17,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
{
|
||||
// log when new commands start
|
||||
if (rover.should_log(MASK_LOG_CMD)) {
|
||||
rover.DataFlash.Log_Write_Mission_Cmd(mission, cmd);
|
||||
rover.logger.Log_Write_Mission_Cmd(mission, cmd);
|
||||
}
|
||||
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Executing %s(ID=%i)",
|
||||
|
|
|
@ -63,7 +63,7 @@ void Rover::cruise_learn_complete()
|
|||
// logging for cruise learn
|
||||
void Rover::log_write_cruise_learn()
|
||||
{
|
||||
DataFlash_Class::instance()->Log_Write("CRSE", "TimeUS,State,Speed,Throttle", "Qbff",
|
||||
AP_Logger::instance()->Log_Write("CRSE", "TimeUS,State,Speed,Throttle", "Qbff",
|
||||
AP_HAL::micros64,
|
||||
cruise_learn.learn_start_ms > 0,
|
||||
cruise_learn.speed_filt.get(),
|
||||
|
|
|
@ -58,7 +58,7 @@ void Rover::update_visual_odom()
|
|||
visual_odom_last_update_ms,
|
||||
g2.visual_odom.get_pos_offset());
|
||||
// log sensor data
|
||||
DataFlash.Log_Write_VisualOdom(time_delta_sec,
|
||||
logger.Log_Write_VisualOdom(time_delta_sec,
|
||||
g2.visual_odom.get_angle_delta(),
|
||||
g2.visual_odom.get_position_delta(),
|
||||
g2.visual_odom.get_confidence());
|
||||
|
@ -268,7 +268,7 @@ void Rover::update_sensor_status_flags(void)
|
|||
if (g2.visual_odom.enabled()) {
|
||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
|
||||
}
|
||||
if (rover.DataFlash.logging_present()) { // primary logging only (usually File)
|
||||
if (rover.logger.logging_present()) { // primary logging only (usually File)
|
||||
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
if (rover.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) {
|
||||
|
@ -292,7 +292,7 @@ void Rover::update_sensor_status_flags(void)
|
|||
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
|
||||
}
|
||||
|
||||
if (rover.DataFlash.logging_enabled()) {
|
||||
if (rover.logger.logging_enabled()) {
|
||||
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
|
||||
|
@ -339,7 +339,7 @@ void Rover::update_sensor_status_flags(void)
|
|||
if (rover.g2.proximity.get_status() == AP_Proximity::Proximity_NoData) {
|
||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION;
|
||||
}
|
||||
if (rover.DataFlash.logging_failed()) {
|
||||
if (rover.logger.logging_failed()) {
|
||||
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
|
||||
|
|
|
@ -183,9 +183,9 @@ void Rover::startup_ground(void)
|
|||
// initialise mission library
|
||||
mode_auto.mission.init();
|
||||
|
||||
// initialise DataFlash library
|
||||
// initialise AP_Logger library
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
DataFlash.setVehicle_Startup_Log_Writer(
|
||||
logger.setVehicle_Startup_Log_Writer(
|
||||
FUNCTOR_BIND(&rover, &Rover::Log_Write_Vehicle_Startup_Messages, void)
|
||||
);
|
||||
#endif
|
||||
|
@ -262,7 +262,7 @@ bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)
|
|||
old_mode.exit();
|
||||
|
||||
control_mode_reason = reason;
|
||||
DataFlash.Log_Write_Mode(control_mode->mode_number(), control_mode_reason);
|
||||
logger.Log_Write_Mode(control_mode->mode_number(), control_mode_reason);
|
||||
|
||||
notify_mode(control_mode);
|
||||
return true;
|
||||
|
@ -308,7 +308,7 @@ uint8_t Rover::check_digital_pin(uint8_t pin)
|
|||
*/
|
||||
bool Rover::should_log(uint32_t mask)
|
||||
{
|
||||
return DataFlash.should_log(mask);
|
||||
return logger.should_log(mask);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -44,7 +44,7 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
|
|||
SCHED_TASK_CLASS(AP_Baro, &tracker.barometer, accumulate, 50, 900),
|
||||
SCHED_TASK(ten_hz_logging_loop, 10, 300),
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
SCHED_TASK_CLASS(DataFlash_Class, &tracker.DataFlash, periodic_tasks, 50, 300),
|
||||
SCHED_TASK_CLASS(AP_Logger, &tracker.logger, periodic_tasks, 50, 300),
|
||||
#endif
|
||||
SCHED_TASK_CLASS(AP_InertialSensor, &tracker.ins, periodic, 50, 50),
|
||||
SCHED_TASK_CLASS(AP_Notify, &tracker.notify, update, 50, 100),
|
||||
|
@ -123,23 +123,23 @@ void Tracker::one_second_loop()
|
|||
void Tracker::ten_hz_logging_loop()
|
||||
{
|
||||
if (should_log(MASK_LOG_IMU)) {
|
||||
DataFlash.Log_Write_IMU();
|
||||
logger.Log_Write_IMU();
|
||||
}
|
||||
if (should_log(MASK_LOG_ATTITUDE)) {
|
||||
Log_Write_Attitude();
|
||||
}
|
||||
if (should_log(MASK_LOG_RCIN)) {
|
||||
DataFlash.Log_Write_RCIN();
|
||||
logger.Log_Write_RCIN();
|
||||
}
|
||||
if (should_log(MASK_LOG_RCOUT)) {
|
||||
DataFlash.Log_Write_RCOUT();
|
||||
logger.Log_Write_RCOUT();
|
||||
}
|
||||
}
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||||
|
||||
Tracker::Tracker(void)
|
||||
: DataFlash(g.log_bitmask)
|
||||
: logger(g.log_bitmask)
|
||||
{
|
||||
memset(&vehicle, 0, sizeof(vehicle));
|
||||
}
|
||||
|
|
|
@ -580,7 +580,7 @@ void Tracker::mavlink_delay_cb()
|
|||
return;
|
||||
}
|
||||
|
||||
DataFlash.EnableWrites(false);
|
||||
logger.EnableWrites(false);
|
||||
|
||||
uint32_t tnow = AP_HAL::millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
|
@ -598,7 +598,7 @@ void Tracker::mavlink_delay_cb()
|
|||
last_5s = tnow;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
|
||||
}
|
||||
DataFlash.EnableWrites(true);
|
||||
logger.EnableWrites(true);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
|
||||
// Code to Write and Read packets from DataFlash log memory
|
||||
// Code to Write and Read packets from AP_Logger log memory
|
||||
|
||||
// Write an attitude packet
|
||||
void Tracker::Log_Write_Attitude()
|
||||
|
@ -10,13 +10,13 @@ void Tracker::Log_Write_Attitude()
|
|||
Vector3f targets;
|
||||
targets.y = nav_status.pitch * 100.0f;
|
||||
targets.z = wrap_360_cd(nav_status.bearing * 100.0f);
|
||||
DataFlash.Log_Write_Attitude(ahrs, targets);
|
||||
DataFlash.Log_Write_EKF(ahrs);
|
||||
DataFlash.Log_Write_AHRS2(ahrs);
|
||||
logger.Log_Write_Attitude(ahrs, targets);
|
||||
logger.Log_Write_EKF(ahrs);
|
||||
logger.Log_Write_AHRS2(ahrs);
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
sitl.Log_Write_SIMSTATE();
|
||||
#endif
|
||||
DataFlash.Log_Write_POS(ahrs);
|
||||
logger.Log_Write_POS(ahrs);
|
||||
}
|
||||
|
||||
struct PACKED log_Vehicle_Baro {
|
||||
|
@ -35,7 +35,7 @@ void Tracker::Log_Write_Vehicle_Baro(float pressure, float altitude)
|
|||
press : pressure,
|
||||
alt_diff : altitude
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Vehicle_Pos {
|
||||
|
@ -62,11 +62,11 @@ void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, const
|
|||
vehicle_vel_y : vel.y,
|
||||
vehicle_vel_z : vel.z,
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// type and unit information can be found in
|
||||
// libraries/DataFlash/Logstructure.h; search for "log_Units" for
|
||||
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
|
||||
// units and "Format characters" for field type information
|
||||
const struct LogStructure Tracker::log_structure[] = {
|
||||
LOG_COMMON_STRUCTURES,
|
||||
|
@ -78,13 +78,13 @@ const struct LogStructure Tracker::log_structure[] = {
|
|||
|
||||
void Tracker::Log_Write_Vehicle_Startup_Messages()
|
||||
{
|
||||
DataFlash.Log_Write_Mode(control_mode, MODE_REASON_INITIALISED);
|
||||
gps.Write_DataFlash_Log_Startup_messages();
|
||||
logger.Log_Write_Mode(control_mode, MODE_REASON_INITIALISED);
|
||||
gps.Write_AP_Logger_Log_Startup_messages();
|
||||
}
|
||||
|
||||
void Tracker::log_init(void)
|
||||
{
|
||||
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
logger.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
}
|
||||
|
||||
#else // LOGGING_ENABLED
|
||||
|
|
|
@ -41,7 +41,7 @@
|
|||
|
||||
#include <AP_SerialManager/AP_SerialManager.h> // Serial manager library
|
||||
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AC_PID/AC_PID.h>
|
||||
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
|
||||
#include <AP_NavEKF2/AP_NavEKF2.h>
|
||||
|
@ -98,7 +98,7 @@ private:
|
|||
|
||||
uint32_t start_time_ms = 0;
|
||||
|
||||
DataFlash_Class DataFlash;
|
||||
AP_Logger logger;
|
||||
|
||||
AP_GPS gps;
|
||||
|
||||
|
|
|
@ -17,7 +17,7 @@ void Tracker::update_compass(void)
|
|||
if (g.compass_enabled && compass.read()) {
|
||||
ahrs.set_compass(&compass);
|
||||
if (should_log(MASK_LOG_COMPASS)) {
|
||||
DataFlash.Log_Write_Compass();
|
||||
logger.Log_Write_Compass();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -103,7 +103,7 @@ void Tracker::update_sensor_status_flags()
|
|||
if (gps.status() > AP_GPS::NO_GPS) {
|
||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
|
||||
}
|
||||
if (DataFlash.logging_present()) { // primary logging only (usually File)
|
||||
if (logger.logging_present()) { // primary logging only (usually File)
|
||||
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
|
||||
|
@ -112,7 +112,7 @@ void Tracker::update_sensor_status_flags()
|
|||
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
|
||||
~MAV_SYS_STATUS_SENSOR_BATTERY);
|
||||
|
||||
if (DataFlash.logging_enabled()) {
|
||||
if (logger.logging_enabled()) {
|
||||
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
|
||||
|
@ -143,7 +143,7 @@ void Tracker::update_sensor_status_flags()
|
|||
// AHRS subsystem is unhealthy
|
||||
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
|
||||
}
|
||||
if (DataFlash.logging_failed()) {
|
||||
if (logger.logging_failed()) {
|
||||
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
if (!battery.healthy() || battery.has_failsafed()) {
|
||||
|
|
|
@ -76,8 +76,8 @@ void Tracker::init_tracker()
|
|||
|
||||
barometer.calibrate();
|
||||
|
||||
// initialise DataFlash library
|
||||
DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&tracker, &Tracker::Log_Write_Vehicle_Startup_Messages, void));
|
||||
// initialise AP_Logger library
|
||||
logger.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&tracker, &Tracker::Log_Write_Vehicle_Startup_Messages, void));
|
||||
|
||||
// set serial ports non-blocking
|
||||
serial_manager.set_blocking_writes_all(false);
|
||||
|
@ -163,13 +163,13 @@ void Tracker::set_home(struct Location temp)
|
|||
void Tracker::arm_servos()
|
||||
{
|
||||
hal.util->set_soft_armed(true);
|
||||
DataFlash.set_vehicle_armed(true);
|
||||
logger.set_vehicle_armed(true);
|
||||
}
|
||||
|
||||
void Tracker::disarm_servos()
|
||||
{
|
||||
hal.util->set_soft_armed(false);
|
||||
DataFlash.set_vehicle_armed(false);
|
||||
logger.set_vehicle_armed(false);
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -207,7 +207,7 @@ void Tracker::set_mode(enum ControlMode mode, mode_reason_t reason)
|
|||
}
|
||||
|
||||
// log mode change
|
||||
DataFlash.Log_Write_Mode(control_mode, reason);
|
||||
logger.Log_Write_Mode(control_mode, reason);
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -215,7 +215,7 @@ void Tracker::set_mode(enum ControlMode mode, mode_reason_t reason)
|
|||
*/
|
||||
bool Tracker::should_log(uint32_t mask)
|
||||
{
|
||||
if (!DataFlash.should_log(mask)) {
|
||||
if (!logger.should_log(mask)) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
|
|
|
@ -147,7 +147,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
|
|||
#if LOGGING_ENABLED == ENABLED
|
||||
SCHED_TASK(ten_hz_logging_loop, 10, 350),
|
||||
SCHED_TASK(twentyfive_hz_logging, 25, 110),
|
||||
SCHED_TASK_CLASS(DataFlash_Class, &copter.DataFlash, periodic_tasks, 400, 300),
|
||||
SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300),
|
||||
#endif
|
||||
SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50),
|
||||
SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75),
|
||||
|
@ -312,7 +312,7 @@ void Copter::update_batt_compass(void)
|
|||
compass.read();
|
||||
// log compass information
|
||||
if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
|
||||
DataFlash.Log_Write_Compass();
|
||||
logger.Log_Write_Compass();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -339,27 +339,27 @@ void Copter::ten_hz_logging_loop()
|
|||
Log_Write_MotBatt();
|
||||
}
|
||||
if (should_log(MASK_LOG_RCIN)) {
|
||||
DataFlash.Log_Write_RCIN();
|
||||
logger.Log_Write_RCIN();
|
||||
if (rssi.enabled()) {
|
||||
DataFlash.Log_Write_RSSI(rssi);
|
||||
logger.Log_Write_RSSI(rssi);
|
||||
}
|
||||
}
|
||||
if (should_log(MASK_LOG_RCOUT)) {
|
||||
DataFlash.Log_Write_RCOUT();
|
||||
logger.Log_Write_RCOUT();
|
||||
}
|
||||
if (should_log(MASK_LOG_NTUN) && (flightmode->requires_GPS() || landing_with_GPS())) {
|
||||
pos_control->write_log();
|
||||
}
|
||||
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
|
||||
DataFlash.Log_Write_Vibration();
|
||||
logger.Log_Write_Vibration();
|
||||
}
|
||||
if (should_log(MASK_LOG_CTUN)) {
|
||||
attitude_control->control_monitor_log();
|
||||
#if PROXIMITY_ENABLED == ENABLED
|
||||
DataFlash.Log_Write_Proximity(g2.proximity); // Write proximity sensor distances
|
||||
logger.Log_Write_Proximity(g2.proximity); // Write proximity sensor distances
|
||||
#endif
|
||||
#if BEACON_ENABLED == ENABLED
|
||||
DataFlash.Log_Write_Beacon(g2.beacon);
|
||||
logger.Log_Write_Beacon(g2.beacon);
|
||||
#endif
|
||||
}
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
@ -382,7 +382,7 @@ void Copter::twentyfive_hz_logging()
|
|||
|
||||
// log IMU data if we're not already logging at the higher rate
|
||||
if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) {
|
||||
DataFlash.Log_Write_IMU();
|
||||
logger.Log_Write_IMU();
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -24,7 +24,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
|||
constructor for main Copter class
|
||||
*/
|
||||
Copter::Copter(void)
|
||||
: DataFlash(g.log_bitmask),
|
||||
: logger(g.log_bitmask),
|
||||
flight_modes(&g.flight_mode1),
|
||||
control_mode(STABILIZE),
|
||||
scaleLongDown(1),
|
||||
|
|
|
@ -37,7 +37,7 @@
|
|||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h> // Serial manager library
|
||||
#include <AP_GPS/AP_GPS.h> // ArduPilot GPS library
|
||||
#include <DataFlash/DataFlash.h> // ArduPilot Mega Flash Memory Library
|
||||
#include <AP_Logger/AP_Logger.h> // ArduPilot Mega Flash Memory Library
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_Compass/AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
|
@ -233,7 +233,7 @@ private:
|
|||
RC_Channel *channel_yaw;
|
||||
|
||||
// Dataflash
|
||||
DataFlash_Class DataFlash;
|
||||
AP_Logger logger;
|
||||
|
||||
AP_GPS gps;
|
||||
|
||||
|
|
|
@ -228,7 +228,7 @@ void GCS_MAVLINK_Copter::send_pid_tuning()
|
|||
default:
|
||||
continue;
|
||||
}
|
||||
const DataFlash_Class::PID_Info &pid_info = pid.get_pid_info();
|
||||
const AP_Logger::PID_Info &pid_info = pid.get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan,
|
||||
axes[i],
|
||||
pid_info.desired*0.01f,
|
||||
|
@ -1360,7 +1360,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
|||
case MAVLINK_MSG_ID_RADIO:
|
||||
case MAVLINK_MSG_ID_RADIO_STATUS: // MAV ID: 109
|
||||
{
|
||||
handle_radio_status(msg, copter.DataFlash, copter.should_log(MASK_LOG_PM));
|
||||
handle_radio_status(msg, copter.logger, copter.should_log(MASK_LOG_PM));
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -1438,7 +1438,7 @@ void Copter::mavlink_delay_cb()
|
|||
static uint32_t last_1hz, last_50hz, last_5s;
|
||||
if (!gcs().chan(0).initialised) return;
|
||||
|
||||
DataFlash.EnableWrites(false);
|
||||
logger.EnableWrites(false);
|
||||
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
|
@ -1457,7 +1457,7 @@ void Copter::mavlink_delay_cb()
|
|||
gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
|
||||
}
|
||||
|
||||
DataFlash.EnableWrites(true);
|
||||
logger.EnableWrites(true);
|
||||
}
|
||||
|
||||
AP_AdvancedFailsafe *GCS_MAVLINK_Copter::get_advanced_failsafe() const
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
|
||||
// Code to Write and Read packets from DataFlash log memory
|
||||
// Code to Write and Read packets from AP_Logger log memory
|
||||
// Code to interact with the user to dump or erase logs
|
||||
|
||||
struct PACKED log_Control_Tuning {
|
||||
|
@ -29,7 +29,7 @@ void Copter::Log_Write_Control_Tuning()
|
|||
float terr_alt = 0.0f;
|
||||
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
||||
if (!terrain.height_above_terrain(terr_alt, true)) {
|
||||
terr_alt = DataFlash.quiet_nan();
|
||||
terr_alt = logger.quiet_nan();
|
||||
}
|
||||
#endif
|
||||
float des_alt_m = 0.0f;
|
||||
|
@ -43,7 +43,7 @@ void Copter::Log_Write_Control_Tuning()
|
|||
if (target_rangefinder_alt_used) {
|
||||
_target_rangefinder_alt = target_rangefinder_alt * 0.01f; // cm->m
|
||||
} else {
|
||||
_target_rangefinder_alt = DataFlash.quiet_nan();
|
||||
_target_rangefinder_alt = logger.quiet_nan();
|
||||
}
|
||||
struct log_Control_Tuning pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
|
||||
|
@ -61,7 +61,7 @@ void Copter::Log_Write_Control_Tuning()
|
|||
target_climb_rate : target_climb_rate_cms,
|
||||
climb_rate : climb_rate
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// Write an attitude packet
|
||||
|
@ -69,25 +69,25 @@ void Copter::Log_Write_Attitude()
|
|||
{
|
||||
Vector3f targets = attitude_control->get_att_target_euler_cd();
|
||||
targets.z = wrap_360_cd(targets.z);
|
||||
DataFlash.Log_Write_Attitude(ahrs, targets);
|
||||
DataFlash.Log_Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
|
||||
logger.Log_Write_Attitude(ahrs, targets);
|
||||
logger.Log_Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
|
||||
if (should_log(MASK_LOG_PID)) {
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDA_MSG, pos_control->get_accel_z_pid().get_pid_info() );
|
||||
logger.Log_Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDA_MSG, pos_control->get_accel_z_pid().get_pid_info() );
|
||||
}
|
||||
}
|
||||
|
||||
// Write an EKF and POS packet
|
||||
void Copter::Log_Write_EKF_POS()
|
||||
{
|
||||
DataFlash.Log_Write_EKF(ahrs);
|
||||
DataFlash.Log_Write_AHRS2(ahrs);
|
||||
logger.Log_Write_EKF(ahrs);
|
||||
logger.Log_Write_AHRS2(ahrs);
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
sitl.Log_Write_SIMSTATE();
|
||||
#endif
|
||||
DataFlash.Log_Write_POS(ahrs);
|
||||
logger.Log_Write_POS(ahrs);
|
||||
}
|
||||
|
||||
struct PACKED log_MotBatt {
|
||||
|
@ -111,7 +111,7 @@ void Copter::Log_Write_MotBatt()
|
|||
bat_res : (float)(battery.get_resistance()),
|
||||
th_limit : (float)(motors->get_throttle_limit())
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));
|
||||
logger.WriteBlock(&pkt_mot, sizeof(pkt_mot));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -130,7 +130,7 @@ void Copter::Log_Write_Event(uint8_t id)
|
|||
time_us : AP_HAL::micros64(),
|
||||
id : id
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -152,7 +152,7 @@ void Copter::Log_Write_Data(uint8_t id, int16_t value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -174,7 +174,7 @@ void Copter::Log_Write_Data(uint8_t id, uint16_t value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -195,7 +195,7 @@ void Copter::Log_Write_Data(uint8_t id, int32_t value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -216,7 +216,7 @@ void Copter::Log_Write_Data(uint8_t id, uint32_t value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -238,7 +238,7 @@ void Copter::Log_Write_Data(uint8_t id, float value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -258,7 +258,7 @@ void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
|
|||
sub_system : sub_system,
|
||||
error_code : error_code,
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_ParameterTuning {
|
||||
|
@ -283,7 +283,7 @@ void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t
|
|||
tuning_high : tune_high
|
||||
};
|
||||
|
||||
DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune));
|
||||
logger.WriteBlock(&pkt_tune, sizeof(pkt_tune));
|
||||
}
|
||||
|
||||
// logs when baro or compass becomes unhealthy
|
||||
|
@ -325,7 +325,7 @@ void Copter::Log_Write_Heli()
|
|||
desired_rotor_speed : motors->get_desired_rotor_speed(),
|
||||
main_rotor_speed : motors->get_main_rotor_speed(),
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt_heli, sizeof(pkt_heli));
|
||||
logger.WriteBlock(&pkt_heli, sizeof(pkt_heli));
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -379,7 +379,7 @@ void Copter::Log_Write_Precland()
|
|||
ekf_outcount : precland.ekf_outlier_count(),
|
||||
estimator : precland.estimator_type()
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
#endif // PRECISION_LANDING == ENABLED
|
||||
}
|
||||
|
||||
|
@ -410,11 +410,11 @@ void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_tar
|
|||
vel_target_y : vel_target.y,
|
||||
vel_target_z : vel_target.z
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// type and unit information can be found in
|
||||
// libraries/DataFlash/Logstructure.h; search for "log_Units" for
|
||||
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
|
||||
// units and "Format characters" for field type information
|
||||
const struct LogStructure Copter::log_structure[] = {
|
||||
LOG_COMMON_STRUCTURES,
|
||||
|
@ -452,20 +452,20 @@ const struct LogStructure Copter::log_structure[] = {
|
|||
|
||||
void Copter::Log_Write_Vehicle_Startup_Messages()
|
||||
{
|
||||
// only 200(?) bytes are guaranteed by DataFlash
|
||||
DataFlash.Log_Write_MessageF("Frame: %s", get_frame_string());
|
||||
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
// only 200(?) bytes are guaranteed by AP_Logger
|
||||
logger.Log_Write_MessageF("Frame: %s", get_frame_string());
|
||||
logger.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
#if AC_RALLY
|
||||
DataFlash.Log_Write_Rally(rally);
|
||||
logger.Log_Write_Rally(rally);
|
||||
#endif
|
||||
ahrs.Log_Write_Home_And_Origin();
|
||||
gps.Write_DataFlash_Log_Startup_messages();
|
||||
gps.Write_AP_Logger_Log_Startup_messages();
|
||||
}
|
||||
|
||||
|
||||
void Copter::log_init(void)
|
||||
{
|
||||
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
logger.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
}
|
||||
|
||||
#else // LOGGING_ENABLED
|
||||
|
|
|
@ -577,8 +577,8 @@ const AP_Param::Info Copter::var_info[] = {
|
|||
#endif
|
||||
|
||||
// @Group: LOG
|
||||
// @Path: ../libraries/DataFlash/DataFlash.cpp
|
||||
GOBJECT(DataFlash, "LOG", DataFlash_Class),
|
||||
// @Path: ../libraries/AP_Logger/AP_Logger.cpp
|
||||
GOBJECT(logger, "LOG", AP_Logger),
|
||||
|
||||
// @Group: BATT
|
||||
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
|
||||
|
|
|
@ -358,7 +358,7 @@ public:
|
|||
k_param_rpm_sensor,
|
||||
k_param_autotune_min_d, // remove
|
||||
k_param_arming, // 252 - AP_Arming
|
||||
k_param_DataFlash = 253, // 253 - Logging Group
|
||||
k_param_logger = 253, // 253 - Logging Group
|
||||
|
||||
// 254,255: reserved
|
||||
|
||||
|
|
|
@ -89,7 +89,7 @@ bool Copter::set_home(const Location& loc, bool lock)
|
|||
if (should_log(MASK_LOG_CMD)) {
|
||||
AP_Mission::Mission_Command temp_cmd;
|
||||
if (mode_auto.mission.read_cmd_from_storage(0, temp_cmd)) {
|
||||
DataFlash.Log_Write_Mission_Cmd(mode_auto.mission, temp_cmd);
|
||||
logger.Log_Write_Mission_Cmd(mode_auto.mission, temp_cmd);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -50,7 +50,7 @@ void Copter::crash_check()
|
|||
// log an error in the dataflash
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
|
||||
// keep logging even if disarmed:
|
||||
DataFlash_Class::instance()->set_force_log_disarmed(true);
|
||||
AP_Logger::instance()->set_force_log_disarmed(true);
|
||||
// send message to gcs
|
||||
gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
|
||||
// disarm motors
|
||||
|
|
|
@ -213,7 +213,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
|
|||
flightmode = new_flightmode;
|
||||
control_mode = mode;
|
||||
control_mode_reason = reason;
|
||||
DataFlash.Log_Write_Mode(control_mode, reason);
|
||||
logger.Log_Write_Mode(control_mode, reason);
|
||||
|
||||
#if ADSB_ENABLED == ENABLED
|
||||
adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));
|
||||
|
|
|
@ -379,7 +379,7 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
{
|
||||
// To-Do: logging when new commands start/end
|
||||
if (copter.should_log(MASK_LOG_CMD)) {
|
||||
copter.DataFlash.Log_Write_Mission_Cmd(mission, cmd);
|
||||
copter.logger.Log_Write_Mission_Cmd(mission, cmd);
|
||||
}
|
||||
|
||||
switch(cmd.id) {
|
||||
|
|
|
@ -114,9 +114,9 @@ void Copter::AutoTune::init_z_limits()
|
|||
|
||||
void Copter::AutoTune::log_pids()
|
||||
{
|
||||
copter.DataFlash.Log_Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
copter.DataFlash.Log_Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
copter.DataFlash.Log_Write_PID(LOG_PIDY_MSG, copter.attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
copter.logger.Log_Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
copter.logger.Log_Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
copter.logger.Log_Write_PID(LOG_PIDY_MSG, copter.attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -211,7 +211,7 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic
|
|||
bf_angles.y = constrain_float(bf_angles.y, -copter.aparm.angle_max, copter.aparm.angle_max);
|
||||
|
||||
if (log_counter++ % 20 == 0) {
|
||||
DataFlash_Class::instance()->Log_Write("FHLD", "TimeUS,SFx,SFy,Ax,Ay,Qual,Ix,Iy", "Qfffffff",
|
||||
AP_Logger::instance()->Log_Write("FHLD", "TimeUS,SFx,SFy,Ax,Ay,Qual,Ix,Iy", "Qfffffff",
|
||||
AP_HAL::micros64(),
|
||||
(double)sensor_flow.x, (double)sensor_flow.y,
|
||||
(double)bf_angles.x, (double)bf_angles.y,
|
||||
|
@ -511,7 +511,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
|
|||
// new height estimate for logging
|
||||
height_estimate = ins_height + height_offset;
|
||||
|
||||
DataFlash_Class::instance()->Log_Write("FXY", "TimeUS,DFx,DFy,DVx,DVy,Hest,DH,Hofs,InsH,LastInsH,DTms", "QfffffffffI",
|
||||
AP_Logger::instance()->Log_Write("FXY", "TimeUS,DFx,DFy,DVx,DVy,Hest,DH,Hofs,InsH,LastInsH,DTms", "QfffffffffI",
|
||||
AP_HAL::micros64(),
|
||||
(double)delta_flowrate.x,
|
||||
(double)delta_flowrate.y,
|
||||
|
|
|
@ -194,7 +194,7 @@ void Copter::ModeThrow::run()
|
|||
const bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
|
||||
const bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
|
||||
const bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
|
||||
DataFlash_Class::instance()->Log_Write(
|
||||
AP_Logger::instance()->Log_Write(
|
||||
"THRO",
|
||||
"TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk",
|
||||
"s-nnoo----",
|
||||
|
|
|
@ -157,7 +157,7 @@ bool Copter::init_arm_motors(const AP_Arming::ArmingMethod method, const bool do
|
|||
}
|
||||
|
||||
// let dataflash know that we're armed (it may open logs e.g.)
|
||||
DataFlash_Class::instance()->set_vehicle_armed(true);
|
||||
AP_Logger::instance()->set_vehicle_armed(true);
|
||||
|
||||
// disable cpu failsafe because initialising everything takes a while
|
||||
failsafe_disable();
|
||||
|
@ -219,7 +219,7 @@ bool Copter::init_arm_motors(const AP_Arming::ArmingMethod method, const bool do
|
|||
Log_Write_Event(DATA_ARMED);
|
||||
|
||||
// log flight mode in case it was changed while vehicle was disarmed
|
||||
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
logger.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
|
||||
// reenable failsafe
|
||||
failsafe_enable();
|
||||
|
@ -285,7 +285,7 @@ void Copter::init_disarm_motors()
|
|||
mode_auto.mission.reset();
|
||||
#endif
|
||||
|
||||
DataFlash_Class::instance()->set_vehicle_armed(false);
|
||||
AP_Logger::instance()->set_vehicle_armed(false);
|
||||
|
||||
// disable gps velocity based centrefugal force compensation
|
||||
ahrs.set_correct_centrifugal(false);
|
||||
|
|
|
@ -27,7 +27,7 @@ void Copter::read_rangefinder(void)
|
|||
|
||||
if (rangefinder.num_sensors() > 0 &&
|
||||
should_log(MASK_LOG_CTUN)) {
|
||||
DataFlash.Log_Write_RFND(rangefinder);
|
||||
logger.Log_Write_RFND(rangefinder);
|
||||
}
|
||||
|
||||
rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX));
|
||||
|
@ -79,7 +79,7 @@ void Copter::rpm_update(void)
|
|||
rpm_sensor.update();
|
||||
if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
|
||||
if (should_log(MASK_LOG_RCIN)) {
|
||||
DataFlash.Log_Write_RPM(rpm_sensor);
|
||||
logger.Log_Write_RPM(rpm_sensor);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -216,7 +216,7 @@ void Copter::update_sensor_status_flags(void)
|
|||
if (ap.rc_receiver_present) {
|
||||
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
|
||||
}
|
||||
if (copter.DataFlash.logging_present()) { // primary logging only (usually File)
|
||||
if (copter.logger.logging_present()) { // primary logging only (usually File)
|
||||
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
#if PROXIMITY_ENABLED == ENABLED
|
||||
|
@ -277,7 +277,7 @@ void Copter::update_sensor_status_flags(void)
|
|||
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
|
||||
}
|
||||
|
||||
if (copter.DataFlash.logging_enabled()) {
|
||||
if (copter.logger.logging_enabled()) {
|
||||
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
|
||||
|
@ -338,7 +338,7 @@ void Copter::update_sensor_status_flags(void)
|
|||
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
|
||||
}
|
||||
|
||||
if (copter.DataFlash.logging_failed()) {
|
||||
if (copter.logger.logging_failed()) {
|
||||
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
|
||||
|
@ -418,7 +418,7 @@ void Copter::update_visual_odom()
|
|||
visual_odom_last_update_ms,
|
||||
g2.visual_odom.get_pos_offset());
|
||||
// log sensor data
|
||||
DataFlash.Log_Write_VisualOdom(time_delta_sec,
|
||||
logger.Log_Write_VisualOdom(time_delta_sec,
|
||||
g2.visual_odom.get_angle_delta(),
|
||||
g2.visual_odom.get_position_delta(),
|
||||
g2.visual_odom.get_confidence());
|
||||
|
|
|
@ -239,8 +239,8 @@ void Copter::init_ardupilot()
|
|||
g2.smart_rtl.init();
|
||||
#endif
|
||||
|
||||
// initialise DataFlash library
|
||||
DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void));
|
||||
// initialise AP_Logger library
|
||||
logger.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void));
|
||||
|
||||
// initialise rc channels including setting mode
|
||||
rc().init();
|
||||
|
@ -408,8 +408,8 @@ void Copter::update_auto_armed()
|
|||
bool Copter::should_log(uint32_t mask)
|
||||
{
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
ap.logging_started = DataFlash.logging_started();
|
||||
return DataFlash.should_log(mask);
|
||||
ap.logging_started = logger.logging_started();
|
||||
return logger.should_log(mask);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
|
|
@ -991,7 +991,7 @@ void ToyMode::thrust_limiting(float *thrust, uint8_t num_motors)
|
|||
uint16_t pwm[4];
|
||||
hal.rcout->read(pwm, 4);
|
||||
if (motor_log_counter++ % 10 == 0) {
|
||||
DataFlash_Class::instance()->Log_Write("THST", "TimeUS,Vol,Mul,M1,M2,M3,M4", "QffHHHH",
|
||||
AP_Logger::instance()->Log_Write("THST", "TimeUS,Vol,Mul,M1,M2,M3,M4", "QffHHHH",
|
||||
AP_HAL::micros64(),
|
||||
(double)filtered_voltage,
|
||||
(double)thrust_mul,
|
||||
|
|
|
@ -85,7 +85,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
|
|||
#endif // AP_TERRAIN_AVAILABLE
|
||||
SCHED_TASK(update_is_flying_5Hz, 5, 100),
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
SCHED_TASK_CLASS(DataFlash_Class, &plane.DataFlash, periodic_tasks, 50, 400),
|
||||
SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400),
|
||||
#endif
|
||||
SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50),
|
||||
SCHED_TASK(avoidance_adsb_update, 10, 100),
|
||||
|
@ -130,7 +130,7 @@ void Plane::update_soft_armed()
|
|||
{
|
||||
hal.util->set_soft_armed(arming.is_armed() &&
|
||||
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
||||
DataFlash.set_vehicle_armed(hal.util->get_soft_armed());
|
||||
logger.set_vehicle_armed(hal.util->get_soft_armed());
|
||||
}
|
||||
|
||||
// update AHRS system
|
||||
|
@ -148,7 +148,7 @@ void Plane::ahrs_update()
|
|||
ahrs.update();
|
||||
|
||||
if (should_log(MASK_LOG_IMU)) {
|
||||
DataFlash.Log_Write_IMU();
|
||||
logger.Log_Write_IMU();
|
||||
}
|
||||
|
||||
// calculate a scaled roll limit based on current pitch
|
||||
|
@ -195,7 +195,7 @@ void Plane::update_compass(void)
|
|||
if (g.compass_enabled && compass.read()) {
|
||||
ahrs.set_compass(&compass);
|
||||
if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
|
||||
DataFlash.Log_Write_Compass();
|
||||
logger.Log_Write_Compass();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -210,10 +210,10 @@ void Plane::update_logging1(void)
|
|||
}
|
||||
|
||||
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU))
|
||||
DataFlash.Log_Write_IMU();
|
||||
logger.Log_Write_IMU();
|
||||
|
||||
if (should_log(MASK_LOG_ATTITUDE_MED))
|
||||
DataFlash.Log_Write_AOA_SSA(ahrs);
|
||||
logger.Log_Write_AOA_SSA(ahrs);
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -231,7 +231,7 @@ void Plane::update_logging2(void)
|
|||
Log_Write_RC();
|
||||
|
||||
if (should_log(MASK_LOG_IMU))
|
||||
DataFlash.Log_Write_Vibration();
|
||||
logger.Log_Write_Vibration();
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -344,7 +344,7 @@ void NOINLINE Plane::send_rpm(mavlink_channel_t chan)
|
|||
}
|
||||
|
||||
// sends a single pid info over the provided channel
|
||||
void Plane::send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info,
|
||||
void Plane::send_pid_info(const mavlink_channel_t chan, const AP_Logger::PID_Info *pid_info,
|
||||
const uint8_t axis, const float achieved)
|
||||
{
|
||||
if (pid_info == nullptr) {
|
||||
|
@ -368,7 +368,7 @@ void Plane::send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::P
|
|||
void Plane::send_pid_tuning(mavlink_channel_t chan)
|
||||
{
|
||||
const Vector3f &gyro = ahrs.get_gyro();
|
||||
const DataFlash_Class::PID_Info *pid_info;
|
||||
const AP_Logger::PID_Info *pid_info;
|
||||
if (g.gcs_pid_mask & TUNING_BITS_ROLL) {
|
||||
if (quadplane.in_vtol_mode()) {
|
||||
pid_info = &quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
|
||||
|
@ -1272,7 +1272,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
|
|||
case MAVLINK_MSG_ID_RADIO:
|
||||
case MAVLINK_MSG_ID_RADIO_STATUS:
|
||||
{
|
||||
handle_radio_status(msg, plane.DataFlash, plane.should_log(MASK_LOG_PM));
|
||||
handle_radio_status(msg, plane.logger, plane.should_log(MASK_LOG_PM));
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -1478,7 +1478,7 @@ void Plane::mavlink_delay_cb()
|
|||
static uint32_t last_1hz, last_50hz, last_5s;
|
||||
if (!gcs().chan(0).initialised) return;
|
||||
|
||||
DataFlash.EnableWrites(false);
|
||||
logger.EnableWrites(false);
|
||||
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
|
@ -1497,7 +1497,7 @@ void Plane::mavlink_delay_cb()
|
|||
gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
|
||||
}
|
||||
|
||||
DataFlash.EnableWrites(true);
|
||||
logger.EnableWrites(true);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -21,31 +21,31 @@ void Plane::Log_Write_Attitude(void)
|
|||
// we need the attitude targets from the AC_AttitudeControl controller, as they
|
||||
// account for the acceleration limits
|
||||
targets = quadplane.attitude_control->get_att_target_euler_cd();
|
||||
DataFlash.Log_Write_AttitudeView(*quadplane.ahrs_view, targets);
|
||||
logger.Log_Write_AttitudeView(*quadplane.ahrs_view, targets);
|
||||
} else {
|
||||
DataFlash.Log_Write_Attitude(ahrs, targets);
|
||||
logger.Log_Write_Attitude(ahrs, targets);
|
||||
}
|
||||
if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) {
|
||||
// log quadplane PIDs separately from fixed wing PIDs
|
||||
DataFlash.Log_Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIQA_MSG, quadplane.pos_control->get_accel_z_pid().get_pid_info() );
|
||||
logger.Log_Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIQA_MSG, quadplane.pos_control->get_accel_z_pid().get_pid_info() );
|
||||
}
|
||||
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info());
|
||||
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
DataFlash.Log_Write_EKF(ahrs);
|
||||
DataFlash.Log_Write_AHRS2(ahrs);
|
||||
logger.Log_Write_EKF(ahrs);
|
||||
logger.Log_Write_AHRS2(ahrs);
|
||||
#endif
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
sitl.Log_Write_SIMSTATE();
|
||||
#endif
|
||||
DataFlash.Log_Write_POS(ahrs);
|
||||
logger.Log_Write_POS(ahrs);
|
||||
}
|
||||
|
||||
// do logging at loop rate
|
||||
|
@ -72,7 +72,7 @@ void Plane::Log_Write_Startup(uint8_t type)
|
|||
startup_type : type,
|
||||
command_total : mission.num_commands()
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Control_Tuning {
|
||||
|
@ -106,7 +106,7 @@ void Plane::Log_Write_Control_Tuning()
|
|||
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand(),
|
||||
airspeed_estimate : est_airspeed
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Nav_Tuning {
|
||||
|
@ -143,7 +143,7 @@ void Plane::Log_Write_Nav_Tuning()
|
|||
target_alt : next_WP_loc.alt,
|
||||
target_airspeed : target_airspeed_cm,
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Status {
|
||||
|
@ -174,7 +174,7 @@ void Plane::Log_Write_Status()
|
|||
,impact : crash_state.impact_detected
|
||||
};
|
||||
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
struct PACKED log_Sonar {
|
||||
|
@ -202,9 +202,9 @@ void Plane::Log_Write_Sonar()
|
|||
count : rangefinder_state.in_range_count,
|
||||
correction : rangefinder_state.correction
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
DataFlash.Log_Write_RFND(rangefinder);
|
||||
logger.Log_Write_RFND(rangefinder);
|
||||
}
|
||||
|
||||
struct PACKED log_Arm_Disarm {
|
||||
|
@ -221,7 +221,7 @@ void Plane::Log_Arm_Disarm() {
|
|||
arm_state : arming.is_armed(),
|
||||
arm_checks : arming.get_enabled_checks()
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
|
||||
|
@ -247,21 +247,21 @@ void Plane::Log_Write_AETR()
|
|||
,flap : SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto)
|
||||
};
|
||||
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
void Plane::Log_Write_RC(void)
|
||||
{
|
||||
DataFlash.Log_Write_RCIN();
|
||||
DataFlash.Log_Write_RCOUT();
|
||||
logger.Log_Write_RCIN();
|
||||
logger.Log_Write_RCOUT();
|
||||
if (rssi.enabled()) {
|
||||
DataFlash.Log_Write_RSSI(rssi);
|
||||
logger.Log_Write_RSSI(rssi);
|
||||
}
|
||||
Log_Write_AETR();
|
||||
}
|
||||
|
||||
// type and unit information can be found in
|
||||
// libraries/DataFlash/Logstructure.h; search for "log_Units" for
|
||||
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
|
||||
// units and "Format characters" for field type information
|
||||
const struct LogStructure Plane::log_structure[] = {
|
||||
LOG_COMMON_STRUCTURES,
|
||||
|
@ -297,12 +297,12 @@ const struct LogStructure Plane::log_structure[] = {
|
|||
|
||||
void Plane::Log_Write_Vehicle_Startup_Messages()
|
||||
{
|
||||
// only 200(?) bytes are guaranteed by DataFlash
|
||||
// only 200(?) bytes are guaranteed by AP_Logger
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
DataFlash.Log_Write_Rally(rally);
|
||||
logger.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
logger.Log_Write_Rally(rally);
|
||||
ahrs.Log_Write_Home_And_Origin();
|
||||
gps.Write_DataFlash_Log_Startup_messages();
|
||||
gps.Write_AP_Logger_Log_Startup_messages();
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -310,7 +310,7 @@ void Plane::Log_Write_Vehicle_Startup_Messages()
|
|||
*/
|
||||
void Plane::log_init(void)
|
||||
{
|
||||
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
logger.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
}
|
||||
|
||||
#else // LOGGING_ENABLED
|
||||
|
|
|
@ -1018,8 +1018,8 @@ const AP_Param::Info Plane::var_info[] = {
|
|||
#endif
|
||||
|
||||
// @Group: LOG
|
||||
// @Path: ../libraries/DataFlash/DataFlash.cpp
|
||||
GOBJECT(DataFlash, "LOG", DataFlash_Class),
|
||||
// @Path: ../libraries/AP_Logger/AP_Logger.cpp
|
||||
GOBJECT(logger, "LOG", AP_Logger),
|
||||
|
||||
// @Group: BATT
|
||||
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
|
||||
|
|
|
@ -342,7 +342,7 @@ public:
|
|||
k_param_mixing_offset,
|
||||
k_param_dspoiler_rud_rate,
|
||||
|
||||
k_param_DataFlash = 253, // Logging Group
|
||||
k_param_logger = 253, // Logging Group
|
||||
|
||||
// 254,255: reserved
|
||||
};
|
||||
|
|
|
@ -24,7 +24,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
|||
constructor for main Plane class
|
||||
*/
|
||||
Plane::Plane(void)
|
||||
: DataFlash(g.log_bitmask)
|
||||
: logger(g.log_bitmask)
|
||||
{
|
||||
// C++11 doesn't allow in-class initialisation of bitfields
|
||||
auto_state.takeoff_complete = true;
|
||||
|
|
|
@ -58,7 +58,7 @@
|
|||
#include <AP_SerialManager/AP_SerialManager.h> // Serial manager library
|
||||
#include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
|
||||
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
|
||||
#include <AP_Scheduler/PerfInfo.h> // loop perf monitoring
|
||||
|
||||
|
@ -196,7 +196,7 @@ private:
|
|||
// notification object for LEDs, buzzers etc (parameter set to false disables external leds)
|
||||
AP_Notify notify;
|
||||
|
||||
DataFlash_Class DataFlash;
|
||||
AP_Logger logger;
|
||||
|
||||
// scaled roll limit based on pitch
|
||||
int32_t roll_limit_cd;
|
||||
|
@ -808,7 +808,7 @@ private:
|
|||
void send_nav_controller_output(mavlink_channel_t chan);
|
||||
void send_servo_out(mavlink_channel_t chan);
|
||||
void send_wind(mavlink_channel_t chan);
|
||||
void send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info, const uint8_t axis, const float achieved);
|
||||
void send_pid_info(const mavlink_channel_t chan, const AP_Logger::PID_Info *pid_info, const uint8_t axis, const float achieved);
|
||||
void send_pid_tuning(mavlink_channel_t chan);
|
||||
void send_rpm(mavlink_channel_t chan);
|
||||
|
||||
|
|
|
@ -10,7 +10,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
// log when new commands start
|
||||
if (should_log(MASK_LOG_CMD)) {
|
||||
DataFlash.Log_Write_Mission_Cmd(mission, cmd);
|
||||
logger.Log_Write_Mission_Cmd(mission, cmd);
|
||||
}
|
||||
|
||||
// special handling for nav vs non-nav commands
|
||||
|
@ -337,7 +337,7 @@ void Plane::do_RTL(int32_t rtl_altitude)
|
|||
setup_glide_slope();
|
||||
setup_turn_angle();
|
||||
|
||||
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
logger.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -51,7 +51,7 @@ void QAutoTune::Log_Write_Event(enum at_event id)
|
|||
{
|
||||
// offset of 30 aligned with ArduCopter autotune events
|
||||
uint8_t ev_id = 30 + (uint8_t)id;
|
||||
DataFlash_Class::instance()->Log_Write(
|
||||
AP_Logger::instance()->Log_Write(
|
||||
"EVT",
|
||||
"TimeUS,Id",
|
||||
"s-",
|
||||
|
@ -64,9 +64,9 @@ void QAutoTune::Log_Write_Event(enum at_event id)
|
|||
// log VTOL PIDs for during twitch
|
||||
void QAutoTune::log_pids(void)
|
||||
{
|
||||
DataFlash_Class::instance()->Log_Write_PID(LOG_PIQR_MSG, plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
DataFlash_Class::instance()->Log_Write_PID(LOG_PIQP_MSG, plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
DataFlash_Class::instance()->Log_Write_PID(LOG_PIQY_MSG, plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
AP_Logger::instance()->Log_Write_PID(LOG_PIQR_MSG, plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
AP_Logger::instance()->Log_Write_PID(LOG_PIQP_MSG, plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
AP_Logger::instance()->Log_Write_PID(LOG_PIQY_MSG, plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
}
|
||||
|
||||
#endif // QAUTOTUNE_ENABLED
|
||||
|
|
|
@ -1643,7 +1643,7 @@ void QuadPlane::motors_output(bool run_rate_controller)
|
|||
|
||||
motors->output();
|
||||
if (motors->armed() && motors->get_throttle() > 0) {
|
||||
plane.DataFlash.Log_Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
|
||||
plane.logger.Log_Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
|
||||
Log_Write_QControl_Tuning();
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
if (now - last_ctrl_log_ms > 100) {
|
||||
|
@ -2384,7 +2384,7 @@ void QuadPlane::Log_Write_QControl_Tuning()
|
|||
climb_rate : int16_t(inertial_nav.get_velocity_z()),
|
||||
throttle_mix : attitude_control->get_throttle_mix(),
|
||||
};
|
||||
plane.DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
plane.logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
// write multicopter position control message
|
||||
pos_control->write_log();
|
||||
|
|
|
@ -87,7 +87,7 @@ void Plane::rpm_update(void)
|
|||
rpm_sensor.update();
|
||||
if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
|
||||
if (should_log(MASK_LOG_RC)) {
|
||||
DataFlash.Log_Write_RPM(rpm_sensor);
|
||||
logger.Log_Write_RPM(rpm_sensor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -124,7 +124,7 @@ void Plane::update_sensor_status_flags(void)
|
|||
if (have_reverse_thrust()) {
|
||||
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
|
||||
}
|
||||
if (plane.DataFlash.logging_present()) { // primary logging only (usually File)
|
||||
if (plane.logger.logging_present()) { // primary logging only (usually File)
|
||||
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
|
||||
|
@ -139,7 +139,7 @@ void Plane::update_sensor_status_flags(void)
|
|||
control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
|
||||
}
|
||||
|
||||
if (plane.DataFlash.logging_enabled()) {
|
||||
if (plane.logger.logging_enabled()) {
|
||||
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
|
||||
|
@ -248,7 +248,7 @@ void Plane::update_sensor_status_flags(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
if (plane.DataFlash.logging_failed()) {
|
||||
if (plane.logger.logging_failed()) {
|
||||
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
|
||||
}
|
||||
|
||||
|
|
|
@ -247,9 +247,9 @@ void Plane::startup_ground(void)
|
|||
// initialise mission library
|
||||
mission.init();
|
||||
|
||||
// initialise DataFlash library
|
||||
// initialise AP_Logger library
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
DataFlash.setVehicle_Startup_Log_Writer(
|
||||
logger.setVehicle_Startup_Log_Writer(
|
||||
FUNCTOR_BIND(&plane, &Plane::Log_Write_Vehicle_Startup_Messages, void)
|
||||
);
|
||||
#endif
|
||||
|
@ -505,7 +505,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|||
|
||||
adsb.set_is_auto_mode(auto_navigation_mode);
|
||||
|
||||
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
logger.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
|
||||
// update notify with flight mode change
|
||||
notify_flight_mode(control_mode);
|
||||
|
@ -735,7 +735,7 @@ void Plane::notify_flight_mode(enum FlightMode mode)
|
|||
bool Plane::should_log(uint32_t mask)
|
||||
{
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
return DataFlash.should_log(mask);
|
||||
return logger.should_log(mask);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
|
|
@ -49,7 +49,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
|
|||
#endif
|
||||
SCHED_TASK(ten_hz_logging_loop, 10, 350),
|
||||
SCHED_TASK(twentyfive_hz_logging, 25, 110),
|
||||
SCHED_TASK_CLASS(DataFlash_Class, &sub.DataFlash, periodic_tasks, 400, 300),
|
||||
SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300),
|
||||
SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50),
|
||||
SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75),
|
||||
#if RPM_ENABLED == ENABLED
|
||||
|
@ -173,7 +173,7 @@ void Sub::update_batt_compass()
|
|||
compass.read();
|
||||
// log compass information
|
||||
if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
|
||||
DataFlash.Log_Write_Compass();
|
||||
logger.Log_Write_Compass();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -185,28 +185,28 @@ void Sub::ten_hz_logging_loop()
|
|||
// log attitude data if we're not already logging at the higher rate
|
||||
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||
Log_Write_Attitude();
|
||||
DataFlash.Log_Write_Rate(&ahrs_view, motors, attitude_control, pos_control);
|
||||
logger.Log_Write_Rate(&ahrs_view, motors, attitude_control, pos_control);
|
||||
if (should_log(MASK_LOG_PID)) {
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDA_MSG, pos_control.get_accel_z_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDA_MSG, pos_control.get_accel_z_pid().get_pid_info());
|
||||
}
|
||||
}
|
||||
if (should_log(MASK_LOG_MOTBATT)) {
|
||||
Log_Write_MotBatt();
|
||||
}
|
||||
if (should_log(MASK_LOG_RCIN)) {
|
||||
DataFlash.Log_Write_RCIN();
|
||||
logger.Log_Write_RCIN();
|
||||
}
|
||||
if (should_log(MASK_LOG_RCOUT)) {
|
||||
DataFlash.Log_Write_RCOUT();
|
||||
logger.Log_Write_RCOUT();
|
||||
}
|
||||
if (should_log(MASK_LOG_NTUN) && mode_requires_GPS(control_mode)) {
|
||||
pos_control.write_log();
|
||||
}
|
||||
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
|
||||
DataFlash.Log_Write_Vibration();
|
||||
logger.Log_Write_Vibration();
|
||||
}
|
||||
if (should_log(MASK_LOG_CTUN)) {
|
||||
attitude_control.control_monitor_log();
|
||||
|
@ -219,18 +219,18 @@ void Sub::twentyfive_hz_logging()
|
|||
{
|
||||
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||
Log_Write_Attitude();
|
||||
DataFlash.Log_Write_Rate(&ahrs_view, motors, attitude_control, pos_control);
|
||||
logger.Log_Write_Rate(&ahrs_view, motors, attitude_control, pos_control);
|
||||
if (should_log(MASK_LOG_PID)) {
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDA_MSG, pos_control.get_accel_z_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
|
||||
logger.Log_Write_PID(LOG_PIDA_MSG, pos_control.get_accel_z_pid().get_pid_info());
|
||||
}
|
||||
}
|
||||
|
||||
// log IMU data if we're not already logging at the higher rate
|
||||
if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) {
|
||||
DataFlash.Log_Write_IMU();
|
||||
logger.Log_Write_IMU();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -320,7 +320,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan)
|
|||
{
|
||||
const Vector3f &gyro = ahrs.get_gyro();
|
||||
if (g.gcs_pid_mask & 1) {
|
||||
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_roll_pid().get_pid_info();
|
||||
const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_roll_pid().get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL,
|
||||
pid_info.desired*0.01f,
|
||||
degrees(gyro.x),
|
||||
|
@ -333,7 +333,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan)
|
|||
}
|
||||
}
|
||||
if (g.gcs_pid_mask & 2) {
|
||||
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_pitch_pid().get_pid_info();
|
||||
const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_pitch_pid().get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
|
||||
pid_info.desired*0.01f,
|
||||
degrees(gyro.y),
|
||||
|
@ -346,7 +346,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan)
|
|||
}
|
||||
}
|
||||
if (g.gcs_pid_mask & 4) {
|
||||
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_yaw_pid().get_pid_info();
|
||||
const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_yaw_pid().get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
|
||||
pid_info.desired*0.01f,
|
||||
degrees(gyro.z),
|
||||
|
@ -359,7 +359,7 @@ void Sub::send_pid_tuning(mavlink_channel_t chan)
|
|||
}
|
||||
}
|
||||
if (g.gcs_pid_mask & 8) {
|
||||
const DataFlash_Class::PID_Info &pid_info = pos_control.get_accel_z_pid().get_pid_info();
|
||||
const AP_Logger::PID_Info &pid_info = pos_control.get_accel_z_pid().get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
|
||||
pid_info.desired*0.01f,
|
||||
-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS),
|
||||
|
@ -1141,7 +1141,7 @@ void Sub::mavlink_delay_cb()
|
|||
return;
|
||||
}
|
||||
|
||||
DataFlash.EnableWrites(false);
|
||||
logger.EnableWrites(false);
|
||||
|
||||
uint32_t tnow = AP_HAL::millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
|
@ -1160,7 +1160,7 @@ void Sub::mavlink_delay_cb()
|
|||
gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
|
||||
}
|
||||
|
||||
DataFlash.EnableWrites(true);
|
||||
logger.EnableWrites(true);
|
||||
}
|
||||
|
||||
MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) {
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
|
||||
// Code to Write and Read packets from DataFlash log memory
|
||||
// Code to Write and Read packets from AP_Logger log memory
|
||||
// Code to interact with the user to dump or erase logs
|
||||
|
||||
struct PACKED log_Control_Tuning {
|
||||
|
@ -47,7 +47,7 @@ void Sub::Log_Write_Control_Tuning()
|
|||
target_climb_rate : (int16_t)pos_control.get_vel_target_z(),
|
||||
climb_rate : climb_rate
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// Write an attitude packet
|
||||
|
@ -55,14 +55,14 @@ void Sub::Log_Write_Attitude()
|
|||
{
|
||||
Vector3f targets = attitude_control.get_att_target_euler_cd();
|
||||
targets.z = wrap_360_cd(targets.z);
|
||||
DataFlash.Log_Write_Attitude(ahrs, targets);
|
||||
logger.Log_Write_Attitude(ahrs, targets);
|
||||
|
||||
DataFlash.Log_Write_EKF(ahrs);
|
||||
DataFlash.Log_Write_AHRS2(ahrs);
|
||||
logger.Log_Write_EKF(ahrs);
|
||||
logger.Log_Write_AHRS2(ahrs);
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
sitl.Log_Write_SIMSTATE();
|
||||
#endif
|
||||
DataFlash.Log_Write_POS(ahrs);
|
||||
logger.Log_Write_POS(ahrs);
|
||||
}
|
||||
|
||||
struct PACKED log_MotBatt {
|
||||
|
@ -85,7 +85,7 @@ void Sub::Log_Write_MotBatt()
|
|||
bat_res : (float)(battery.get_resistance()),
|
||||
th_limit : (float)(motors.get_throttle_limit())
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));
|
||||
logger.WriteBlock(&pkt_mot, sizeof(pkt_mot));
|
||||
}
|
||||
|
||||
struct PACKED log_Event {
|
||||
|
@ -103,7 +103,7 @@ void Sub::Log_Write_Event(uint8_t id)
|
|||
time_us : AP_HAL::micros64(),
|
||||
id : id
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -125,7 +125,7 @@ void Sub::Log_Write_Data(uint8_t id, int16_t value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -147,7 +147,7 @@ void Sub::Log_Write_Data(uint8_t id, uint16_t value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -168,7 +168,7 @@ void Sub::Log_Write_Data(uint8_t id, int32_t value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -189,7 +189,7 @@ void Sub::Log_Write_Data(uint8_t id, uint32_t value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -211,7 +211,7 @@ void Sub::Log_Write_Data(uint8_t id, float value)
|
|||
id : id,
|
||||
data_value : value
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -231,7 +231,7 @@ void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
|
|||
sub_system : sub_system,
|
||||
error_code : error_code,
|
||||
};
|
||||
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteCriticalBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// logs when baro or compass becomes unhealthy
|
||||
|
@ -276,11 +276,11 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target
|
|||
vel_target_y : vel_target.y,
|
||||
vel_target_z : vel_target.z
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// type and unit information can be found in
|
||||
// libraries/DataFlash/Logstructure.h; search for "log_Units" for
|
||||
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
|
||||
// units and "Format characters" for field type information
|
||||
const struct LogStructure Sub::log_structure[] = {
|
||||
LOG_COMMON_STRUCTURES,
|
||||
|
@ -308,16 +308,16 @@ const struct LogStructure Sub::log_structure[] = {
|
|||
|
||||
void Sub::Log_Write_Vehicle_Startup_Messages()
|
||||
{
|
||||
// only 200(?) bytes are guaranteed by DataFlash
|
||||
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
// only 200(?) bytes are guaranteed by AP_Logger
|
||||
logger.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
ahrs.Log_Write_Home_And_Origin();
|
||||
gps.Write_DataFlash_Log_Startup_messages();
|
||||
gps.Write_AP_Logger_Log_Startup_messages();
|
||||
}
|
||||
|
||||
|
||||
void Sub::log_init()
|
||||
{
|
||||
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
logger.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
}
|
||||
|
||||
#else // LOGGING_ENABLED
|
||||
|
|
|
@ -482,8 +482,8 @@ const AP_Param::Info Sub::var_info[] = {
|
|||
#endif
|
||||
|
||||
// @Group: LOG
|
||||
// @Path: ../libraries/DataFlash/DataFlash.cpp
|
||||
GOBJECT(DataFlash, "LOG", DataFlash_Class),
|
||||
// @Path: ../libraries/AP_Logger/AP_Logger.cpp
|
||||
GOBJECT(logger, "LOG", AP_Logger),
|
||||
|
||||
// @Group: BATT
|
||||
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
|
||||
|
|
|
@ -58,7 +58,7 @@ public:
|
|||
// Hardware/Software configuration
|
||||
k_param_BoardConfig = 20, // Board configuration (PX4/Linux/etc)
|
||||
k_param_scheduler, // Scheduler (for debugging/perf_info)
|
||||
k_param_DataFlash, // DataFlash Logging
|
||||
k_param_logger, // AP_Logger Logging
|
||||
k_param_serial_manager, // Serial ports, AP_SerialManager
|
||||
k_param_notify, // Notify Library, AP_Notify
|
||||
k_param_arming = 26, // Arming checks
|
||||
|
|
|
@ -24,7 +24,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
|||
constructor for main Sub class
|
||||
*/
|
||||
Sub::Sub()
|
||||
: DataFlash(g.log_bitmask),
|
||||
: logger(g.log_bitmask),
|
||||
control_mode(MANUAL),
|
||||
motors(MAIN_LOOP_RATE),
|
||||
scaleLongDown(1),
|
||||
|
|
|
@ -40,7 +40,7 @@
|
|||
// Application dependencies
|
||||
#include <AP_SerialManager/AP_SerialManager.h> // Serial manager library
|
||||
#include <AP_GPS/AP_GPS.h> // ArduPilot GPS library
|
||||
#include <DataFlash/DataFlash.h> // ArduPilot Mega Flash Memory Library
|
||||
#include <AP_Logger/AP_Logger.h> // ArduPilot Mega Flash Memory Library
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_Compass/AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
#include <AP_InertialSensor/AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
|
||||
|
@ -163,7 +163,7 @@ private:
|
|||
RC_Channel *channel_lateral;
|
||||
|
||||
// Dataflash
|
||||
DataFlash_Class DataFlash;
|
||||
AP_Logger logger;
|
||||
|
||||
AP_GPS gps;
|
||||
|
||||
|
|
|
@ -78,7 +78,7 @@ bool Sub::set_home(const Location& loc, bool lock)
|
|||
if (should_log(MASK_LOG_CMD)) {
|
||||
AP_Mission::Mission_Command temp_cmd;
|
||||
if (mission.read_cmd_from_storage(0, temp_cmd)) {
|
||||
DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
|
||||
logger.Log_Write_Mission_Cmd(mission, temp_cmd);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -9,7 +9,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
{
|
||||
// To-Do: logging when new commands start/end
|
||||
if (should_log(MASK_LOG_CMD)) {
|
||||
DataFlash.Log_Write_Mission_Cmd(mission, cmd);
|
||||
logger.Log_Write_Mission_Cmd(mission, cmd);
|
||||
}
|
||||
|
||||
const Location &target_loc = cmd.content.location;
|
||||
|
|
|
@ -70,7 +70,7 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
|
|||
|
||||
control_mode = mode;
|
||||
control_mode_reason = reason;
|
||||
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
logger.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
|
||||
// update notify object
|
||||
notify_flight_mode(control_mode);
|
||||
|
|
|
@ -26,7 +26,7 @@ bool Sub::init_arm_motors(AP_Arming::ArmingMethod method)
|
|||
}
|
||||
|
||||
// let dataflash know that we're armed (it may open logs e.g.)
|
||||
DataFlash_Class::instance()->set_vehicle_armed(true);
|
||||
AP_Logger::instance()->set_vehicle_armed(true);
|
||||
|
||||
// disable cpu failsafe because initialising everything takes a while
|
||||
mainloop_failsafe_disable();
|
||||
|
@ -69,7 +69,7 @@ bool Sub::init_arm_motors(AP_Arming::ArmingMethod method)
|
|||
Log_Write_Event(DATA_ARMED);
|
||||
|
||||
// log flight mode in case it was changed while vehicle was disarmed
|
||||
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
logger.Log_Write_Mode(control_mode, control_mode_reason);
|
||||
|
||||
// reenable failsafe
|
||||
mainloop_failsafe_enable();
|
||||
|
@ -115,7 +115,7 @@ void Sub::init_disarm_motors()
|
|||
// reset the mission
|
||||
mission.reset();
|
||||
|
||||
DataFlash_Class::instance()->set_vehicle_armed(false);
|
||||
AP_Logger::instance()->set_vehicle_armed(false);
|
||||
|
||||
// disable gps velocity based centrefugal force compensation
|
||||
ahrs.set_correct_centrifugal(false);
|
||||
|
|
|
@ -74,7 +74,7 @@ void Sub::rpm_update(void)
|
|||
rpm_sensor.update();
|
||||
if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
|
||||
if (should_log(MASK_LOG_RCIN)) {
|
||||
DataFlash.Log_Write_RPM(rpm_sensor);
|
||||
logger.Log_Write_RPM(rpm_sensor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -185,9 +185,9 @@ void Sub::init_ardupilot()
|
|||
// initialise mission library
|
||||
mission.init();
|
||||
|
||||
// initialise DataFlash library
|
||||
// initialise AP_Logger library
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void));
|
||||
logger.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void));
|
||||
#endif
|
||||
|
||||
startup_INS_ground();
|
||||
|
@ -289,8 +289,8 @@ bool Sub::optflow_position_ok()
|
|||
bool Sub::should_log(uint32_t mask)
|
||||
{
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
ap.logging_started = DataFlash.logging_started();
|
||||
return DataFlash.should_log(mask);
|
||||
ap.logging_started = logger.logging_started();
|
||||
return logger.should_log(mask);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
|
|
@ -1,4 +1,4 @@
|
|||
#include "DataFlashFileReader.h"
|
||||
#include "AP_LoggerFileReader.h"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <string.h>
|
||||
|
@ -19,11 +19,11 @@ uint64_t now() {
|
|||
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)));
|
||||
}
|
||||
|
||||
DataFlashFileReader::DataFlashFileReader() :
|
||||
AP_LoggerFileReader::AP_LoggerFileReader() :
|
||||
start_micros(now())
|
||||
{}
|
||||
|
||||
DataFlashFileReader::~DataFlashFileReader()
|
||||
AP_LoggerFileReader::~AP_LoggerFileReader()
|
||||
{
|
||||
const uint64_t micros = now();
|
||||
const uint64_t delta = micros - start_micros;
|
||||
|
@ -31,7 +31,7 @@ DataFlashFileReader::~DataFlashFileReader()
|
|||
::printf("Replay rates: %" PRIu64 " bytes/second %" PRIu64 " messages/second\n", bytes_read*1000000/delta, message_count*1000000/delta);
|
||||
}
|
||||
|
||||
bool DataFlashFileReader::open_log(const char *logfile)
|
||||
bool AP_LoggerFileReader::open_log(const char *logfile)
|
||||
{
|
||||
fd = ::open(logfile, O_RDONLY|O_CLOEXEC);
|
||||
if (fd == -1) {
|
||||
|
@ -40,14 +40,14 @@ bool DataFlashFileReader::open_log(const char *logfile)
|
|||
return true;
|
||||
}
|
||||
|
||||
ssize_t DataFlashFileReader::read_input(void *buffer, const size_t count)
|
||||
ssize_t AP_LoggerFileReader::read_input(void *buffer, const size_t count)
|
||||
{
|
||||
uint64_t ret = ::read(fd, buffer, count);
|
||||
bytes_read += ret;
|
||||
return ret;
|
||||
}
|
||||
|
||||
void DataFlashFileReader::format_type(uint16_t type, char dest[5])
|
||||
void AP_LoggerFileReader::format_type(uint16_t type, char dest[5])
|
||||
{
|
||||
const struct log_Format &f = formats[type];
|
||||
memset(dest,0,5);
|
||||
|
@ -56,12 +56,12 @@ void DataFlashFileReader::format_type(uint16_t type, char dest[5])
|
|||
}
|
||||
strncpy(dest, f.name, 4);
|
||||
}
|
||||
void DataFlashFileReader::get_packet_counts(uint64_t dest[])
|
||||
void AP_LoggerFileReader::get_packet_counts(uint64_t dest[])
|
||||
{
|
||||
memcpy(dest, packet_counts, sizeof(packet_counts));
|
||||
}
|
||||
|
||||
bool DataFlashFileReader::update(char type[5])
|
||||
bool AP_LoggerFileReader::update(char type[5])
|
||||
{
|
||||
uint8_t hdr[3];
|
||||
if (read_input(hdr, 3) != 3) {
|
||||
|
|
|
@ -1,15 +1,15 @@
|
|||
#pragma once
|
||||
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#define LOGREADER_MAX_FORMATS 255 // must be >= highest MESSAGE
|
||||
|
||||
class DataFlashFileReader
|
||||
class AP_LoggerFileReader
|
||||
{
|
||||
public:
|
||||
|
||||
DataFlashFileReader();
|
||||
~DataFlashFileReader();
|
||||
AP_LoggerFileReader();
|
||||
~AP_LoggerFileReader();
|
||||
|
||||
bool open_log(const char *logfile);
|
||||
bool update(char type[5]);
|
||||
|
|
|
@ -5,7 +5,7 @@
|
|||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
LR_MsgHandler::LR_MsgHandler(struct log_Format &_f,
|
||||
DataFlash_Class &_dataflash,
|
||||
AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec) :
|
||||
dataflash(_dataflash), last_timestamp_usec(_last_timestamp_usec),
|
||||
MsgHandler(_f) {
|
||||
|
@ -406,8 +406,8 @@ void LR_MsgHandler_PARM::process_message(uint8_t *msg)
|
|||
} else {
|
||||
// older logs can have a lot of FMT and PARM messages up the
|
||||
// front which don't have timestamps. Since in Replay we run
|
||||
// DataFlash's IO only when stop_clock is called, we can
|
||||
// overflow DataFlash's ringbuffer. This should force us to
|
||||
// AP_Logger's IO only when stop_clock is called, we can
|
||||
// overflow AP_Logger's ringbuffer. This should force us to
|
||||
// do IO:
|
||||
hal.scheduler->stop_clock(last_timestamp_usec);
|
||||
}
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
class LR_MsgHandler : public MsgHandler {
|
||||
public:
|
||||
LR_MsgHandler(struct log_Format &f,
|
||||
DataFlash_Class &_dataflash,
|
||||
AP_Logger &_dataflash,
|
||||
uint64_t &last_timestamp_usec);
|
||||
virtual void process_message(uint8_t *msg) = 0;
|
||||
|
||||
|
@ -20,7 +20,7 @@ public:
|
|||
};
|
||||
|
||||
protected:
|
||||
DataFlash_Class &dataflash;
|
||||
AP_Logger &dataflash;
|
||||
void wait_timestamp(uint32_t timestamp);
|
||||
void wait_timestamp_usec(uint64_t timestamp);
|
||||
void wait_timestamp_from_msg(uint8_t *msg);
|
||||
|
@ -34,7 +34,7 @@ protected:
|
|||
class LR_MsgHandler_AHR2 : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_AHR2(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude)
|
||||
: LR_MsgHandler(_f, _dataflash,_last_timestamp_usec),
|
||||
ahr2_attitude(_ahr2_attitude) { };
|
||||
|
@ -49,7 +49,7 @@ private:
|
|||
class LR_MsgHandler_ARM : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_ARM(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_ARM(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
|
||||
|
||||
|
@ -60,7 +60,7 @@ public:
|
|||
class LR_MsgHandler_ARSP : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_ARSP(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) :
|
||||
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { };
|
||||
|
||||
|
@ -73,7 +73,7 @@ private:
|
|||
class LR_MsgHandler_NKF1 : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_NKF1(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_NKF1(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec) :
|
||||
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
|
||||
|
||||
|
@ -84,7 +84,7 @@ public:
|
|||
class LR_MsgHandler_ATT : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_ATT(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, Vector3f &_attitude)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude)
|
||||
{ };
|
||||
|
@ -98,7 +98,7 @@ private:
|
|||
class LR_MsgHandler_CHEK : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_CHEK(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_CHEK(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, CheckState &_check_state)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
|
||||
check_state(_check_state)
|
||||
|
@ -112,7 +112,7 @@ private:
|
|||
class LR_MsgHandler_BARO : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_BARO(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec)
|
||||
{ };
|
||||
|
@ -125,7 +125,7 @@ public:
|
|||
class LR_MsgHandler_Event : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_Event(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_Event(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
|
||||
|
||||
|
@ -139,7 +139,7 @@ class LR_MsgHandler_GPS_Base : public LR_MsgHandler
|
|||
{
|
||||
|
||||
public:
|
||||
LR_MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_GPS_Base(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
|
||||
uint32_t &_ground_alt_cm)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
|
||||
|
@ -156,7 +156,7 @@ private:
|
|||
class LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_GPS(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
|
||||
uint32_t &_ground_alt_cm)
|
||||
: LR_MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec,
|
||||
|
@ -177,7 +177,7 @@ private:
|
|||
class LR_MsgHandler_GPS2 : public LR_MsgHandler_GPS_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_GPS2(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
|
||||
uint32_t &_ground_alt_cm)
|
||||
: LR_MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec,
|
||||
|
@ -193,7 +193,7 @@ class LR_MsgHandler_GPA_Base : public LR_MsgHandler
|
|||
{
|
||||
|
||||
public:
|
||||
LR_MsgHandler_GPA_Base(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_GPA_Base(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, AP_GPS &_gps)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), gps(_gps) { };
|
||||
|
||||
|
@ -208,7 +208,7 @@ private:
|
|||
class LR_MsgHandler_GPA : public LR_MsgHandler_GPA_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_GPA(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_GPA(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, AP_GPS &_gps)
|
||||
: LR_MsgHandler_GPA_Base(_f, _dataflash,_last_timestamp_usec,
|
||||
_gps), gps(_gps) { };
|
||||
|
@ -222,7 +222,7 @@ private:
|
|||
class LR_MsgHandler_GPA2 : public LR_MsgHandler_GPA_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_GPA2(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_GPA2(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, AP_GPS &_gps)
|
||||
: LR_MsgHandler_GPA_Base(_f, _dataflash, _last_timestamp_usec,
|
||||
_gps), gps(_gps) { };
|
||||
|
@ -238,7 +238,7 @@ private:
|
|||
class LR_MsgHandler_IMU_Base : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_IMU_Base(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_IMU_Base(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
uint8_t &_accel_mask, uint8_t &_gyro_mask,
|
||||
AP_InertialSensor &_ins) :
|
||||
|
@ -257,7 +257,7 @@ private:
|
|||
class LR_MsgHandler_IMU : public LR_MsgHandler_IMU_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_IMU(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_IMU(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
uint8_t &_accel_mask, uint8_t &_gyro_mask,
|
||||
AP_InertialSensor &_ins)
|
||||
|
@ -270,7 +270,7 @@ public:
|
|||
class LR_MsgHandler_IMU2 : public LR_MsgHandler_IMU_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_IMU2(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_IMU2(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
uint8_t &_accel_mask, uint8_t &_gyro_mask,
|
||||
AP_InertialSensor &_ins)
|
||||
|
@ -283,7 +283,7 @@ public:
|
|||
class LR_MsgHandler_IMU3 : public LR_MsgHandler_IMU_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_IMU3(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_IMU3(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
uint8_t &_accel_mask, uint8_t &_gyro_mask,
|
||||
AP_InertialSensor &_ins)
|
||||
|
@ -297,7 +297,7 @@ public:
|
|||
class LR_MsgHandler_IMT_Base : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_IMT_Base(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_IMT_Base(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
uint8_t &_accel_mask, uint8_t &_gyro_mask,
|
||||
bool &_use_imt,
|
||||
|
@ -319,7 +319,7 @@ private:
|
|||
class LR_MsgHandler_IMT : public LR_MsgHandler_IMT_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_IMT(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_IMT(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
uint8_t &_accel_mask, uint8_t &_gyro_mask,
|
||||
bool &_use_imt,
|
||||
|
@ -333,7 +333,7 @@ public:
|
|||
class LR_MsgHandler_IMT2 : public LR_MsgHandler_IMT_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_IMT2(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_IMT2(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
uint8_t &_accel_mask, uint8_t &_gyro_mask,
|
||||
bool &_use_imt,
|
||||
|
@ -347,7 +347,7 @@ public:
|
|||
class LR_MsgHandler_IMT3 : public LR_MsgHandler_IMT_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_IMT3(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_IMT3(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
uint8_t &_accel_mask, uint8_t &_gyro_mask,
|
||||
bool &_use_imt,
|
||||
|
@ -362,7 +362,7 @@ public:
|
|||
class LR_MsgHandler_MAG_Base : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_MAG_Base(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, Compass &_compass)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), compass(_compass) { };
|
||||
|
||||
|
@ -376,7 +376,7 @@ private:
|
|||
class LR_MsgHandler_MAG : public LR_MsgHandler_MAG_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_MAG(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, Compass &_compass)
|
||||
: LR_MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
|
||||
|
||||
|
@ -386,7 +386,7 @@ public:
|
|||
class LR_MsgHandler_MAG2 : public LR_MsgHandler_MAG_Base
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_MAG2(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, Compass &_compass)
|
||||
: LR_MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
|
||||
|
||||
|
@ -398,7 +398,7 @@ public:
|
|||
class LR_MsgHandler_MSG : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_MSG(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_MSG(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) :
|
||||
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
|
||||
|
@ -416,7 +416,7 @@ private:
|
|||
class LR_MsgHandler_NTUN_Copter : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_NTUN_Copter(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec, Vector3f &_inavpos)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {};
|
||||
|
||||
|
@ -430,7 +430,7 @@ private:
|
|||
class LR_MsgHandler_PARM : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_PARM(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
const std::function<bool(const char *name, const float)>&set_parameter_callback) :
|
||||
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
|
||||
|
@ -447,7 +447,7 @@ private:
|
|||
class LR_MsgHandler_PM : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_PM(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_PM(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
|
||||
|
||||
|
@ -460,7 +460,7 @@ private:
|
|||
class LR_MsgHandler_SIM : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash,
|
||||
LR_MsgHandler_SIM(log_Format &_f, AP_Logger &_dataflash,
|
||||
uint64_t &_last_timestamp_usec,
|
||||
Vector3f &_sim_attitude)
|
||||
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
#include <AP_Compass/AP_Compass.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#include "LogReader.h"
|
||||
#include <stdio.h>
|
||||
|
@ -38,11 +38,11 @@ LogReader::LogReader(AP_AHRS &_ahrs,
|
|||
Compass &_compass,
|
||||
AP_GPS &_gps,
|
||||
AP_Airspeed &_airspeed,
|
||||
DataFlash_Class &_dataflash,
|
||||
AP_Logger &_dataflash,
|
||||
struct LogStructure *log_structure,
|
||||
uint8_t log_structure_count,
|
||||
const char **&_nottypes):
|
||||
DataFlashFileReader(),
|
||||
AP_LoggerFileReader(),
|
||||
vehicle(VehicleType::VEHICLE_UNKNOWN),
|
||||
ahrs(_ahrs),
|
||||
ins(_ins),
|
||||
|
@ -214,7 +214,7 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
|
|||
if (already_mapped) {
|
||||
continue;
|
||||
}
|
||||
if (DataFlash_Class::instance()->msg_type_in_use(n)) {
|
||||
if (AP_Logger::instance()->msg_type_in_use(n)) {
|
||||
continue;
|
||||
}
|
||||
mapped_msgid[intype] = n;
|
||||
|
@ -277,7 +277,7 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
|
|||
s.labels = f.labels;
|
||||
}
|
||||
|
||||
// emit the FMT to DataFlash:
|
||||
// emit the FMT to AP_Logger:
|
||||
struct log_Format pkt {};
|
||||
pkt.head1 = HEAD_BYTE1;
|
||||
pkt.head2 = HEAD_BYTE2;
|
||||
|
|
|
@ -1,9 +1,9 @@
|
|||
#include "VehicleType.h"
|
||||
#include "DataFlashFileReader.h"
|
||||
#include "AP_LoggerFileReader.h"
|
||||
#include "LR_MsgHandler.h"
|
||||
#include "Parameters.h"
|
||||
|
||||
class LogReader : public DataFlashFileReader
|
||||
class LogReader : public AP_LoggerFileReader
|
||||
{
|
||||
public:
|
||||
LogReader(AP_AHRS &_ahrs,
|
||||
|
@ -11,7 +11,7 @@ public:
|
|||
Compass &_compass,
|
||||
AP_GPS &_gps,
|
||||
AP_Airspeed &_airspeed,
|
||||
DataFlash_Class &_dataflash,
|
||||
AP_Logger &_dataflash,
|
||||
struct LogStructure *log_structure,
|
||||
uint8_t log_structure_count,
|
||||
const char **¬types);
|
||||
|
@ -47,7 +47,7 @@ private:
|
|||
Compass &compass;
|
||||
AP_GPS &gps;
|
||||
AP_Airspeed &airspeed;
|
||||
DataFlash_Class &dataflash;
|
||||
AP_Logger &dataflash;
|
||||
struct LogStructure *_log_structure;
|
||||
uint8_t _log_structure_count;
|
||||
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include "VehicleType.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
|
|
@ -25,7 +25,7 @@
|
|||
#endif
|
||||
|
||||
#include "LogReader.h"
|
||||
#include "DataFlashFileReader.h"
|
||||
#include "AP_LoggerFileReader.h"
|
||||
#include "Replay.h"
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
|
@ -74,8 +74,8 @@ const AP_Param::Info ReplayVehicle::var_info[] = {
|
|||
GOBJECT(compass, "COMPASS_", Compass),
|
||||
|
||||
// @Group: LOG
|
||||
// @Path: ../libraries/DataFlash/DataFlash.cpp
|
||||
GOBJECT(dataflash, "LOG", DataFlash_Class),
|
||||
// @Path: ../libraries/AP_Logger/AP_Logger.cpp
|
||||
GOBJECT(dataflash, "LOG", AP_Logger),
|
||||
|
||||
// @Group: EK3_
|
||||
// @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp
|
||||
|
@ -326,7 +326,7 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
|
|||
}
|
||||
}
|
||||
|
||||
class IMUCounter : public DataFlashFileReader {
|
||||
class IMUCounter : public AP_LoggerFileReader {
|
||||
public:
|
||||
IMUCounter() {}
|
||||
bool handle_log_format_msg(const struct log_Format &f);
|
||||
|
|
|
@ -26,7 +26,7 @@
|
|||
#include <AP_Airspeed/AP_Airspeed.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <AP_Notify/AP_Notify.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
|
@ -69,7 +69,7 @@ public:
|
|||
AP_Int32 unused; // logging is magic for Replay; this is unused
|
||||
struct LogStructure log_structure[256] = {
|
||||
};
|
||||
DataFlash_Class dataflash{unused};
|
||||
AP_Logger dataflash{unused};
|
||||
|
||||
private:
|
||||
Parameters g;
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AC_PosControl.h"
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -838,7 +838,7 @@ void AC_PosControl::write_log()
|
|||
float accel_x, accel_y;
|
||||
lean_angles_to_accel(accel_x, accel_y);
|
||||
|
||||
DataFlash_Class::instance()->Log_Write("PSC", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY",
|
||||
AP_Logger::instance()->Log_Write("PSC", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY",
|
||||
"smmmmnnnnoooo", "FBBBBBBBBBBBB", "Qffffffffffff",
|
||||
AP_HAL::micros64(),
|
||||
(double)pos_target.x,
|
||||
|
|
|
@ -24,15 +24,15 @@ void AC_AttitudeControl::control_monitor_filter_pid(float value, float &rms)
|
|||
*/
|
||||
void AC_AttitudeControl::control_monitor_update(void)
|
||||
{
|
||||
const DataFlash_Class::PID_Info &iroll = get_rate_roll_pid().get_pid_info();
|
||||
const AP_Logger::PID_Info &iroll = get_rate_roll_pid().get_pid_info();
|
||||
control_monitor_filter_pid(iroll.P + iroll.FF, _control_monitor.rms_roll_P);
|
||||
control_monitor_filter_pid(iroll.D, _control_monitor.rms_roll_D);
|
||||
|
||||
const DataFlash_Class::PID_Info &ipitch = get_rate_pitch_pid().get_pid_info();
|
||||
const AP_Logger::PID_Info &ipitch = get_rate_pitch_pid().get_pid_info();
|
||||
control_monitor_filter_pid(ipitch.P + iroll.FF, _control_monitor.rms_pitch_P);
|
||||
control_monitor_filter_pid(ipitch.D, _control_monitor.rms_pitch_D);
|
||||
|
||||
const DataFlash_Class::PID_Info &iyaw = get_rate_yaw_pid().get_pid_info();
|
||||
const AP_Logger::PID_Info &iyaw = get_rate_yaw_pid().get_pid_info();
|
||||
control_monitor_filter_pid(iyaw.P + iyaw.D + iyaw.FF, _control_monitor.rms_yaw);
|
||||
}
|
||||
|
||||
|
@ -41,7 +41,7 @@ void AC_AttitudeControl::control_monitor_update(void)
|
|||
*/
|
||||
void AC_AttitudeControl::control_monitor_log(void)
|
||||
{
|
||||
DataFlash_Class::instance()->Log_Write("CTRL", "TimeUS,RMSRollP,RMSRollD,RMSPitchP,RMSPitchD,RMSYaw", "Qfffff",
|
||||
AP_Logger::instance()->Log_Write("CTRL", "TimeUS,RMSRollP,RMSRollD,RMSPitchP,RMSPitchD,RMSYaw", "Qfffff",
|
||||
AP_HAL::micros64(),
|
||||
(double)safe_sqrt(_control_monitor.rms_roll_P),
|
||||
(double)safe_sqrt(_control_monitor.rms_roll_D),
|
||||
|
|
|
@ -661,7 +661,7 @@ void AC_AutoTune::control_attitude()
|
|||
|
||||
// log this iterations lean angle and rotation rate
|
||||
Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
|
||||
DataFlash_Class::instance()->Log_Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
|
||||
AP_Logger::instance()->Log_Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
|
||||
log_pids();
|
||||
break;
|
||||
}
|
||||
|
@ -1696,7 +1696,7 @@ void AC_AutoTune::get_poshold_attitude(int32_t &roll_cd_out, int32_t &pitch_cd_o
|
|||
// Write an Autotune data packet
|
||||
void AC_AutoTune::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt)
|
||||
{
|
||||
DataFlash_Class::instance()->Log_Write(
|
||||
AP_Logger::instance()->Log_Write(
|
||||
"ATUN",
|
||||
"TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt",
|
||||
"s--ddd---o",
|
||||
|
@ -1717,7 +1717,7 @@ void AC_AutoTune::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float mea
|
|||
// Write an Autotune data packet
|
||||
void AC_AutoTune::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
|
||||
{
|
||||
DataFlash_Class::instance()->Log_Write(
|
||||
AP_Logger::instance()->Log_Write(
|
||||
"ATDE",
|
||||
"TimeUS,Angle,Rate",
|
||||
"sdk",
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
#include <AP_Param/AP_Param.h>
|
||||
#include <stdlib.h>
|
||||
#include <cmath>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#define AC_PID_FILT_HZ_DEFAULT 20.0f // default input filter frequency
|
||||
#define AC_PID_FILT_HZ_MIN 0.01f // minimum input filter frequency
|
||||
|
@ -80,7 +80,7 @@ public:
|
|||
void set_desired_rate(float desired) { _pid_info.desired = desired; }
|
||||
void set_actual_rate(float actual) { _pid_info.actual = actual; }
|
||||
|
||||
const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
|
||||
// parameter var table
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
@ -106,5 +106,5 @@ protected:
|
|||
float _input; // last input for derivative
|
||||
float _derivative; // last derivative for low-pass filter
|
||||
|
||||
DataFlash_Class::PID_Info _pid_info;
|
||||
AP_Logger::PID_Info _pid_info;
|
||||
};
|
||||
|
|
|
@ -266,7 +266,7 @@ void AP_AutoTune::check_save(void)
|
|||
*/
|
||||
void AP_AutoTune::log_param_change(float v, const char *suffix)
|
||||
{
|
||||
DataFlash_Class *dataflash = DataFlash_Class::instance();
|
||||
AP_Logger *dataflash = AP_Logger::instance();
|
||||
if (!dataflash->logging_started()) {
|
||||
return;
|
||||
}
|
||||
|
@ -327,7 +327,7 @@ void AP_AutoTune::save_gains(const ATGains &v)
|
|||
|
||||
void AP_AutoTune::write_log(float servo, float demanded, float achieved)
|
||||
{
|
||||
DataFlash_Class *dataflash = DataFlash_Class::instance();
|
||||
AP_Logger *dataflash = AP_Logger::instance();
|
||||
if (!dataflash->logging_started()) {
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
class AP_AutoTune {
|
||||
public:
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include "AP_AutoTune.h"
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
class AP_PitchController {
|
||||
|
@ -29,7 +29,7 @@ public:
|
|||
void autotune_start(void) { autotune.start(); }
|
||||
void autotune_restore(void) { autotune.stop(); }
|
||||
|
||||
const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
|
@ -47,7 +47,7 @@ private:
|
|||
uint32_t _last_t;
|
||||
float _last_out;
|
||||
|
||||
DataFlash_Class::PID_Info _pid_info;
|
||||
AP_Logger::PID_Info _pid_info;
|
||||
|
||||
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed);
|
||||
float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include "AP_AutoTune.h"
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
class AP_RollController {
|
||||
|
@ -29,7 +29,7 @@ public:
|
|||
void autotune_start(void) { autotune.start(); }
|
||||
void autotune_restore(void) { autotune.stop(); }
|
||||
|
||||
const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
|
@ -52,7 +52,7 @@ private:
|
|||
uint32_t _last_t;
|
||||
float _last_out;
|
||||
|
||||
DataFlash_Class::PID_Info _pid_info;
|
||||
AP_Logger::PID_Info _pid_info;
|
||||
|
||||
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);
|
||||
|
||||
|
|
|
@ -3,7 +3,7 @@
|
|||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
class AP_SteerController {
|
||||
public:
|
||||
|
@ -46,7 +46,7 @@ public:
|
|||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
|
||||
void set_reverse(bool reverse) {
|
||||
_reverse = reverse;
|
||||
|
@ -67,7 +67,7 @@ private:
|
|||
AP_Float _deratefactor;
|
||||
AP_Float _mindegree;
|
||||
|
||||
DataFlash_Class::PID_Info _pid_info {};
|
||||
AP_Logger::PID_Info _pid_info {};
|
||||
|
||||
AP_AHRS &_ahrs;
|
||||
|
||||
|
|
|
@ -3,7 +3,7 @@
|
|||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <cmath>
|
||||
|
||||
class AP_YawController {
|
||||
|
@ -26,7 +26,7 @@ public:
|
|||
|
||||
void reset_I();
|
||||
|
||||
const DataFlash_Class::PID_Info& get_pid_info(void) const {return _pid_info; }
|
||||
const AP_Logger::PID_Info& get_pid_info(void) const {return _pid_info; }
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
|
@ -45,7 +45,7 @@ private:
|
|||
|
||||
float _integrator;
|
||||
|
||||
DataFlash_Class::PID_Info _pid_info;
|
||||
AP_Logger::PID_Info _pid_info;
|
||||
|
||||
AP_AHRS &_ahrs;
|
||||
};
|
||||
|
|
|
@ -17,7 +17,7 @@
|
|||
#include "AP_AHRS.h"
|
||||
#include "AP_AHRS_View.h"
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -475,7 +475,7 @@ Vector2f AP_AHRS::rotate_body_to_earth2D(const Vector2f &bf) const
|
|||
// log ahrs home and EKF origin to dataflash
|
||||
void AP_AHRS::Log_Write_Home_And_Origin()
|
||||
{
|
||||
DataFlash_Class *df = DataFlash_Class::instance();
|
||||
AP_Logger *df = AP_Logger::instance();
|
||||
if (df == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -22,7 +22,7 @@
|
|||
#include <AP_Math/AP_Math.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <SRV_Channel/SRV_Channel.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <utility>
|
||||
#include "AP_Airspeed.h"
|
||||
#include "AP_Airspeed_MS4525.h"
|
||||
|
@ -428,7 +428,7 @@ void AP_Airspeed::update(bool log)
|
|||
#endif
|
||||
|
||||
if (log) {
|
||||
DataFlash_Class *_dataflash = DataFlash_Class::instance();
|
||||
AP_Logger *_dataflash = AP_Logger::instance();
|
||||
if (_dataflash != nullptr) {
|
||||
_dataflash->Log_Write_Airspeed(*this);
|
||||
}
|
||||
|
|
|
@ -200,11 +200,11 @@ bool AP_Arming::logging_checks(bool report)
|
|||
{
|
||||
if ((checks_to_perform & ARMING_CHECK_ALL) ||
|
||||
(checks_to_perform & ARMING_CHECK_LOGGING)) {
|
||||
if (DataFlash_Class::instance()->logging_failed()) {
|
||||
if (AP_Logger::instance()->logging_failed()) {
|
||||
check_failed(ARMING_CHECK_LOGGING, report, "Logging failed");
|
||||
return false;
|
||||
}
|
||||
if (!DataFlash_Class::instance()->CardInserted()) {
|
||||
if (!AP_Logger::instance()->CardInserted()) {
|
||||
check_failed(ARMING_CHECK_LOGGING, report, "No SD card");
|
||||
return false;
|
||||
}
|
||||
|
@ -720,11 +720,11 @@ bool AP_Arming::arm_checks(ArmingMethod method)
|
|||
}
|
||||
}
|
||||
|
||||
// note that this will prepare DataFlash to start logging
|
||||
// note that this will prepare AP_Logger to start logging
|
||||
// so should be the last check to be done before arming
|
||||
if ((checks_to_perform & ARMING_CHECK_ALL) ||
|
||||
(checks_to_perform & ARMING_CHECK_LOGGING)) {
|
||||
DataFlash_Class *df = DataFlash_Class::instance();
|
||||
AP_Logger *df = AP_Logger::instance();
|
||||
df->PrepForArming();
|
||||
if (!df->logging_started()) {
|
||||
check_failed(ARMING_CHECK_LOGGING, true, "Logging not started");
|
||||
|
|
|
@ -30,7 +30,7 @@
|
|||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -1346,7 +1346,7 @@ void AP_BLHeli::read_telemetry_packet(void)
|
|||
last_telem[last_telem_esc] = td;
|
||||
last_telem[last_telem_esc].count++;
|
||||
|
||||
DataFlash_Class *df = DataFlash_Class::instance();
|
||||
AP_Logger *df = AP_Logger::instance();
|
||||
if (df && df->logging_enabled()) {
|
||||
struct log_Esc pkt = {
|
||||
LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC1_MSG+last_telem_esc)),
|
||||
|
|
|
@ -734,7 +734,7 @@ void AP_Baro::_probe_i2c_barometers(void)
|
|||
|
||||
bool AP_Baro::should_df_log() const
|
||||
{
|
||||
DataFlash_Class *instance = DataFlash_Class::instance();
|
||||
AP_Logger *instance = AP_Logger::instance();
|
||||
if (instance == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
@ -816,7 +816,7 @@ void AP_Baro::update(void)
|
|||
|
||||
// logging
|
||||
if (should_df_log() && !AP::ahrs().have_ekf_logging()) {
|
||||
DataFlash_Class::instance()->Log_Write_Baro();
|
||||
AP_Logger::instance()->Log_Write_Baro();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -27,7 +27,7 @@
|
|||
#include <stdio.h>
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#include <AP_InertialSensor/AP_InertialSensor_Invensense_registers.h>
|
||||
|
||||
|
@ -343,7 +343,7 @@ void AP_Baro_ICM20789::update()
|
|||
{
|
||||
#if BARO_ICM20789_DEBUG
|
||||
// useful for debugging
|
||||
DataFlash_Class::instance()->Log_Write("ICMB", "TimeUS,Traw,Praw,P,T", "QIIff",
|
||||
AP_Logger::instance()->Log_Write("ICMB", "TimeUS,Traw,Praw,P,T", "QIIff",
|
||||
AP_HAL::micros64(),
|
||||
dd.Traw, dd.Praw, dd.P, dd.T);
|
||||
#endif
|
||||
|
|
|
@ -12,7 +12,7 @@
|
|||
#endif
|
||||
|
||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
@ -237,7 +237,7 @@ AP_BattMonitor::read()
|
|||
}
|
||||
}
|
||||
|
||||
DataFlash_Class *df = DataFlash_Class::instance();
|
||||
AP_Logger *df = AP_Logger::instance();
|
||||
if (df->should_log(_log_battery_bit)) {
|
||||
df->Log_Write_Current();
|
||||
df->Log_Write_Power();
|
||||
|
|
|
@ -390,7 +390,7 @@ void AP_Camera::setup_feedback_callback(void)
|
|||
// log_picture - log picture taken and send feedback to GCS
|
||||
void AP_Camera::log_picture()
|
||||
{
|
||||
DataFlash_Class *df = DataFlash_Class::instance();
|
||||
AP_Logger *df = AP_Logger::instance();
|
||||
if (df == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
@ -437,7 +437,7 @@ void AP_Camera::update_trigger()
|
|||
_camera_trigger_logged = _camera_trigger_count;
|
||||
|
||||
gcs().send_message(MSG_CAMERA_FEEDBACK);
|
||||
DataFlash_Class *df = DataFlash_Class::instance();
|
||||
AP_Logger *df = AP_Logger::instance();
|
||||
if (df != nullptr) {
|
||||
if (df->should_log(log_camera_bit)) {
|
||||
uint32_t tdiff = AP_HAL::micros() - timestamp32;
|
||||
|
|
|
@ -21,7 +21,7 @@
|
|||
#include <utility>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <stdio.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
|
@ -224,7 +224,7 @@ void AP_Compass_MMC3416::timer()
|
|||
}
|
||||
|
||||
#if 0
|
||||
DataFlash_Class::instance()->Log_Write("MMO", "TimeUS,Nx,Ny,Nz,Ox,Oy,Oz", "Qffffff",
|
||||
AP_Logger::instance()->Log_Write("MMO", "TimeUS,Nx,Ny,Nz,Ox,Oy,Oz", "Qffffff",
|
||||
AP_HAL::micros64(),
|
||||
(double)new_offset.x,
|
||||
(double)new_offset.y,
|
||||
|
|
|
@ -3,7 +3,7 @@
|
|||
|
||||
#include <AP_Compass/AP_Compass.h>
|
||||
#include <AP_Declination/AP_Declination.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#include "Compass_learn.h"
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
@ -104,7 +104,7 @@ void CompassLearn::update(void)
|
|||
}
|
||||
|
||||
if (sample_available) {
|
||||
DataFlash_Class::instance()->Log_Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
|
||||
AP_Logger::instance()->Log_Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
|
||||
AP_HAL::micros64(),
|
||||
(double)best_offsets.x,
|
||||
(double)best_offsets.y,
|
||||
|
|
|
@ -307,7 +307,7 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
|
|||
}
|
||||
|
||||
// log lead's estimated vs reported position
|
||||
DataFlash_Class::instance()->Log_Write("FOLL",
|
||||
AP_Logger::instance()->Log_Write("FOLL",
|
||||
"TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE", // labels
|
||||
"sDUmnnnDUm", // units
|
||||
"F--B000--B", // mults
|
||||
|
|
|
@ -561,7 +561,7 @@ AP_GPS::GPS_Status AP_GPS::highest_supported_status(uint8_t instance) const
|
|||
|
||||
bool AP_GPS::should_df_log() const
|
||||
{
|
||||
DataFlash_Class *instance = DataFlash_Class::instance();
|
||||
AP_Logger *instance = AP_Logger::instance();
|
||||
if (instance == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
@ -657,7 +657,7 @@ void AP_GPS::update_instance(uint8_t instance)
|
|||
if (data_should_be_logged &&
|
||||
should_df_log() &&
|
||||
!AP::ahrs().have_ekf_logging()) {
|
||||
DataFlash_Class::instance()->Log_Write_GPS(instance);
|
||||
AP_Logger::instance()->Log_Write_GPS(instance);
|
||||
}
|
||||
|
||||
if (state[instance].status >= GPS_OK_FIX_3D) {
|
||||
|
@ -1086,13 +1086,13 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
void AP_GPS::Write_DataFlash_Log_Startup_messages()
|
||||
void AP_GPS::Write_AP_Logger_Log_Startup_messages()
|
||||
{
|
||||
for (uint8_t instance=0; instance<num_instances; instance++) {
|
||||
if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
|
||||
continue;
|
||||
}
|
||||
drivers[instance]->Write_DataFlash_Log_Startup_messages();
|
||||
drivers[instance]->Write_AP_Logger_Log_Startup_messages();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -409,7 +409,7 @@ public:
|
|||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
void Write_DataFlash_Log_Startup_messages();
|
||||
void Write_AP_Logger_Log_Startup_messages();
|
||||
|
||||
// indicate which bit in LOG_BITMASK indicates gps logging enabled
|
||||
void set_log_gps_bit(uint32_t bit) { _log_gps_bit = bit; }
|
||||
|
|
|
@ -22,7 +22,7 @@
|
|||
|
||||
#include "AP_GPS.h"
|
||||
#include "AP_GPS_GSOF.h"
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
#include "AP_GPS.h"
|
||||
#include "AP_GPS_NOVA.h"
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
|
|
@ -22,7 +22,7 @@
|
|||
|
||||
#include "AP_GPS.h"
|
||||
#include "AP_GPS_SBF.h"
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
@ -260,7 +260,7 @@ AP_GPS_SBF::log_ExtEventPVTGeodetic(const msg4007 &temp)
|
|||
COG:temp.COG
|
||||
};
|
||||
|
||||
DataFlash_Class::instance()->WriteBlock(&header, sizeof(header));
|
||||
AP_Logger::instance()->WriteBlock(&header, sizeof(header));
|
||||
}
|
||||
|
||||
bool
|
||||
|
|
|
@ -22,7 +22,7 @@
|
|||
|
||||
#include "AP_GPS.h"
|
||||
#include "AP_GPS_SBP.h"
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -404,7 +404,7 @@ AP_GPS_SBP::logging_log_full_update()
|
|||
last_iar_num_hypotheses : last_iar_num_hypotheses,
|
||||
};
|
||||
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
AP_Logger::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
};
|
||||
|
||||
void
|
||||
|
@ -438,7 +438,7 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
|
|||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt.data, msg_buff, MIN(msg_len, 48));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
AP_Logger::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
for (uint8_t i = 0; i < pages - 1; i++) {
|
||||
struct log_SbpRAWM pkt2 = {
|
||||
|
@ -451,7 +451,7 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
|
|||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt2.data, &msg_buff[48 + i * 104], MIN(msg_len - (48 + i * 104), 104));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
AP_Logger::instance()->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
@ -177,7 +177,7 @@ private:
|
|||
uint32_t crc_error_counter;
|
||||
|
||||
// ************************************************************************
|
||||
// Logging to DataFlash
|
||||
// Logging to AP_Logger
|
||||
// ************************************************************************
|
||||
|
||||
void logging_log_full_update();
|
||||
|
|
|
@ -22,7 +22,7 @@
|
|||
|
||||
#include "AP_GPS.h"
|
||||
#include "AP_GPS_SBP2.h"
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
|
@ -454,7 +454,7 @@ AP_GPS_SBP2::logging_log_full_update()
|
|||
last_injected_data_ms : last_injected_data_ms,
|
||||
last_iar_num_hypotheses : 0,
|
||||
};
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
AP_Logger::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
};
|
||||
|
||||
void
|
||||
|
@ -488,7 +488,7 @@ AP_GPS_SBP2::logging_log_raw_sbp(uint16_t msg_type,
|
|||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt.data, msg_buff, MIN(msg_len, 48));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
AP_Logger::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
for (uint8_t i = 0; i < pages - 1; i++) {
|
||||
struct log_SbpRAWM pkt2 = {
|
||||
|
@ -501,7 +501,7 @@ AP_GPS_SBP2::logging_log_raw_sbp(uint16_t msg_type,
|
|||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt2.data, &msg_buff[48 + i * 104], MIN(msg_len - (48 + i * 104), 104));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
AP_Logger::instance()->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -520,5 +520,5 @@ AP_GPS_SBP2::logging_ext_event() {
|
|||
level : last_event.flags.level,
|
||||
quality : last_event.flags.quality,
|
||||
};
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
AP_Logger::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
};
|
||||
|
|
|
@ -188,7 +188,7 @@ private:
|
|||
uint32_t crc_error_counter;
|
||||
|
||||
// ************************************************************************
|
||||
// Logging to DataFlash
|
||||
// Logging to AP_Logger
|
||||
// ************************************************************************
|
||||
|
||||
void logging_log_full_update();
|
||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue