C++ DataFlash_Class::WriteBlock方法代码示例

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

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

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

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

/* 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 : hal.scheduler->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));
}

void OpticalFlow::Log_Write_Optflow()
{
    DataFlash_Class *instance = DataFlash_Class::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));
}