mirror of https://github.com/ArduPilot/ardupilot
Tracker: correct compilation when HAL_LOGGING_ENABLED is false
This commit is contained in:
parent
852944a1b1
commit
dae819ba54
|
@ -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
|
||||
|
|
|
@ -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_
|
||||
|
|
|
@ -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
|
||||
{
|
||||
}
|
||||
|
||||
|
|
|
@ -79,7 +79,9 @@ private:
|
|||
|
||||
uint32_t start_time_ms = 0;
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
AP_Logger logger;
|
||||
#endif
|
||||
|
||||
/**
|
||||
antenna control channels
|
||||
|
|
|
@ -57,9 +57,6 @@
|
|||
//
|
||||
// Logging control
|
||||
//
|
||||
#ifndef LOGGING_ENABLED
|
||||
# define LOGGING_ENABLED ENABLED
|
||||
#endif
|
||||
|
||||
// Default logging bitmask
|
||||
#ifndef DEFAULT_LOG_BITMASK
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue