// Construct a dynamic configuration descriptor
// This really needs dynamic endpoint allocation etc
uint32_t USBDeviceClass::sendConfiguration(uint32_t maxlen)
{
uint32_t total = 0;
// Count and measure interfaces
_dry_run = true;
uint8_t interfaces = SendInterfaces(&total);
_Pragma("pack(1)")
ConfigDescriptor config = D_CONFIG((uint16_t)(total + sizeof(ConfigDescriptor)), interfaces);
_Pragma("pack()")
// Now send them
_dry_run = false;
if (maxlen == sizeof(ConfigDescriptor)) {
sendControl(&config, sizeof(ConfigDescriptor));
return true;
}
total = 0;
packMessages(true);
sendControl(&config, sizeof(ConfigDescriptor));
SendInterfaces(&total);
packMessages(false);
return true;
}
/*Initializes the LCD*/
void lcdInit(){
CTRL = 0x0;//0 out CTRL
DATA = 0x0;//0 out data
_delay_ms(30);//at least 20 ms
//initialize screen 2 lines, 5x10 units
sendControl(0b00111000);
_delay_ms(2);
//clear display
sendControl(0b00000001);
_delay_ms(2);
sendControl(0b00001101);
_delay_ms(2);
//set cursor to move to the right
sendControl(0b00000110);
_delay_ms(2);
//move to home
sendControl(0b10000000);
}
void SLCD::cursor(int line, int col) {
line %= _numRows;
col %= _numCols;
int offset = ((line%2)*64) + (line >1 ? 20 : 0);
sendControl((char)(offset + col + 128));
delay(10);
}
void SLCD::vscroll(int spaces, int delayMs) {
byte controlChar = (spaces >= 0) ? SCROLL_RIGHT : SCROLL_LEFT;
int numSpaces = abs(spaces);
for (int i =0; i<numSpaces; i++) {
sendControl((char)controlChar);
delay(delayMs);
}
}
void USBDeviceClass::packMessages(bool val)
{
if (val) {
_pack_message = true;
_pack_size = 0;
} else {
_pack_message = false;
sendControl(_pack_buffer, _pack_size);
}
}
/*
* Process event from the tick handler.
*
* The super-class requests desired levels from the throttle and
* brake and decides which one to apply.
*/
void BrusaMotorController::handleTick() {
MotorController::handleTick(); // call parent
tickCounter++;
sendControl(); // send CTRL every 20ms
if (tickCounter > 4) {
sendControl2(); // send CTRL_2 every 100ms
sendLimits(); // send LIMIT every 100ms
tickCounter = 0;
}
}
int main()
{
for (int i=0; i<20; i++) {
usleep(1000000);
sendPress(16);
sendPress(14);
sendPress(36);
}
sendControl(8);
sleep(1);
return 0;
}
void SLCD::blinkCursorOn(){
sendControl(BLINK_CURSOR_ON);
}
void SLCD::cursorRight(){
sendControl(CURSOR_RIGHT);
}
void SLCD::underlineCursorOff(){
sendControl(UNDERLINE_CURSOR_OFF);
}
void newline(){
/*just move cursor to address 40
which is line 2
*/
sendControl(0b10101000);
}
void SLCD::cursorLeft(){
sendControl(CURSOR_LEFT);
}
void SLCD::displayOff(){
sendControl(DISPLAY_OFF);
}
void XimeaController::stop()
{
sendControl( cuttlefish_msgs::XimeaControl_CommandType_STOP );
}
int appSend(char * port, const char * fpath) {
printf("[appSend] Connecting to Receiver...n");
if(llopen(port, SENDER) < 0) {
printf("[appSend]ERROR: Couldn't reach the receiver!n");
return -1;
}
printf("[appSend] CONNECTEDn");
if( define_file(fpath) != 0) {
printf("[appSend] !!Cannot open file!!n");
return -1;
}
sendControl(START_PAC,pingu.size);
appDef.seqNr = 0;
int lastOne;
char buf[256];
if(pingu.size % PAC_SIZE > 0)
lastOne = 1;
else
lastOne = 0;
int totalPacs = (pingu.size / PAC_SIZE) + lastOne;
int real_totalPacs=0,duplicados=0;
float percent_lost;
int n;
while(1) {
printf("[appSend] Sending packet %d of %dn", appDef.seqNr,totalPacs);
int size = fread(buf, sizeof(char), PAC_SIZE, pingu.p);
if (size <= 0) break;
if( sendData(size, buf) == -1) {
printf("[appSend] !!!Error sending packet %d!!!n", appDef.seqNr);
while( sendData(size, buf) == -1) {
printf("[appSend] Re-sending packet %d of %dn", appDef.seqNr,totalPacs);
real_totalPacs++;
duplicados++;
}
}
appDef.seqNr = (appDef.seqNr + 1) % 256;
real_totalPacs++;
}
printf("[appSend] Sending end packet...n");
sendControl(END_PAC, pingu.size);
printf("[appSend] Sent %d packets, of which %d duplicatesn",real_totalPacs,duplicados);
percent_lost = (duplicados * 100)/real_totalPacs;
printf("[appSend] %.2f %% of the packets sent were lostn",percent_lost);
fclose(pingu.p);
printf("[appSend] Closing connection...n");
llclose(appDef.status);
printf("[appSend] Exiting Application!n");
}
static void usbIRQHandler(uint32_t token) {
// we need to mask because GINTSTS is set for a particular interrupt even if it's masked in GINTMSK (GINTMSK just prevents an interrupt being generated)
uint32_t status = GET_REG(USB + GINTSTS) & GET_REG(USB + GINTMSK);
int process = FALSE;
//uartPrintf("<begin interrupt: %x>\r\n", status);
if(status) {
process = TRUE;
}
while(process) {
if((status & GINTMSK_OTG) == GINTMSK_OTG) {
// acknowledge OTG interrupt (these bits are all R_SS_WC which means Write Clear, a write of 1 clears the bits)
SET_REG(USB + GOTGINT, GET_REG(USB + GOTGINT));
// acknowledge interrupt (this bit is actually RO, but should've been cleared when we cleared GOTGINT. Still, iBoot pokes it as if it was WC, so we will too)
SET_REG(USB + GINTSTS, GINTMSK_OTG);
process = TRUE;
} else {
// we only care about OTG
process = FALSE;
}
if((status & GINTMSK_RESET) == GINTMSK_RESET) {
if(usb_state < USBError) {
bufferPrintf("usb: reset detected\r\n");
change_state(USBPowered);
}
int retval = resetUSB();
SET_REG(USB + GINTSTS, GINTMSK_RESET);
if(retval) {
bufferPrintf("usb: listening for further usb events\r\n");
return;
}
process = TRUE;
}
if(((status & GINTMSK_INEP) == GINTMSK_INEP) || ((status & GINTMSK_OEP) == GINTMSK_OEP)) {
// aha, got something on one of the endpoints. Now the real fun begins
// first, let's get the interrupt status of individual endpoints
getEndpointInterruptStatuses();
if(isSetupPhaseDone()) {
// recall our earlier receiveControl calls. We now should have 8 bytes of goodness in controlRecvBuffer.
USBSetupPacket* setupPacket = (USBSetupPacket*) controlRecvBuffer;
uint16_t length;
uint32_t totalLength;
USBStringDescriptor* strDesc;
if(USBSetupPacketRequestTypeType(setupPacket->bmRequestType) != USBSetupPacketVendor) {
switch(setupPacket->bRequest) {
case USB_GET_DESCRIPTOR:
length = setupPacket->wLength;
// descriptor type is high, descriptor index is low
switch(setupPacket->wValue >> 8) {
case USBDeviceDescriptorType:
if(length > sizeof(USBDeviceDescriptor))
length = sizeof(USBDeviceDescriptor);
memcpy(controlSendBuffer, usb_get_device_descriptor(), length);
break;
case USBConfigurationDescriptorType:
// hopefully SET_ADDRESS was received beforehand to set the speed
totalLength = getConfigurationTree(setupPacket->wValue & 0xFF, usb_speed, controlSendBuffer);
if(length > totalLength)
length = totalLength;
break;
case USBStringDescriptorType:
strDesc = usb_get_string_descriptor(setupPacket->wValue & 0xFF);
if(length > strDesc->bLength)
length = strDesc->bLength;
memcpy(controlSendBuffer, strDesc, length);
break;
case USBDeviceQualifierDescriptorType:
if(length > sizeof(USBDeviceQualifierDescriptor))
length = sizeof(USBDeviceQualifierDescriptor);
memcpy(controlSendBuffer, usb_get_device_qualifier_descriptor(), length);
break;
default:
bufferPrintf("Unknown descriptor request: %d\r\n", setupPacket->wValue >> 8);
if(usb_state < USBError) {
change_state(USBUnknownDescriptorRequest);
}
}
if(usb_state < USBError) {
sendControl(controlSendBuffer, length);
}
break;
case USB_SET_ADDRESS:
usb_speed = DSTS_GET_SPEED(GET_REG(USB + DSTS));
usb_max_packet_size = packetsizeFromSpeed(usb_speed);
SET_REG(USB + DCFG, (GET_REG(USB + DCFG) & ~DCFG_DEVICEADDRMSK)
| ((setupPacket->wValue & DCFG_DEVICEADDR_UNSHIFTED_MASK) << DCFG_DEVICEADDR_SHIFT));
// send an acknowledgement
sendControl(controlSendBuffer, 0);
if(usb_state < USBError) {
change_state(USBAddress);
}
break;
case USB_SET_INTERFACE:
// send an acknowledgement
sendControl(controlSendBuffer, 0);
break;
case USB_GET_STATUS:
// FIXME: iBoot doesn't really care about this status
*((uint16_t*) controlSendBuffer) = 0;
sendControl(controlSendBuffer, sizeof(uint16_t));
break;
case USB_GET_CONFIGURATION:
// FIXME: iBoot just puts out a debug message on console for this request.
break;
case USB_SET_CONFIGURATION:
setConfiguration(0);
// send an acknowledgment
sendControl(controlSendBuffer, 0);
if(usb_state < USBError) {
change_state(USBConfigured);
startHandler();
}
break;
default:
if(usb_state < USBError) {
change_state(USBUnknownRequest);
}
}
// get the next SETUP packet
receiveControl(controlRecvBuffer, sizeof(USBSetupPacket));
}
} else {
//uartPrintf("\t<begin callEndpointHandlers>\r\n");
callEndpointHandlers();
//uartPrintf("\t<end callEndpointHandlers>\r\n");
}
process = TRUE;
}
void XimeaController::open()
{
sendControl( cuttlefish_msgs::XimeaControl_CommandType_OPEN );
}
void Telemetry::main(const State& state, Config& config)
{
// Re-initialize variables
cmd_ = 0;
checksum_ = 0;
// Check whether we have a request available
if ((n_bytes_available_ = Serial_Telemetry.available()) > 0)
{
// Read all data to tmp buffer. It is possible that not all the data has arrived yet
Serial_Telemetry.readBytes(&(rx_data_buffer_[ptr_]), n_bytes_available_);
ptr_ += n_bytes_available_;
}
else // No more data to receive
{
if(ptr_ > 0) // We have data to process
{
// Check for the magic word
if (rx_data_buffer_[0] == magic_word_[0] && rx_data_buffer_[1] == magic_word_[1])
{
cmd_ = rx_data_buffer_[2];
}
if (cmd_ == TELEMETRY_CMD_IN_CONFIG)
{
Telemetry::receiveConfig(config);
}
else
{
// Send Magic Word
write8((uint8_t)magic_word_[0]);
write8((uint8_t)magic_word_[1]);
// Send cmd
write8(cmd_);
// Perform desired operation
switch (cmd_)
{
case TELEMETRY_CMD_OUT_STATUS:
sendStatus(state);
break;
case TELEMETRY_CMD_OUT_IMU:
sendIMU(state);
break;
case TELEMETRY_CMD_OUT_MAG:
sendMagnetometer(state);
break;
case TELEMETRY_CMD_OUT_BARO:
sendBarometer(state);
break;
case TELEMETRY_CMD_OUT_TEMP:
sendTemperature(state);
break;
case TELEMETRY_CMD_OUT_RC:
sendRC(state);
break;
case TELEMETRY_CMD_OUT_GPS:
sendGPS(state);
break;
case TELEMETRY_CMD_OUT_SONAR:
sendSonar(state);
break;
case TELEMETRY_CMD_OUT_ATTITUDE:
sendAttitude(state);
break;
case TELEMETRY_CMD_OUT_CONTROL:
sendControl(state);
break;
case TELEMETRY_CMD_OUT_CONFIG:
sendConfig(config);
break;
} // switch cmd
sendCheckSum();
} // config_in
ptr_ = 0; // Prepare for next package
} // ptr_ > 0
} // n_bytes > 0
}
void XimeaController::close()
{
sendControl( cuttlefish_msgs::XimeaControl_CommandType_CLOSE );
}
void XimeaController::stopTrigger()
{
sendControl( cuttlefish_msgs::XimeaControl_CommandType_TRIGGER_STOP );
}
void XimeaController::terminate()
{
sendControl( cuttlefish_msgs::XimeaControl_CommandType_TERMINATE );
}
void SLCD::blinkCursorOff(){
sendControl(BLINK_CURSOR_OFF);
}
void * DevicePolling(void * host_number) // thread
{
unsigned char poll_en = 0;
unsigned int time_poll = 30000;
unsigned char destroy = 0;
int host = (int) host_number;
unsigned char trying_time = 0, dev_disconnect_try_time = 3;
float_struct_t my_float;
if (host < 0 || host >= DEV_HOST_NUMBER)
{
printf("Host number not valid.\r\nIt should be greater or equal zero and lester than %d.\r\n", DEV_HOST_NUMBER);
printf("Thread exiting.\r\n");
pthread_exit(NULL);
}
printf("Thread: %d start with host: %d.\n",
(int)polling_thread[host], host);
while(1)
{
if (pthread_mutex_trylock(&device_control_access) == 0)
{
poll_en = dev_host[host].polling_control.enable;
time_poll = dev_host[host].polling_control.time_poll_ms * 1000;
destroy = dev_host[host].polling_control.destroy;
pthread_mutex_unlock(&device_control_access);
}
else
{
printf("Thread: %d. host: %d. Fail to access device control.\n",
(int)polling_thread[host], host);
usleep(1000);
}
if (destroy)
{
printf("Thread: %d. host: %d. Destroying.\n",
(int)polling_thread[host], host);
pthread_exit(NULL);
}
if (poll_en)
{
//while (pthread_mutex_trylock(&device_control_access) != 0)
//usleep(1000);
if (dev_host[host].type != DEV_UNKNOWN) // already known device type
{
trying_time = 0;
while (pthread_mutex_trylock(&serial_access) != 0)
{
usleep(1000);
trying_time ++;
if (trying_time > 10)
break;
}
if (trying_time > 10)
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Fail to access serial port.\n",
(int)polling_thread[host], host);
#endif
//pthread_mutex_unlock(&device_control_access);
usleep(time_poll);
continue;
}
else
{
RaspiExt_Pin_Hostx_Active(host + 1);
if (queryData(&dev_host[host]))
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Got data from device.\n",
(int)polling_thread[host], host);
DeviceInfo(&dev_host[host]);
#endif
dev_disconnect_try_time = 3;
}
else
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. No device here.\n",
(int)polling_thread[host], host);
#endif
if (dev_disconnect_try_time == 0)
{
// TODO: unregister this device
unsigned char reg_id = dev_host[host].number | dev_host[host].type;
printf("Thread: %d. host: %d. Unregister device %X.\n",
(int)polling_thread[host],
host,
reg_id);
UnRegisterID(®_id);
if (IS_MY_THESIS(DEV_TYPE_MASK(dev_host[host].type)))
{
if (dev_host[host].data != NULL)
memset(dev_host[host].data, 0, sizeof(struct ThesisData));
}
else
{
if (dev_host[host].data != NULL)
memset(dev_host[host].data, 0, getTypeLength(dev_host[host].type));
}
dev_host[host].type = DEV_UNKNOWN;
dev_host[host].number = DEV_NUMBER_UNKNOWN;
}
else
{
dev_disconnect_try_time--;
}
}
RaspiExt_Pin_Hostx_Inactive(host + 1);
}
pthread_mutex_unlock(&serial_access);
switch (dev_host[host].type)
{
case DEV_SENSOR_TEMPERATURE:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 1000, host + 1);
if (IS_BIG_ENDIAN_BYTE_ORDER(dev_host[host].data_type))
{
my_float.f_byte[0] = dev_host[host].data[3];
my_float.f_byte[1] = dev_host[host].data[2];
my_float.f_byte[2] = dev_host[host].data[1];
my_float.f_byte[3] = dev_host[host].data[0];
}
else
{
my_float.f_byte[0] = dev_host[host].data[0];
my_float.f_byte[1] = dev_host[host].data[1];
my_float.f_byte[2] = dev_host[host].data[2];
my_float.f_byte[3] = dev_host[host].data[3];
}
printf("Thread: %d. host: %d. Temperature: %0.3f.\n",
(int)polling_thread[host],
host,
my_float.f);
// adjust time polling
if (pthread_mutex_trylock(&device_control_access) == 0)
{
dev_host[host].polling_control.time_poll_ms = 50;
}
else
{
printf("Thread: %d. host: %d. Fail to access device control.\n",
(int)polling_thread[host],
host);
}
// save to shared memory
break;
case DEV_SENSOR_ULTRA_SONIC:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 100, host + 1);
my_float.f_byte[0] = dev_host[host].data[3];
my_float.f_byte[1] = dev_host[host].data[2];
my_float.f_byte[2] = dev_host[host].data[1];
my_float.f_byte[3] = dev_host[host].data[0];
printf("Thread: %d. host: %d. Distance: %0.3f.\n",
(int)polling_thread[host],
host,
my_float.f);
// put to db
#if DATABASE
//// put this device into database
//if (DB_IsExist_sensors(dev_host[host].number, dev_host[host].type, UltraSonic_name,
//UltraSonic_code, UltraSonic_symbol, 30, 10, UltraSonic_unit) == 0) // not exist
//{
//DB_Record_sensors(dev_host[host].number, dev_host[host].type, UltraSonic_name,
//UltraSonic_code, UltraSonic_symbol, 30, 10, UltraSonic_unit);
//}
//if (DB_IsExist_sensor_types(dev_host[host].type, UltraSonic_name, UltraSonic_description) == 0) // not exist
//{
//DB_Record_sensor_types(dev_host[host].type, UltraSonic_name, UltraSonic_description);
//}
////DB_Record_sensor_values(dev_host[host].type, UltraSonic_name, UltraSonic_description);
DB_Record_sensor_values_short(my_float.f, millis()/1000);
#endif
// adjust time polling
// save to shared memory
break;
case DEV_SENSOR_LIGTH:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 50, host + 1);
break;
case DEV_RF:
break;
case DEV_BLUETOOTH:
break;
case DEV_BUZZER:
break;
case DEV_SENSOR_GAS:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 50, host + 1);
break;
case DEV_SIM900:
break;
case DEV_MY_THESIS:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 1000, host + 1);
break;
default:
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Unknown device type.\n",
(int)polling_thread[host], host);
#endif
dev_host[host].type = DEV_UNKNOWN;
dev_host[host].number = DEV_NUMBER_UNKNOWN;
break;
}
}
else // unknown device type
{
RaspiExt_LED_Hostx_Config(LED_MODE_OFF, 50, host + 1);
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Unknown device, identifying.\n",
(int)polling_thread[host], host);
#endif
// query broadcast id to identify what it is
// adjust time polling to 500 ms
time_poll = 500000;
trying_time = 0;
while (pthread_mutex_trylock(&serial_access) != 0)
{
usleep(1000);
trying_time ++;
if (trying_time > 10)
{
break;
}
}
if (trying_time > 10)
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Fail to access serial port.\n",
(int)polling_thread[host], host);
#endif
usleep(time_poll);
continue;
}
else
{
#if DEVICE_DEBUG
//dev_host[host].type = DEV_SENSOR_ULTRA_SONIC;
//dev_host[host].number = 0x01;
#endif
RaspiExt_Pin_Hostx_Active(host + 1);
if (queryData(&dev_host[host]))
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Got data from device.\n",
(int)polling_thread[host], host);
#endif
// TODO: register new id
unsigned char reg_id = dev_host[host].type | dev_host[host].number;
if (RegisterID(®_id) != 0)
{
printf("Thread: %d. host: %d. Fail to register new device.\n",
(int)polling_thread[host],
host);
}
else
{
printf("Thread: %d. host: %d. Registered new device %X.\n",
(int)polling_thread[host],
host,
reg_id);
}
dev_host[host].type = DEV_TYPE_MASK(reg_id);
dev_host[host].number = DEV_NUMBER_MASK(reg_id);
sendControl(dev_host[host]);
}
else
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. No device here.\n",
(int)polling_thread[host], host);
#endif
}
RaspiExt_Pin_Hostx_Inactive(host + 1);
}
pthread_mutex_unlock(&serial_access);
}
usleep(time_poll);
//pthread_mutex_unlock(&device_control_access);
}// query device
}
}
void SLCD::displayOn(){
sendControl(DISPLAY_ON);
}
void SLCD::clear() {
sendControl(CLEAR);
}
bool USBDeviceClass::sendDescriptor(USBSetup &setup)
{
uint8_t t = setup.wValueH;
uint8_t desc_length = 0;
bool _cdcComposite;
int ret;
const uint8_t *desc_addr = 0;
if (t == USB_CONFIGURATION_DESCRIPTOR_TYPE)
{
return USBDevice.sendConfiguration(setup.wLength);
}
#ifdef PLUGGABLE_USB_ENABLED
ret = PluggableUSB().getDescriptor(setup);
if (ret != 0) {
return (ret > 0 ? true : false);
}
#endif
if (t == USB_DEVICE_DESCRIPTOR_TYPE)
{
if (setup.wLength == 8)
_cdcComposite = 1;
desc_addr = _cdcComposite ? (const uint8_t*)&USB_DeviceDescriptorB : (const uint8_t*)&USB_DeviceDescriptor;
if (*desc_addr > setup.wLength) {
desc_length = setup.wLength;
}
}
else if (USB_STRING_DESCRIPTOR_TYPE == t)
{
if (setup.wValueL == 0) {
desc_addr = (const uint8_t*)&STRING_LANGUAGE;
}
else if (setup.wValueL == IPRODUCT) {
return sendStringDescriptor(STRING_PRODUCT, setup.wLength);
}
else if (setup.wValueL == IMANUFACTURER) {
return sendStringDescriptor(STRING_MANUFACTURER, setup.wLength);
}
else if (setup.wValueL == ISERIAL) {
#ifdef PLUGGABLE_USB_ENABLED
char name[ISERIAL_MAX_LEN];
PluggableUSB().getShortName(name);
return sendStringDescriptor((uint8_t*)name, setup.wLength);
#endif
}
else {
return false;
}
if (*desc_addr > setup.wLength) {
desc_length = setup.wLength;
}
}
else
{
}
if (desc_addr == 0) {
return false;
}
if (desc_length == 0) {
desc_length = *desc_addr;
}
sendControl(desc_addr, desc_length);
return true;
}
void XimeaController::start()
{
sendControl( cuttlefish_msgs::XimeaControl_CommandType_START );
}
