C++ DataFlash_Class类代码示例

bool AP_Arming::arm_checks(ArmingMethod 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;
        }
    }

    // check system health on arm as well as prearm
    if ((checks_to_perform & ARMING_CHECK_ALL) ||
        (checks_to_perform & ARMING_CHECK_SYSTEM)) {
        if (!system_checks(true)) {
            return false;
        }
    }
    
    // note that this will prepare DataFlash 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();
        df->PrepForArming();
        if (!df->logging_started()) {
            check_failed(ARMING_CHECK_LOGGING, true, "Logging not started");
            return false;
        }
    }
    return true;
}

/**
   handle all types of log download requests from the GCS
 */
void GCS_MAVLINK::handle_log_request_list(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
    mavlink_log_request_list_t packet;
    mavlink_msg_log_request_list_decode(msg, &packet);

    _log_listing = false;
    _log_sending = false;

    _log_num_logs = dataflash.get_num_logs();
    if (_log_num_logs == 0) {
        _log_next_list_entry = 0;
        _log_last_list_entry = 0;        
    } else {
        uint16_t last_log_num = dataflash.find_last_log();

        _log_next_list_entry = packet.start;
        _log_last_list_entry = packet.end;

        if (_log_last_list_entry > last_log_num) {
            _log_last_list_entry = last_log_num;
        }
        if (_log_next_list_entry < last_log_num + 1 - _log_num_logs) {
            _log_next_list_entry = last_log_num + 1 - _log_num_logs;
        }
    }

    _log_listing = true;
    handle_log_send_listing(dataflash);
}

/**
   handle all types of log download requests from the GCS
 */
void GCS_MAVLINK::handle_log_request_list(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
    mavlink_log_request_list_t packet;
    mavlink_msg_log_request_list_decode(msg, &packet);
    if (mavlink_check_target(packet.target_system, packet.target_component))
        return;
    _log_listing = false;
    _log_sending = false;

    _log_num_logs = dataflash.get_num_logs();
    if (_log_num_logs == 0) {
        return;
    }
    int16_t last_log_num = dataflash.find_last_log();

    _log_next_list_entry = packet.start;
    _log_last_list_entry = packet.end;

    if (_log_last_list_entry > last_log_num) {
        _log_last_list_entry = last_log_num;
    }
    if (_log_next_list_entry < last_log_num + 1 - _log_num_logs) {
        _log_next_list_entry = last_log_num + 1 - _log_num_logs;
    }

    _log_listing = true;
    handle_log_send_listing(dataflash);
}

void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
                                                            const Vector3f &gyro,
                                                            uint64_t sample_us)
{
    float dt;

    if (_imu._gyro_raw_sample_rates[instance] <= 0) {
        return;
    }

    dt = 1.0f / _imu._gyro_raw_sample_rates[instance];

    // call gyro_sample hook if any
    AP_Module::call_hook_gyro_sample(instance, dt, gyro);
    
    // compute delta angle
    Vector3f delta_angle = (gyro + _imu._last_raw_gyro[instance]) * 0.5f * dt;

    // compute coning correction
    // see page 26 of:
    // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
    // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
    // see also examples/coning.py
    Vector3f delta_coning = (_imu._delta_angle_acc[instance] +
                             _imu._last_delta_angle[instance] * (1.0f / 6.0f));
    delta_coning = delta_coning % delta_angle;
    delta_coning *= 0.5f;

    // integrate delta angle accumulator
    // the angles and coning corrections are accumulated separately in the
    // referenced paper, but in simulation little difference was found between
    // integrating together and integrating separately (see examples/coning.py)
    _imu._delta_angle_acc[instance] += delta_angle + delta_coning;
    _imu._delta_angle_acc_dt[instance] += dt;

    // save previous delta angle for coning correction
    _imu._last_delta_angle[instance] = delta_angle;
    _imu._last_raw_gyro[instance] = gyro;

    _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
    if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) {
        _imu._gyro_filter[instance].reset();
    }

    _imu._new_gyro_data[instance] = true;

    DataFlash_Class *dataflash = get_dataflash();
    if (dataflash != NULL) {
        uint64_t now = AP_HAL::micros64();
        struct log_GYRO pkt = {
            LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GYR1_MSG+instance)),
            time_us   : now,
            sample_us : sample_us?sample_us:now,
            GyrX      : gyro.x,
            GyrY      : gyro.y,
            GyrZ      : gyro.z
        };
        dataflash->WriteBlock(&pkt, sizeof(pkt));
    }

void SoloGimbal::write_logs()
{
    DataFlash_Class *dataflash = DataFlash_Class::instance();
    if (dataflash == 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
    };
    dataflash->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
    };
    dataflash->WriteBlock(&pkt2, sizeof(pkt2));

    _log_dt = 0;
    _log_del_ang.zero();
    _log_del_vel.zero();
}

bool AP_GPS_Backend::should_df_log() const
{
    DataFlash_Class *instance = DataFlash_Class::instance();
    if (instance == nullptr) {
        return false;
    }
    if (gps._log_gps_bit == (uint32_t)-1) {
        return false;
    }
    if (!instance->should_log(gps._log_gps_bit)) {
        return false;
    }
    return true;
}

/**
   trigger sending of log messages if there are some pending
 */
void GCS_MAVLINK::handle_log_send_listing(DataFlash_Class &dataflash)
{
    uint16_t txspace = comm_get_txspace(chan);
    if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_LOG_ENTRY_LEN) {
        // no space
        return;
    }
    if (hal.scheduler->millis() - last_heartbeat_time > 3000) {
        // give a heartbeat a chance
        return;
    }

    uint32_t size, time_utc;
    if (_log_next_list_entry == 0) {
        size = 0;
        time_utc = 0;
    } else {
        dataflash.get_log_info(_log_next_list_entry, size, time_utc);
    }
    mavlink_msg_log_entry_send(chan, _log_next_list_entry, _log_num_logs, _log_last_list_entry, time_utc, size);
    if (_log_next_list_entry == _log_last_list_entry) {
        _log_listing = false;
    } else {
        _log_next_list_entry++;
    }
}

void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio)
{
    mavlink_radio_t packet;
    mavlink_msg_radio_decode(msg, &packet);

    // record if the GCS has been receiving radio messages from
    // the aircraft
    if (packet.remrssi != 0) {
        last_radio_status_remrssi_ms = hal.scheduler->millis();
    }

    // use the state of the transmit buffer in the radio to
    // control the stream rate, giving us adaptive software
    // flow control
    if (packet.txbuf < 20 && stream_slowdown < 100) {
        // we are very low on space - slow down a lot
        stream_slowdown += 3;
    } else if (packet.txbuf < 50 && stream_slowdown < 100) {
        // we are a bit low on space, slow down slightly
        stream_slowdown += 1;
    } else if (packet.txbuf > 95 && stream_slowdown > 10) {
        // the buffer has plenty of space, speed up a lot
        stream_slowdown -= 2;
    } else if (packet.txbuf > 90 && stream_slowdown != 0) {
        // the buffer has enough space, speed up a bit
        stream_slowdown--;
    }

    //log rssi, noise, etc if logging Performance monitoring data
    if (log_radio) {
        dataflash.Log_Write_Radio(packet);
    }
}

/**
   trigger sending of log messages if there are some pending
 */
void GCS_MAVLINK::handle_log_send_listing(DataFlash_Class &dataflash)
{
    if (!HAVE_PAYLOAD_SPACE(chan, LOG_ENTRY)) {
        // no space
        return;
    }
    if (AP_HAL::millis() - last_heartbeat_time > 3000) {
        // give a heartbeat a chance
        return;
    }

    uint32_t size, time_utc;
    if (_log_next_list_entry == 0) {
        size = 0;
        time_utc = 0;
    } else {
        dataflash.get_log_info(_log_next_list_entry, size, time_utc);
    }
    mavlink_msg_log_entry_send(chan, _log_next_list_entry, _log_num_logs, _log_last_list_entry, time_utc, size);
    if (_log_next_list_entry == _log_last_list_entry) {
        _log_listing = false;
    } else {
        _log_next_list_entry++;
    }
}

/**
   handle request to erase log data
 */
void GCS_MAVLINK::handle_log_request_erase(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
    mavlink_log_erase_t packet;
    mavlink_msg_log_erase_decode(msg, &packet);

    dataflash.EraseAll();
}

/* report SITL state to DataFlash */
void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash)
{
    double p, q, r;
    float yaw;

    // we want the gyro values to be directly comparable to the
    // raw_imu message, which is in body frame
    convert_body_frame(state.rollDeg, state.pitchDeg,
                       state.rollRate, state.pitchRate, state.yawRate,
                       &p, &q, &r);

    // 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_ms : hal.scheduler->millis(),
        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));
}

// log the contents of the log_tuning structure to dataflash
void AP_TECS::log_data(DataFlash_Class &dataflash, uint8_t msgid)
{
    log_tuning.head1 = HEAD_BYTE1;
    log_tuning.head2 = HEAD_BYTE2;
    log_tuning.msgid = msgid;
	dataflash.WriteBlock(&log_tuning, sizeof(log_tuning));
}

/**
   trigger sending of log data if there are some pending
 */
bool GCS_MAVLINK::handle_log_send_data(DataFlash_Class &dataflash)
{
    int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
    if (payload_space < MAVLINK_MSG_ID_LOG_DATA_LEN) {
        // no space
        return false;
    }
    if (hal.scheduler->millis() - last_heartbeat_time > 3000) {
        // give a heartbeat a chance
        return false;
    }

    int16_t ret = 0;
    uint32_t len = _log_data_remaining;
    uint8_t data[90];

    if (len > 90) {
        len = 90;
    }
    ret = dataflash.get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, data);
    if (ret < 0) {
        // report as EOF on error
        ret = 0;
    }
    if (ret < 90) {
        memset(&data[ret], 0, 90-ret);
    }
    mavlink_msg_log_data_send(chan, _log_num_data, _log_data_offset, ret, data);
    _log_data_offset += len;
    _log_data_remaining -= len;
    if (ret < 90 || _log_data_remaining == 0) {
        _log_sending = false;
    }
    return true;
}

// 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();
        }
    }

    DataFlash_Class *df = DataFlash_Class::instance();
    if (df->should_log(_log_battery_bit)) {
        df->Log_Write_Current();
        df->Log_Write_Power();
    }

    check_failsafes();
}

void DataFlashTest_AllTypes::flush_dataflash(DataFlash_Class &_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 DataFlash_File::_io_timer) the data
    // will go out.
    hal.scheduler->delay(3000);
#endif
}