Tracker: correct compilation when HAL_LOGGING_ENABLED is false

This commit is contained in:
Peter Barker 2024-01-10 15:40:09 +11:00 committed by Andrew Tridgell
parent 852944a1b1
commit dae819ba54
7 changed files with 26 additions and 17 deletions

View File

@ -1,6 +1,6 @@
#include "Tracker.h"
#if LOGGING_ENABLED == ENABLED
#if HAL_LOGGING_ENABLED
// Code to Write and Read packets from AP_Logger log memory
@ -98,13 +98,4 @@ void Tracker::log_init(void)
logger.Init(log_structure, ARRAY_SIZE(log_structure));
}
#else // LOGGING_ENABLED
void Tracker::Log_Write_Attitude(void) {}
void Tracker::log_init(void) {}
void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, const Vector3f& vel) {}
void Tracker::Log_Write_Vehicle_Baro(float pressure, float altitude) {}
void Tracker::Log_Write_Vehicle_Startup_Messages() {}
#endif // LOGGING_ENABLED
#endif // HAL_LOGGING_ENABLED

View File

@ -576,9 +576,11 @@ const AP_Param::Info Tracker::var_info[] = {
// @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp
PARAM_VEHICLE_INFO,
#if HAL_LOGGING_ENABLED
// @Group: LOG
// @Path: ../libraries/AP_Logger/AP_Logger.cpp
GOBJECT(logger, "LOG", AP_Logger),
#endif
#if HAL_NAVEKF2_AVAILABLE
// @Group: EK2_

View File

@ -59,8 +59,8 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
SCHED_TASK_CLASS(GCS, (GCS*)&tracker._gcs, update_receive, 50, 1700, 45),
SCHED_TASK_CLASS(GCS, (GCS*)&tracker._gcs, update_send, 50, 3000, 50),
SCHED_TASK_CLASS(AP_Baro, &tracker.barometer, accumulate, 50, 900, 55),
#if HAL_LOGGING_ENABLED
SCHED_TASK(ten_hz_logging_loop, 10, 300, 60),
#if LOGGING_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Logger, &tracker.logger, periodic_tasks, 50, 300, 65),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &tracker.ins, periodic, 50, 50, 70),
@ -107,6 +107,7 @@ void Tracker::one_second_loop()
g.pidYaw2Srv.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
}
#if HAL_LOGGING_ENABLED
void Tracker::ten_hz_logging_loop()
{
if (should_log(MASK_LOG_IMU)) {
@ -122,6 +123,7 @@ void Tracker::ten_hz_logging_loop()
logger.Write_RCOUT();
}
}
#endif
Mode *Tracker::mode_from_mode_num(const Mode::Number num)
{
@ -164,7 +166,9 @@ void Tracker::stats_update(void)
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
Tracker::Tracker(void)
#if HAL_LOGGING_ENABLED
: logger(g.log_bitmask)
#endif
{
}

View File

@ -79,7 +79,9 @@ private:
uint32_t start_time_ms = 0;
#if HAL_LOGGING_ENABLED
AP_Logger logger;
#endif
/**
antenna control channels

View File

@ -57,9 +57,6 @@
//
// Logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
// Default logging bitmask
#ifndef DEFAULT_LOG_BITMASK

View File

@ -32,7 +32,7 @@ void Tracker::init_ardupilot()
// try to initialise stream rates in the main loop.
gcs().update_send();
#if LOGGING_ENABLED == ENABLED
#if HAL_LOGGING_ENABLED
log_init();
#endif
@ -56,8 +56,10 @@ void Tracker::init_ardupilot()
barometer.calibrate();
#if HAL_LOGGING_ENABLED
// initialise AP_Logger library
logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&tracker, &Tracker::Log_Write_Vehicle_Startup_Messages, void));
#endif
// initialise rc channels including setting mode
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);
@ -152,13 +154,17 @@ bool Tracker::set_home(const Location &temp)
void Tracker::arm_servos()
{
hal.util->set_soft_armed(true);
#if HAL_LOGGING_ENABLED
logger.set_vehicle_armed(true);
#endif
}
void Tracker::disarm_servos()
{
hal.util->set_soft_armed(false);
#if HAL_LOGGING_ENABLED
logger.set_vehicle_armed(false);
#endif
}
/*
@ -189,8 +195,10 @@ void Tracker::set_mode(Mode &newmode, const ModeReason reason)
disarm_servos();
}
#if HAL_LOGGING_ENABLED
// log mode change
logger.Write_Mode((uint8_t)mode->number(), reason);
#endif
gcs().send_message(MSG_HEARTBEAT);
nav_status.bearing = ahrs.yaw_sensor * 0.01f;
@ -228,6 +236,7 @@ bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason)
return true;
}
#if HAL_LOGGING_ENABLED
/*
should we log a message type now?
*/
@ -238,7 +247,7 @@ bool Tracker::should_log(uint32_t mask)
}
return true;
}
#endif
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
#include <AP_Avoidance/AP_Avoidance.h>

View File

@ -138,10 +138,12 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f);
vehicle.last_update_us = AP_HAL::micros();
vehicle.last_update_ms = AP_HAL::millis();
#if HAL_LOGGING_ENABLED
// log vehicle as GPS2
if (should_log(MASK_LOG_GPS)) {
Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);
}
#endif
}
@ -167,8 +169,10 @@ void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg)
}
}
#if HAL_LOGGING_ENABLED
// log vehicle baro data
Log_Write_Vehicle_Baro(aircraft_pressure, alt_diff);
#endif
}
/**