int write_version(int fd) { /* construct version message */ struct { LOG_PACKET_HEADER; struct log_VER_s body; } log_msg_VER = { LOG_PACKET_HEADER_INIT(LOG_VER_MSG), }; /* fill version message and write it */ strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); return write(fd, &log_msg_VER, sizeof(log_msg_VER)); }
// Private Methods ///////////////////////////////////////////////////////////// void AP_GPS_UBLOX::log_mon_hw(void) { if (!should_df_log()) { return; } struct log_Ubx1 pkt = { LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX1_MSG)), time_us : AP_HAL::micros64(), instance : state.instance, noisePerMS : _buffer.mon_hw_60.noisePerMS, jamInd : _buffer.mon_hw_60.jamInd, aPower : _buffer.mon_hw_60.aPower, agcCnt : _buffer.mon_hw_60.agcCnt, config : _unconfigured_messages, };
void AP_GPS_SBP::logging_log_full_update() { if (!should_df_log()) { return; } struct log_SbpHealth pkt = { LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH), time_us : AP_HAL::micros64(), crc_error_counter : crc_error_counter, last_injected_data_ms : last_injected_data_ms, last_iar_num_hypotheses : last_iar_num_hypotheses, };
// Write an Autotune data packet void Copter::ModeAutoTune::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) { struct log_AutoTune pkt = { LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG), time_us : AP_HAL::micros64(), axis : _axis, tune_step : tune_step, meas_target : meas_target, meas_min : meas_min, meas_max : meas_max, new_gain_rp : new_gain_rp, new_gain_rd : new_gain_rd, new_gain_sp : new_gain_sp, new_ddt : new_ddt };
// Write a navigation tuning packe void Plane::Log_Write_Nav_Tuning() { struct log_Nav_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG), time_us : hal.scheduler->micros64(), yaw : (uint16_t)ahrs.yaw_sensor, wp_distance : auto_state.wp_distance, target_bearing_cd : (int16_t)nav_controller->target_bearing_cd(), nav_bearing_cd : (int16_t)nav_controller->nav_bearing_cd(), altitude_error_cm : (int16_t)altitude_error_cm, airspeed_cm : (int16_t)airspeed.get_airspeed_cm(), altitude : barometer.get_altitude(), groundspeed_cm : (uint32_t)(gps.ground_speed()*100) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); }
// Write a control tuning packet. Total length : 22 bytes void Plane::Log_Write_Control_Tuning() { Vector3f accel = ins.get_accel(); struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG), time_us : hal.scheduler->micros64(), nav_roll_cd : (int16_t)nav_roll_cd, roll : (int16_t)ahrs.roll_sensor, nav_pitch_cd : (int16_t)nav_pitch_cd, pitch : (int16_t)ahrs.pitch_sensor, throttle_out : (int16_t)channel_throttle->servo_out, rudder_out : (int16_t)channel_rudder->servo_out, accel_y : accel.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); }
// Write a throttle control packet void Rover::Log_Write_Throttle() { const Vector3f accel = ins.get_accel(); float speed = DataFlash.quiet_nanf(); g2.attitude_control.get_forward_speed(speed); struct log_Throttle pkt = { LOG_PACKET_HEADER_INIT(LOG_THR_MSG), time_us : AP_HAL::micros64(), throttle_in : channel_throttle->get_control_in(), throttle_out : g2.motors.get_throttle(), desired_speed : g2.attitude_control.get_desired_speed(), speed : speed, accel_y : accel.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); }
// Write a steering packet void Rover::Log_Write_Steering() { float lat_accel = DataFlash.quiet_nanf(); g2.attitude_control.get_lat_accel(lat_accel); struct log_Steering pkt = { LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG), time_us : AP_HAL::micros64(), steering_in : channel_steer->get_control_in(), steering_out : g2.motors.get_steering(), desired_lat_accel : g2.attitude_control.get_desired_lat_accel(), lat_accel : lat_accel, desired_turn_rate : degrees(g2.attitude_control.get_desired_turn_rate()), turn_rate : degrees(ahrs.get_yaw_rate_earth()) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); }
// Write a performance monitoring packet. Total length : 19 bytes void Plane::Log_Write_Performance() { struct log_Performance pkt = { LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), time_us : hal.scheduler->micros64(), loop_time : millis() - perf_mon_timer, main_loop_count : mainLoop_count, g_dt_max : G_Dt_max, gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000), gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000), gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000), i2c_lockup_count: hal.i2c->lockup_count(), ins_error_count : ins.error_count() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); }
// Write a control tuning packet. Total length : 22 bytes void Plane::Log_Write_Control_Tuning() { float est_airspeed = 0; ahrs.airspeed_estimate(&est_airspeed); struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG), time_us : AP_HAL::micros64(), nav_roll_cd : (int16_t)nav_roll_cd, roll : (int16_t)ahrs.roll_sensor, nav_pitch_cd : (int16_t)nav_pitch_cd, pitch : (int16_t)ahrs.pitch_sensor, throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), rudder_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_rudder), throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand(), airspeed_estimate : est_airspeed };
void AP_GPS_UBLOX::log_mon_hw2(void) { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { return; } struct log_Ubx2 pkt = { LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX2_MSG)), time_us : AP_HAL::micros64(), instance : state.instance, ofsI : _buffer.mon_hw2.ofsI, magI : _buffer.mon_hw2.magI, ofsQ : _buffer.mon_hw2.ofsQ, magQ : _buffer.mon_hw2.magQ, }; gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); }
void AP_GPS_UBLOX::log_mon_hw2(void) { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { return; } struct log_Ubx2 pkt = { LOG_PACKET_HEADER_INIT(LOG_UBX2_MSG), timestamp : hal.scheduler->millis(), instance : state.instance, ofsI : _buffer.mon_hw2.ofsI, magI : _buffer.mon_hw2.magI, ofsQ : _buffer.mon_hw2.ofsQ, magQ : _buffer.mon_hw2.magQ, }; gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); }
void AP_GPS_SBP::logging_log_full_update() { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { return; } logging_write_headers(); struct log_SbpHealth pkt = { LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH), timestamp : hal.scheduler->millis(), crc_error_counter : crc_error_counter, last_injected_data_ms : last_injected_data_ms, last_iar_num_hypotheses : last_iar_num_hypotheses, };
// Write a performance monitoring packet. Total length : 19 bytes void Rover::Log_Write_Performance() { struct log_Performance pkt = { LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), time_us : AP_HAL::micros64(), loop_time : millis()- perf_mon_timer, main_loop_count : mainLoop_count, g_dt_max : G_Dt_max, gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000), gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000), gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000), i2c_lockup_count: 0, ins_error_count : ins.error_count(), hal.util->available_memory() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); }
void DataFlashTest_AllTypes::Log_Write_TypeMessages() { dataflash.StartNewLog(); 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(), b : -17, // int8_t B : 42, // uint8_t h : -12372, // int16_t H : 19812, // uint16_t i : -98234729, // int32_t I : 74627293, // uint32_t f : 35.87654, // float d : 67.7393274658293, // double n : { 'A', 'B', 'C', 'D' }, // char[4]
void AP_AutoTune::write_log(float servo, float demanded, float achieved) { if (!dataflash.logging_started()) { return; } struct log_ATRP pkt = { LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG), time_us : AP_HAL::micros64(), type : type, state : (uint8_t)state, servo : (int16_t)(servo*100), demanded : demanded, achieved : achieved, P : current.P.get() }; dataflash.WriteBlock(&pkt, sizeof(pkt)); }
// Write a sonar packet void Rover::Log_Write_Sonar() { uint16_t turn_time = 0; if (!is_zero(obstacle.turn_angle)) { turn_time = AP_HAL::millis() - obstacle.detected_time_ms; } struct log_Sonar pkt = { LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), time_us : AP_HAL::micros64(), lateral_accel : lateral_acceleration, sonar1_distance : sonar.distance_cm(0), sonar2_distance : sonar.distance_cm(1), detected_count : obstacle.detected_count, turn_angle : static_cast<int8_t>(obstacle.turn_angle), turn_time : turn_time, ground_speed : static_cast<uint16_t>(fabsf(ground_speed * 100)), throttle : int8_t(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) };
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)); }
void AP_GPS_SBP2::logging_log_full_update() { if (!should_df_log()) { return; } //TODO: Expand with heartbeat info. //TODO: Get rid of IAR NUM HYPO struct log_SbpHealth pkt = { LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH), time_us : AP_HAL::micros64(), crc_error_counter : crc_error_counter, last_injected_data_ms : last_injected_data_ms, last_iar_num_hypotheses : 0, }; DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt)); };
void write_formats(int fd) { /* construct message format packet */ struct { LOG_PACKET_HEADER; struct log_format_s body; } log_format_packet = { LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), }; /* fill message format packet for each format and write to log */ int i; for (i = 0; i < log_formats_num; i++) { log_format_packet.body = log_formats[i]; log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet)); } fsync(fd); }
// Write a control tuning packet. Total length : 22 bytes void Rover::Log_Write_Control_Tuning() { Vector3f accel = ins.get_accel(); struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG), time_us : AP_HAL::micros64(), steer_out : (int16_t)channel_steer->get_servo_out(), roll : (int16_t)ahrs.roll_sensor, pitch : (int16_t)ahrs.pitch_sensor, throttle_out : (int16_t)channel_throttle->get_servo_out(), accel_y : accel.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); }
void AP_GPS_UBLOX::log_mon_hw(void) { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { return; } struct log_Ubx1 pkt = { LOG_PACKET_HEADER_INIT(LOG_UBX1_MSG), timestamp : hal.scheduler->millis(), instance : state.instance, noisePerMS : _buffer.mon_hw_60.noisePerMS, jamInd : _buffer.mon_hw_60.jamInd, aPower : _buffer.mon_hw_60.aPower }; if (_payload_length == 68) { pkt.noisePerMS = _buffer.mon_hw_68.noisePerMS; pkt.jamInd = _buffer.mon_hw_68.jamInd; pkt.aPower = _buffer.mon_hw_68.aPower; } gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); }
int write_formats(int fd) { /* construct message format packet */ struct { LOG_PACKET_HEADER; struct log_format_s body; } log_msg_format = { LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), }; int written = 0; /* fill message format packet for each format and write it */ for (int i = 0; i < log_formats_num; i++) { log_msg_format.body = log_formats[i]; written += write(fd, &log_msg_format, sizeof(log_msg_format)); } return written; }
// Write a rangefinder packet void Rover::Log_Write_Rangefinder() { uint16_t turn_time = 0; if (!is_zero(obstacle.turn_angle)) { turn_time = AP_HAL::millis() - obstacle.detected_time_ms; } AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0); AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1); struct log_Rangefinder pkt = { LOG_PACKET_HEADER_INIT(LOG_RANGEFINDER_MSG), time_us : AP_HAL::micros64(), lateral_accel : g2.attitude_control.get_desired_lat_accel(), rangefinder1_distance : s0 ? s0->distance_cm() : (uint16_t)0, rangefinder2_distance : s1 ? s1->distance_cm() : (uint16_t)0, detected_count : obstacle.detected_count, turn_angle : static_cast<int8_t>(obstacle.turn_angle), turn_time : turn_time, ground_speed : static_cast<uint16_t>(fabsf(ground_speed * 100.0f)), throttle : int8_t(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) };
// Write an optical flow packet void Sub::Log_Write_Optflow() { #if OPTFLOW == ENABLED // exit immediately if not enabled if (!optflow.enabled()) { return; } const Vector2f &flowRate = optflow.flowRate(); const Vector2f &bodyRate = optflow.bodyRate(); struct log_Optflow pkt = { LOG_PACKET_HEADER_INIT(LOG_OPTFLOW_MSG), time_us : AP_HAL::micros64(), surface_quality : optflow.quality(), flow_x : flowRate.x, flow_y : flowRate.y, body_x : bodyRate.x, body_y : bodyRate.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); #endif // OPTFLOW == ENABLED }
// Write a navigation tuning packet void Rover::Log_Write_Nav_Tuning() { struct log_Nav_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG), time_us : AP_HAL::micros64(), yaw : (uint16_t)ahrs.yaw_sensor, wp_distance : wp_distance, target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(), nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(), throttle : (int8_t)(100 * channel_throttle->norm_output()), xtrack_error : nav_controller->crosstrack_error() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); }
int write_parameters(int fd) { /* construct parameter message */ struct { LOG_PACKET_HEADER; struct log_PARM_s body; } log_msg_PARM = { LOG_PACKET_HEADER_INIT(LOG_PARM_MSG), }; int written = 0; param_t params_cnt = param_count(); for (param_t param = 0; param < params_cnt; param++) { /* fill parameter message and write it */ strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name)); float value = NAN; switch (param_type(param)) { case PARAM_TYPE_INT32: { int32_t i; param_get(param, &i); value = i; // cast integer to float break; } case PARAM_TYPE_FLOAT: param_get(param, &value); break; default: break; } log_msg_PARM.body.value = value; written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM)); } return written; }
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_GPS_UBLOX::log_mon_hw(void) { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { return; } struct log_Ubx1 pkt = { LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX1_MSG)), time_us : AP_HAL::micros64(), instance : state.instance, noisePerMS : _buffer.mon_hw_60.noisePerMS, jamInd : _buffer.mon_hw_60.jamInd, aPower : _buffer.mon_hw_60.aPower, agcCnt : _buffer.mon_hw_60.agcCnt, }; if (_payload_length == 68) { pkt.noisePerMS = _buffer.mon_hw_68.noisePerMS; pkt.jamInd = _buffer.mon_hw_68.jamInd; pkt.aPower = _buffer.mon_hw_68.aPower; pkt.agcCnt = _buffer.mon_hw_68.agcCnt; } gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); }
/* report SITL state to DataFlash */ void SITL::Log_Write_SIMSTATE(DataFlash_Class *DataFlash) { float yaw; // convert to same conventions as DCM yaw = state.yawDeg; if (yaw > 180) { yaw -= 360; } struct log_AHRS pkt = { LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG), time_us : AP_HAL::micros64(), roll : (int16_t)(state.rollDeg*100), pitch : (int16_t)(state.pitchDeg*100), yaw : (uint16_t)(wrap_360_cd(yaw*100)), alt : (float)state.altitude, lat : (int32_t)(state.latitude*1.0e7), lng : (int32_t)(state.longitude*1.0e7) }; DataFlash->WriteBlock(&pkt, sizeof(pkt)); }