/********************************************
* get whole descriptor data by usb_test_start
*
*********************************************/
void test_get_dev_descriptor(UINT32 base_addr)
{
UINT8 cmd_get_dev_desc[]={0x80,0x06,0x00,0x01,0x00,0x00,0x12,0x00 };
// UINT8 device_descriptor_buff[18];
// usbd_get_descriptor()
USB_LOGO_PRINTF("get dev descriptor!\n");
set_device_address(base_addr,2);
if(ep0_setup(base_addr,cmd_get_dev_desc,8) != RET_SUCCESS)
goto err;
USB_LOGO_PRINTF("control SETUP phase OK!\n");
if(ep0_in_data(base_addr,((UINT8*)&test_device_descriptor),18) != RET_SUCCESS)
goto err;
USB_LOGO_PRINTF("control DATA phase OK!\n");
if(ep0_status(base_addr, B_TOKEN_OUT) != RET_SUCCESS)
goto err;
USB_LOGO_PRINTF("control STATUS phase OK!\n");
err:
return;
}
SPIDevice::SPIDevice(SPIBus &bus, SPIDesc &device_desc)
: _bus(bus)
, _desc(device_desc)
{
set_device_bus(_bus.bus);
set_device_address(_desc.subdev);
if (_desc.cs_pin != SPI_CS_KERNEL) {
_cs = hal.gpio->channel(_desc.cs_pin);
if (!_cs) {
AP_HAL::panic("Unable to instantiate cs pin");
}
_cs->mode(HAL_GPIO_OUTPUT);
// do not hold the SPI bus initially
_cs_release();
}
}
int
SF1XX::init()
{
int ret = PX4_ERROR;
int hw_model;
param_get(param_find("SENS_EN_SF1XX"), &hw_model);
switch (hw_model) {
case 0:
DEVICE_LOG("disabled.");
return ret;
case 1: /* SF10/a (25m 32Hz) */
_min_distance = 0.01f;
_max_distance = 25.0f;
_conversion_interval = 31250;
break;
case 2: /* SF10/b (50m 32Hz) */
_min_distance = 0.01f;
_max_distance = 50.0f;
_conversion_interval = 31250;
break;
case 3: /* SF10/c (100m 16Hz) */
_min_distance = 0.01f;
_max_distance = 100.0f;
_conversion_interval = 62500;
break;
case 4:
/* SF11/c (120m 20Hz) */
_min_distance = 0.01f;
_max_distance = 120.0f;
_conversion_interval = 50000;
break;
case 5:
/* SF20/LW20 (100m 48-388Hz) */
_min_distance = 0.001f;
_max_distance = 100.0f;
_conversion_interval = 20834;
break;
default:
DEVICE_LOG("invalid HW model %d.", hw_model);
return ret;
}
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
set_device_address(SF1XX_BASEADDR);
if (_reports == nullptr) {
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
// Select altitude register
int ret2 = measure();
if (ret2 == 0) {
ret = OK;
_sensor_ok = true;
DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance,
(int)(1e6f / _conversion_interval), SF1XX_BASEADDR);
}
return ret;
}
void
MB12XX::cycle()
{
if (_collect_phase) {
_index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */
set_device_address(_index_counter);
/* perform collection */
if (OK != collect()) {
DEVICE_DEBUG("collection error");
/* if error restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/* change i2c adress to next sonar */
_cycle_counter = _cycle_counter + 1;
if (_cycle_counter >= addr_ind.size()) {
_cycle_counter = 0;
}
/* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate
Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */
if (_measure_ticks > USEC2TICK(_cycling_rate)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&MB12XX::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(_cycling_rate));
return;
}
}
/* Measurement (firing) phase */
/* ensure sonar i2c adress is still correct */
_index_counter = addr_ind[_cycle_counter];
set_device_address(_index_counter);
/* Perform measurement */
if (OK != measure()) {
DEVICE_DEBUG("measure error sonar adress %d", _index_counter);
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&MB12XX::cycle_trampoline,
this,
USEC2TICK(_cycling_rate));
}
int
MB12XX::init()
{
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
_index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */
set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */
if (_reports == nullptr) {
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
// XXX we should find out why we need to wait 200 ms here
usleep(200000);
/* check for connected rangefinders on each i2c port:
We start from i2c base address (0x70 = 112) and count downwards
So second iteration it uses i2c address 111, third iteration 110 and so on*/
for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
_index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */
set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */
int ret2 = measure();
if (ret2 == 0) { /* sonar is present -> store address_index in array */
addr_ind.push_back(_index_counter);
DEVICE_DEBUG("sonar added");
_latest_sonar_measurements.push_back(200);
}
}
_index_counter = MB12XX_BASEADDR;
set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */
/* if only one sonar detected, no special timing is required between firing, so use default */
if (addr_ind.size() == 1) {
_cycling_rate = MB12XX_CONVERSION_INTERVAL;
} else {
_cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;
}
/* show the connected sonars in terminal */
for (unsigned i = 0; i < addr_ind.size(); i++) {
DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]);
}
DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size());
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
return ret;
}
RET_CODE usb_test_ui_enum(struct usb_device_descriptor *dev_descriptor )
{
UINT32 ret = RET_SUCCESS;
UINT8 cmd_set_dev_addr[]={0x00,0x05,0x02,0x00,0x00,0x00,0x00,0x00 }; //addr: 0x02
UINT8 cmd_get_dev_desc[]={0x80,0x06,0x00,0x01,0x00,0x00,0x12,0x00 };
UINT8 temp_buff[8];
UINT32 bus_speed;
UINT32 base_addr = 0xb800F000;
usb_if_test_flag = IF_TEST_PACKET;
if (sys_ic_is_M3202())//(sys_ic_get_chip_id() == ALI_M3202)
base_addr = HC_M3202_BASE_ADDR;
else if( (sys_ic_get_chip_id() == ALI_M3329E)&&(sys_ic_get_rev_id()>=IC_REV_5))
base_addr = HC_S3329E5_BASE_ADDR;
else if ((sys_ic_get_chip_id() == ALI_M3329E)&&(sys_ic_get_rev_id()<IC_REV_5))
base_addr = HC_S3329E_BASE_ADDR;
else if (sys_ic_get_chip_id() == ALI_S3602)
base_addr = HC_S3602_BASE_ADDR;
else if (sys_ic_get_chip_id() == ALI_S3602F)
{
if(s3602f_test_port == IF_FRONT_PROT)
base_addr = HC_S3602F_BASE_ADDR;
else if(s3602f_test_port == IF_REAR_PROT)
base_addr = HC_S3602F_BASE_ADDR1;
}
else
return !RET_SUCCESS; // usb invalid chip id
//usb_test_show_txt("enumeration...");
usb_hw_init(base_addr);
osal_task_sleep(2);
// osal_delay(100);
//disable int
*((volatile unsigned char *)(base_addr+0x1E))= 0x00;
*((volatile unsigned char *)(base_addr+0x1F))= 0x00;
*((volatile unsigned char *)(base_addr+0x62))= 0xFF;
//TX_DRIVING +10%
if ((sys_ic_get_chip_id() == ALI_M3329E)&&(sys_ic_get_rev_id()<IC_REV_5))
*((volatile unsigned char *)0xb800007C) = 0x03; //USB_PLL_SEL: DISC_SEL/PD_HS/PD_FS/PD_PLL . SQUELCH_LEVEL -/TX_DRIVING -
USB_LOGO_PRINTF("\n 1.reset usb bus!\n");
issue_reset_to_device(base_addr);
if(wait_bus_reset_finish(base_addr) != RET_SUCCESS)
{
ret = !RET_SUCCESS;
goto err;
}
osal_task_sleep(500);
USB_LOGO_PRINTF("get device speed");
bus_speed = get_device_speed(base_addr);
if (bus_speed & B_OPER_FS)
{
USB_LOGO_PRINTF("Device is in FS mode!\n");
}
else if (bus_speed & B_OPER_LS)
{
USB_LOGO_PRINTF("Device is in LS mode!\n");
}
else
{
USB_LOGO_PRINTF("Device is in HS mode!\n");
}
osal_task_sleep(2000);
// usbd_set_dev_address(new_dev) ;
USB_LOGO_PRINTF("\n 2.set_dev_address!\n");
if(ep0_setup(base_addr,cmd_set_dev_addr,8) != RET_SUCCESS)
{
ret = !RET_SUCCESS;
goto err;
}
USB_LOGO_PRINTF("control SETUP phase OK!\n");
if(ep0_in_data(base_addr,temp_buff,0) != RET_SUCCESS)
{
ret = !RET_SUCCESS;
goto err;
}
// if(ep0_status(base_addr, B_TOKEN_IN) != RET_SUCCESS)
// goto err;
USB_LOGO_PRINTF("control STATUS phase OK!\n");
// usbd_get_descriptor()
USB_LOGO_PRINTF("\n 3.get dev descriptor!\n");
set_device_address(base_addr,2);
if(ep0_setup(base_addr,cmd_get_dev_desc,8) != RET_SUCCESS)
{
ret = !RET_SUCCESS;
goto err;
}
USB_LOGO_PRINTF("control SETUP phase OK!\n");
if(ep0_in_data(base_addr,((UINT8*)&test_device_descriptor),18) != RET_SUCCESS)
{
ret = !RET_SUCCESS;
goto err;
}
USB_LOGO_PRINTF("control DATA phase OK!\n");
if(ep0_status(base_addr, B_TOKEN_OUT) != RET_SUCCESS)
{
ret = !RET_SUCCESS;
goto err;
}
USB_LOGO_PRINTF("control STATUS phase OK!\n");
MEMCPY(dev_descriptor , &test_device_descriptor ,18) ;
err:
if(ret != RET_SUCCESS)
{
usb_test_show_txt("Enum Fail , Please check usb port");
}else{
usb_test_show_txt("enumeration success");
}
bus_enum_success = TRUE ;
//usb_test_show_hex("dev_desc:",((UINT8*)&test_device_descriptor),18);
return ret;
}