Esempio n. 1
0
/*
  handle DEVICE_OP_READ message
 */
void GCS_MAVLINK::handle_device_op_read(mavlink_message_t *msg)
{
    mavlink_device_op_read_t packet;
    mavlink_msg_device_op_read_decode(msg, &packet);
    AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr;
    uint8_t retcode = 0;
    uint8_t data[sizeof(mavlink_device_op_read_reply_t::data)] {};
    
    if (packet.bustype == DEVICE_OP_BUSTYPE_I2C) {
        dev = hal.i2c_mgr->get_device(packet.bus, packet.address);
    } else if (packet.bustype == DEVICE_OP_BUSTYPE_SPI) {
        dev = hal.spi->get_device(packet.busname);
    } else {
        retcode = 1;
        goto fail;
    }
    if (!dev) {
        retcode = 2;
        goto fail;
    }
    if (!dev->get_semaphore()->take(10)) {
        retcode = 3;
        goto fail;        
    }
    if (!dev->read_registers(packet.regstart, data, packet.count)) {
        retcode = 4;
        dev->get_semaphore()->give();
        goto fail;
    }
    dev->get_semaphore()->give();
    mavlink_msg_device_op_read_reply_send(
        chan,
        packet.request_id,
        retcode,
        packet.regstart,
        packet.count,
        data);
    return;

fail:
    mavlink_msg_device_op_read_reply_send(
        chan,
        packet.request_id,
        retcode,
        packet.regstart,
        0,
        nullptr);
}
Esempio n. 2
0
/*
  handle DEVICE_OP_WRITE message
 */
void GCS_MAVLINK::handle_device_op_write(mavlink_message_t *msg)
{
    mavlink_device_op_write_t packet;
    mavlink_msg_device_op_write_decode(msg, &packet);
    AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr;
    uint8_t retcode = 0;
    
    if (packet.bustype == DEVICE_OP_BUSTYPE_I2C) {
        dev = hal.i2c_mgr->get_device(packet.bus, packet.address);
    } else if (packet.bustype == DEVICE_OP_BUSTYPE_SPI) {
        dev = hal.spi->get_device(packet.busname);
    } else {
        retcode = 1;
        goto fail;
    }
    if (!dev) {
        retcode = 2;
        goto fail;
    }
    if (!dev->get_semaphore()->take(10)) {
        retcode = 3;
        goto fail;        
    }
    for (uint8_t i=0; i<packet.count; i++) {
        if (!dev->write_register(packet.regstart+i, packet.data[i])) {
            retcode = 4;
            break;
        }
    }
    dev->get_semaphore()->give();

fail:
    mavlink_msg_device_op_write_reply_send(
        chan,
        packet.request_id,
        retcode);
}
void OSD::reset(){
    max7456_on();

    byte cnt=15;

    while(cnt-- && !( MAX_read(MAX7456_STAT_reg_read) & 0x7) ) {//read status register - sync to soft-only versions
        byte j=3;

#ifdef SLAVE_BUILD
        while(j--)  osd_spi->transfer(0xff); // try to sync
#else
        while(j--)  SPI::transfer(0xff); // try to sync
#endif

        delay_15();
    }

    MAX_mode( MAX7456_RESET | video_mode );

    delay_15();

    hw_init();
}
Esempio n. 4
0
static void spi_init()
{
    // SPI reads have flag 0x80 set
    spi_dev->set_read_flag(0x80);

    // run the SPI bus at low speed
    spi_dev->set_speed(AP_HAL::Device::SPEED_LOW);

    uint8_t whoami = 0;
    uint8_t v;

    spi_dev->write_register(0x6B, 0x01);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6C, 0x3f);
    spi_dev->write_register(0xF5, 0x00);
    spi_dev->write_register(0x19, 0x09);
    spi_dev->write_register(0xEA, 0x00);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x1D, 0xC0);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x1A, 0xC0);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x38, 0x01);

    spi_dev->read_registers(0x6A, &v, 1);
    printf("reg 0x6A=0x%02x\n", v);
    spi_dev->read_registers(0x6B, &v, 1);
    printf("reg 0x6B=0x%02x\n", v);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x41);
    spi_dev->write_register(0x6C, 0x3f);
    spi_dev->write_register(0x6B, 0x41);

    spi_dev->read_registers(0x6A, &v, 1);
    printf("reg 0x6A=0x%02x\n", v);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->read_registers(0x6A, &v, 1);
    printf("reg 0x6A=0x%02x\n", v);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x41);
    spi_dev->write_register(0x6C, 0x3f);
    spi_dev->write_register(0x6B, 0x41);

    spi_dev->read_registers(0x6A, &v, 1);
    printf("reg 0x6A=0x%02x\n", v);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    // print the WHOAMI
    spi_dev->read_registers(MPUREG_WHOAMI, &whoami, 1);
    printf("20789 SPI whoami: 0x%02x\n", whoami);

    // wait for sensor to settle
    hal.scheduler->delay(100);

    // dump registers
    printf("ICM20789 registers\n");
#if 0
    for (uint8_t reg = 0; reg<0x80; reg++) {
        v=0;
        spi_dev->read_registers(reg, &v, 1);
        printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
        if ((reg+1) % 16 == 0) {
            printf("\n");
        }
    }
    printf("\n");
#endif

    spi_dev->get_semaphore()->give();
}