//TODO: handle full txspace properly
void DataFlash_MAVLink::send_log_block(uint32_t block_address)
{
    mavlink_channel_t chan = mavlink_channel_t(_chan - MAVLINK_COMM_0);
    if (!_initialised || comm_get_txspace(chan) < 255){
       return; 
    }
    mavlink_message_t msg;
    mavlink_status_t *chan_status = mavlink_get_channel_status(chan);
    uint8_t saved_seq = chan_status->current_tx_seq;
    chan_status->current_tx_seq = mavlink.seq++;
    Debug("Data Sent!!\n");
    uint16_t len = mavlink_msg_remote_log_data_block_pack(mavlink.system_id, 
                                                          mavlink.component_id,
                                                          &msg,
                                                          255,                      //GCS SYS ID
                                                          0,
                                                          _block_max_size,
                                                          _block_num[block_address],
                                                          _buf[block_address]);
    if(comm_get_txspace(chan) < len){
        return;
    }
    chan_status->current_tx_seq = saved_seq;
    _mavlink_resend_uart(chan, &msg);
}
Пример #2
0
//TODO: handle full txspace properly
bool DataFlash_MAVLink::send_log_block(struct dm_block &block)
{
    mavlink_channel_t chan = mavlink_channel_t(_chan - MAVLINK_COMM_0);
    if (!_initialised) {
       return false;
    }
    if (!HAVE_PAYLOAD_SPACE(chan, REMOTE_LOG_DATA_BLOCK)) {
        return false;
    }
    if (comm_get_txspace(chan) < 500) {
        return false;
    }
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    if (rand() < 0.1) {
        return false;
    }
#endif
    
#if DF_MAVLINK_DISABLE_INTERRUPTS
    irqstate_t istate = irqsave();
#endif

// DM_packing: 267039 events, 0 overruns, 8440834us elapsed, 31us avg, min 31us max 32us 0.488us rms
    hal.util->perf_begin(_perf_packing);

    mavlink_message_t msg;
    mavlink_status_t *chan_status = mavlink_get_channel_status(chan);
    uint8_t saved_seq = chan_status->current_tx_seq;
    chan_status->current_tx_seq = mavlink_seq++;
    // Debug("Sending block (%d)", block.seqno);
    mavlink_msg_remote_log_data_block_pack(mavlink_system.sysid,
                                           MAV_COMP_ID_LOG,
                                           &msg,
                                           _target_system_id,
                                           _target_component_id,
                                           block.seqno,
                                           block.buf);

    hal.util->perf_end(_perf_packing);

#if DF_MAVLINK_DISABLE_INTERRUPTS
    irqrestore(istate);
#endif

    block.last_sent = AP_HAL::millis();
    chan_status->current_tx_seq = saved_seq;

    // _last_send_time is set even if we fail to send the packet; if
    // the txspace is repeatedly chockas we should not add to the
    // problem and stop attempting to log
    _last_send_time = AP_HAL::millis();

    _mavlink_resend_uart(chan, &msg);

    return true;
}
//TODO: handle full txspace properly
bool DataFlash_MAVLink::send_log_block(struct dm_block &block)
{
    mavlink_channel_t chan = mavlink_channel_t(_chan - MAVLINK_COMM_0);
    if (!_initialised) {
       return false;
    }
    if (comm_get_txspace(chan) < MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN) {
        return false;
    }
    if (comm_get_txspace(chan) < 500) {
        return false;
    }
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
    if (rand() < 0.1) {
        return false;
    }
#endif
    
    mavlink_message_t msg;
    mavlink_status_t *chan_status = mavlink_get_channel_status(chan);
    uint8_t saved_seq = chan_status->current_tx_seq;
    chan_status->current_tx_seq = mavlink_seq++;
    // Debug("Sending block (%d)", block.seqno);
    mavlink_msg_remote_log_data_block_pack(mavlink_system.sysid,
                                           MAV_COMP_ID_LOG,
                                           &msg,
                                           _target_system_id,
                                           _target_component_id,
                                           block.seqno,
                                           block.buf);
    block.last_sent = hal.scheduler->millis();
    chan_status->current_tx_seq = saved_seq;

    // _last_send_time is set even if we fail to send the packet; if
    // the txspace is repeatedly chockas we should not add to the
    // problem and stop attempting to log
    _last_send_time = hal.scheduler->millis();

    _mavlink_resend_uart(chan, &msg);

    return true;
}