bool AP_InertialSensor::BatchSampler::should_log(uint8_t _instance, IMU_SENSOR_TYPE _type)
{
if (_sensor_mask == 0) {
return false;
}
if (!initialised) {
return false;
}
if (_instance != instance) {
return false;
}
if (_type != type) {
return false;
}
if (data_write_offset >= _required_count) {
return false;
}
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) {
return false;
}
#define MASK_LOG_ANY 0xFFFF
if (!logger->should_log(MASK_LOG_ANY)) {
return false;
}
return true;
}
bool AP_Arming::arm_checks(AP_Arming::Method method)
{
// ensure the GPS drivers are ready on any final changes
if ((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_GPS_CONFIG)) {
if (!AP::gps().prepare_for_arming()) {
return false;
}
}
// note that this will prepare AP_Logger to start logging
// so should be the last check to be done before arming
// Note also that we need to PrepForArming() regardless of whether
// the arming check flag is set - disabling the arming check
// should not stop logging from working.
AP_Logger *logger = AP_Logger::get_singleton();
if (logger->logging_present()) {
// If we're configured to log, prep it
logger->PrepForArming();
if (!logger->logging_started() &&
((checks_to_perform & ARMING_CHECK_ALL) ||
(checks_to_perform & ARMING_CHECK_LOGGING))) {
check_failed(ARMING_CHECK_LOGGING, true, "Logging not started");
return false;
}
}
return true;
}
void SoloGimbal::write_logs()
{
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) {
return;
}
uint32_t tstamp = AP_HAL::millis();
Vector3f eulerEst;
Quaternion quatEst;
_ekf.getQuat(quatEst);
quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
struct log_Gimbal1 pkt1 = {
LOG_PACKET_HEADER_INIT(LOG_GIMBAL1_MSG),
time_ms : tstamp,
delta_time : _log_dt,
delta_angles_x : _log_del_ang.x,
delta_angles_y : _log_del_ang.y,
delta_angles_z : _log_del_ang.z,
delta_velocity_x : _log_del_vel.x,
delta_velocity_y : _log_del_vel.y,
delta_velocity_z : _log_del_vel.z,
joint_angles_x : _measurement.joint_angles.x,
joint_angles_y : _measurement.joint_angles.y,
joint_angles_z : _measurement.joint_angles.z
};
logger->WriteBlock(&pkt1, sizeof(pkt1));
struct log_Gimbal2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_GIMBAL2_MSG),
time_ms : tstamp,
est_sta : (uint8_t) _ekf.getStatus(),
est_x : eulerEst.x,
est_y : eulerEst.y,
est_z : eulerEst.z,
rate_x : _ang_vel_dem_rads.x,
rate_y : _ang_vel_dem_rads.y,
rate_z : _ang_vel_dem_rads.z,
target_x: _att_target_euler_rad.x,
target_y: _att_target_euler_rad.y,
target_z: _att_target_euler_rad.z
};
logger->WriteBlock(&pkt2, sizeof(pkt2));
_log_dt = 0;
_log_del_ang.zero();
_log_del_vel.zero();
}
/*
log a parameter change from autotune
*/
void AP_AutoTune::log_param_change(float v, const char *suffix)
{
AP_Logger *dataflash = AP_Logger::get_singleton();
if (!dataflash->logging_started()) {
return;
}
char key[AP_MAX_NAME_SIZE+1];
if (type == AUTOTUNE_ROLL) {
strncpy(key, "RLL2SRV_", 9);
strncpy(&key[8], suffix, AP_MAX_NAME_SIZE-8);
} else {
strncpy(key, "PTCH2SRV_", 10);
strncpy(&key[9], suffix, AP_MAX_NAME_SIZE-9);
}
key[AP_MAX_NAME_SIZE] = 0;
dataflash->Write_Parameter(key, v);
}
void AP_LoggerTest_AllTypes::Log_Write_TypeMessages()
{
log_num = dataflash.find_last_log();
hal.console->printf("Using log number %u\n", log_num);
struct log_TYP1 typ1 = {
LOG_PACKET_HEADER_INIT(LOG_TYP1_MSG),
time_us : AP_HAL::micros64(),
a : { -32768, 32767, 1, -1, 0, 17 }, // int16[32]
/*
handle a GIMBAL_REPORT message
*/
void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t *msg)
{
_gimbal.update_target(_angle_ef_target_rad);
_gimbal.receive_feedback(chan,msg);
AP_Logger *df = AP_Logger::get_singleton();
if (df == nullptr) {
return;
}
if(!_params_saved && df->logging_started()) {
_gimbal.fetch_params(); //last parameter save might not be stored in dataflash so retry
_params_saved = true;
}
if (_gimbal.get_log_dt() > 1.0f/25.0f) {
_gimbal.write_logs();
}
}
void AP_AutoTune::write_log(float servo, float demanded, float achieved)
{
AP_Logger *dataflash = AP_Logger::get_singleton();
if (!dataflash->logging_started()) {
return;
}
struct log_ATRP pkt = {
LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG),
time_us : AP_HAL::micros64(),
type : static_cast<uint8_t>(type),
state : (uint8_t)state,
servo : (int16_t)(servo*100),
demanded : demanded,
achieved : achieved,
P : current.P.get()
};
dataflash->WriteBlock(&pkt, sizeof(pkt));
}
// log ahrs home and EKF origin to dataflash
void AP_AHRS::Log_Write_Home_And_Origin()
{
AP_Logger *df = AP_Logger::get_singleton();
if (df == nullptr) {
return;
}
#if AP_AHRS_NAVEKF_AVAILABLE
// log ekf origin if set
Location ekf_orig;
if (get_origin(ekf_orig)) {
df->Write_Origin(LogOriginType::ekf_origin, ekf_orig);
}
#endif
// log ahrs home if set
if (home_is_set()) {
df->Write_Origin(LogOriginType::ahrs_home, _home);
}
}
// read - read the voltage and current for all instances
void
AP_BattMonitor::read()
{
for (uint8_t i=0; i<_num_instances; i++) {
if (drivers[i] != nullptr && _params[i].type() != AP_BattMonitor_Params::BattMonitor_TYPE_NONE) {
drivers[i]->read();
drivers[i]->update_resistance_estimate();
}
}
AP_Logger *df = AP_Logger::get_singleton();
if (df->should_log(_log_battery_bit)) {
df->Write_Current();
df->Write_Power();
}
check_failsafes();
checkPoweringOff();
}
void AP_LoggerTest_AllTypes::flush_dataflash(AP_Logger &_dataflash)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
_dataflash.flush();
#else
// flush is not available on e.g. px4 as it would be a somewhat
// dangerous operation, but if we wait long enough (at time of
// writing, 2 seconds, see AP_Logger_File::_io_timer) the data
// will go out.
hal.scheduler->delay(3000);
#endif
}
void OpticalFlow::Log_Write_Optflow()
{
AP_Logger *instance = AP_Logger::instance();
if (instance == nullptr) {
return;
}
if (_log_bit != (uint32_t)-1 &&
!instance->should_log(_log_bit)) {
return;
}
struct log_Optflow pkt = {
LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG),
time_us : AP_HAL::micros64(),
surface_quality : _state.surface_quality,
flow_x : _state.flowRate.x,
flow_y : _state.flowRate.y,
body_x : _state.bodyRate.x,
body_y : _state.bodyRate.y
};
instance->WriteBlock(&pkt, sizeof(pkt));
}
void AP_InertialSensor::BatchSampler::push_data_to_log()
{
if (!initialised) {
return;
}
if (_sensor_mask == 0) {
return;
}
if (data_write_offset - data_read_offset < samples_per_msg) {
// insuffucient data to pack a packet
return;
}
if (AP_HAL::millis() - last_sent_ms < (uint16_t)push_interval_ms) {
// avoid flooding AP_Logger's buffer
return;
}
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) {
// should not have been called
return;
}
// possibly send isb header:
if (!isbh_sent && data_read_offset == 0) {
float sample_rate = 0; // avoid warning about uninitialised values
switch(type) {
case IMU_SENSOR_TYPE_GYRO:
sample_rate = _imu._gyro_raw_sample_rates[instance];
if (_doing_sensor_rate_logging) {
sample_rate *= _imu._gyro_over_sampling[instance];
}
break;
case IMU_SENSOR_TYPE_ACCEL:
sample_rate = _imu._accel_raw_sample_rates[instance];
if (_doing_sensor_rate_logging) {
sample_rate *= _imu._accel_over_sampling[instance];
}
break;
}
if (!logger->Write_ISBH(isb_seqnum,
type,
instance,
multiplier,
_required_count,
measurement_started_us,
sample_rate)) {
// buffer full?
return;
}
isbh_sent = true;
}
// pack and send a data packet:
if (!logger->Write_ISBD(isb_seqnum,
data_read_offset/samples_per_msg,
&data_x[data_read_offset],
&data_y[data_read_offset],
&data_z[data_read_offset])) {
// maybe later?!
return;
}
data_read_offset += samples_per_msg;
last_sent_ms = AP_HAL::millis();
if (data_read_offset >= _required_count) {
// that was the last one. Clean up:
data_read_offset = 0;
isb_seqnum++;
isbh_sent = false;
// rotate to next instance:
rotate_to_next_sensor();
data_write_offset = 0; // unlocks writing process
}
}