int mpu9150_read_dmp(mpudata_t *mpu)
{
short sensors;
unsigned char more;
if (data_ready() == 0)
return -1;
if (dmp_read_fifo(mpu->rawGyro, mpu->rawAccel, mpu->rawQuat, &mpu->dmpTimestamp, &sensors, &more) < 0) {
printk(KERN_WARNING "dmp_read_fifo() failed\n");
sprintf(status_string,"dmp_read_fifo() failed\n");
return -1;
}
while (more) {
// Fell behind, reading again
if (dmp_read_fifo(mpu->rawGyro, mpu->rawAccel, mpu->rawQuat, &mpu->dmpTimestamp, &sensors, &more) < 0) {
printk(KERN_WARNING "dmp_read_fifo() failed\n");
sprintf(status_string,"dmp_read_fifo() failed\n");
return -1;
}
}
return 0;
}
int mpu9150_read_dmp(mpudata_t *mpu)
{
short sensors;
unsigned char more;
if (!data_ready()) {
printf("data_ready() - data not ready.\n");
return -1;
}
if (dmp_read_fifo(mpu->rawGyro, mpu->rawAccel, mpu->rawQuat, &mpu->dmpTimestamp, &sensors, &more) < 0) {
printf("dmp_read_fifo() failed\n");
return -1;
}
while (more) {
// Fell behind, reading again
if (dmp_read_fifo(mpu->rawGyro, mpu->rawAccel, mpu->rawQuat, &mpu->dmpTimestamp, &sensors, &more) < 0) {
printf("dmp_read_fifo() failed\n");
return -1;
}
}
return 0;
}
int mpu9250_read_dmp(mpudata_t *mpu)
{
//printf("read dmp\n");
short sensors;
unsigned char more;
if (!data_ready()){
//printf("dmp data not ready: %d\n",data_ready());
//linux_delay_ms(1);
return -1;
}
if (dmp_read_fifo(mpu->rawGyro, mpu->rawAccel, mpu->rawQuat, &mpu->dmpTimestamp, &sensors, &more) < 0) {
printf("dmp_read_fifo() failed\n");
return -1;
}
while (more) {
// Fell behind, reading again
if (dmp_read_fifo(mpu->rawGyro, mpu->rawAccel, mpu->rawQuat, &mpu->dmpTimestamp, &sensors, &more) < 0) {
printf("dmp_read_fifo() failed\n");
return -1;
}
}
return 0;
}
void UHandler::conversion_request()
{
/*// fprintf (stderr, "UHandler: conversion started.\n");
for (int a = 0; a < 6; a++)
{
// fprintf (stderr, "UHandler: entering the loop.\n");
// fprintf (stderr, "UHandler: usb_control_msg().\n");
nbytes = usb_control_msg(
handle,
USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_IN,
REPORT_VALUES, //request ID - look for further details in firmware source code
0, // wValue, can contain some useful instructions
0, // wIndex, has the same meaning as the wValue does
(char *) buffer,
sizeof(buffer),
1000 // timeout in milliseconds
);
// fprintf (stderr, "UHandler: usb_control_msg() done. Completing the buffer.\n");
readbuf = buffer[0];
results[a] = readbuf*ccof;
// fprintf (stderr, "UHandler: going for string_data[].\n");
// fprintf (stderr, "Uhandler: doing sume business with pack.data[].\n");
pack.data[a] = QString::number (results[a], 'g', 6);
}
// fprintf (stderr, "UHandler: conversion finished.\n");
*/
//TODO: uncomment after burning of a new firmware.
nbytes = usb_control_msg(
handle,
USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_IN,
REPORT_ALL_CHANNELS,
0,
0,
(char *) buffer,
sizeof (buffer),
1000);
for (int a = 0; a < 6; a ++)
{
results[a] = buffer[a]*ccof;
fprintf (stderr, "Result %d = %f\n", a, results[a]);
pack.data[a] = QString::number (results[a], 'g', 6);
}
emit data_ready ();
emit data_ready (&pack);
}
void KinectHelper::drawColor(){
//Q_ASSERT(!mutex()->tryLock());
/// Don't bother drawing empty image
if(!data_ready())
return;
/// Have you setColorLabel?
if(color_label==NULL)
return;
color_label->setPixmap(QPixmap::fromImage(*color_front_buffer));
}
void KinectHelper::drawColor(QImage *image){
Q_ASSERT(!mutex()->tryLock());
/// Don't bother drawing empty image
if(!data_ready())
return;
/// You didn't give me anything, I will instantiate it!
autoCreateWidgetsIfNecessary();
if(image==NULL)
colorLabel->setPixmap(QPixmap::fromImage(*color_front_buffer)); ///< Maybe this is not even necessary?
else
colorLabel->setPixmap(QPixmap::fromImage(*image)); ///< Maybe this is not even necessary?
}
void KinectHelper::drawCloud(){
// qDebug() << "KinectHelper::drawCloud()" << QThread::currentThreadId();
Q_ASSERT(!mutex()->tryLock());
if(!data_ready()) return;
/// The lock allows us to just use a reference to the buffer
PImage& data = (*points_front_buffer);
glDisable(GL_LIGHTING);
glColor3d(1.0,0.0,0.0);
glBegin(GL_POINTS);
for(int c=0; c<data.cols(); c++)
for(int r=0; r<data.rows(); r++)
glVertex3dv( data(r,c).data() );
glEnd();
glEnable(GL_LIGHTING);
}
/*
* Read messages from server and have AI answer choice queries.
*/
int main(int argc, char *argv[])
{
int i;
/* Read card database */
read_cards();
/* Create choice logs */
for (i = 0; i < MAX_PLAYER; i++)
{
/* Create choice log for player */
real_game.p[i].choice_log = (int *)malloc(sizeof(int) * 4096);
}
/* Loop forever */
while (1)
{
/* Try to read data */
data_ready();
}
}
bool waitForData(lirc_t timeout) {
//================
HANDLE events[2];
int count;
//================
events[0] = dataReadyEvent;
events[1] = threadExitEvent;
count = 2;
if(threadExitEvent==NULL) {
count = 1;
}
if(!data_ready()) {
//=========
int result;
//=========
ResetEvent(dataReadyEvent);
if(timeout) {
result = WaitForMultipleObjects(count,events,FALSE,(timeout+500)/1000);
}
else {
result = WaitForMultipleObjects(count,events,FALSE,INFINITE);
}
if(result==(WAIT_OBJECT_0+1)) {
return false;
}
}
return true;
}
int main()
{
if (setup_stream()) {
exit(1);
}
xsvf_file = fopen("cram.xsvf","rb");
while (1) {
int n;
char c;
while (data_ready()>0) {
read(serial_fd, &c, 1);
if (process_command(c)>0) {
goto sync_ok;
}
}
printf("waiting for sync...\n");
usleep(500000);
c = 0;
write(serial_fd, &c, 1);
usleep(500000);
}
sync_ok:
printf("sync ok.\n");
while (1) {
char c;
if (command_plus()) {
break;
}
do {
read(serial_fd, &c, 1);
} while (process_command(c)<=0);
}
return 0;
}
void Parser::setReader(Reader *reader)
{
connect(reader,SIGNAL(data_ready(QString)), this, SLOT(parser_input(QString)));
}
void HttpRequest::read_ready()
{
if (state == Connected){
string s;
if (!readLine(s)) return;
if (s.length() < strlen(HTTP)){
error_state(ErrorRead);
return;
}
const char *r = strchr(s.c_str(), ' ');
if (r == NULL){
error_state(ErrorRead);
return;
}
r++;
int code = atoi(r);
if (code == 401){
dumpPacket(bIn, 0, "Proxy read");
error_state(ErrorProxyAuth);
return;
}
if (code == 502){
dumpPacket(bIn, 0, "Proxy read");
error_state(ErrorRead);
return;
}
if (code != 200){
dumpPacket(bIn, 0, "Proxy read");
error_state(ErrorProxyConnect);
return;
}
state = ReadHeader;
}
if (state == ReadHeader){
for (;;){
string s;
if (!readLine(s)) return;
if (s.length() == 0){
state = ReadData;
break;
}
string h = s.substr(0, strlen(CONTENT_LENGTH));
if (!strcasecmp(h.c_str(), CONTENT_LENGTH)){
h = s.substr(strlen(CONTENT_LENGTH), s.size());
for (const char *p = h.c_str(); *p; p++){
if ((*p >= '0') && (*p <= '9')){
data_size = atol(p);
break;
}
}
}
}
}
if (state == ReadData){
while (data_size > 0){
char b[2048];
unsigned tail = data_size;
if (tail > sizeof(b)) tail = sizeof(b);
int n = m_sock->read(b, tail);
if (n < 0){
dumpPacket(bIn, 0, "Proxy read");
error_state(ErrorProxyConnect);
return;
}
if (n == 0) break;
bIn.pack(b, n);
data_size -= n;
}
if (data_size == 0){
state = None;
dumpPacket(bIn, 0, "Proxy read");
data_ready();
}
}
}
int main() {
uint8_t resp[4];
int8_t last[2][2];
setup_spi();
setup_nrf();
adxl362_begin();
config_measure_mode(MODE_MEASURE);
DDRA |= (1 << PA0);
_delay_ms(10);
power_up_rx();
while(!data_ready())
power_up_rx();
get_data(resp);
_delay_ms(3);
if(resp[3] = 0xaa) {
ts[0] = 0xbb;
send(ts);
}
read_all_axes_short(ts);
last[0][0] = ts[0];
last[0][1] = ts[1];
last[0][2] = ts[2];
_delay_ms(100);
read_all_axes_short(ts);
last[1][0] = ts[0];
last[1][1] = ts[1];
last[1][2] = ts[2];
_delay_ms(100);
while(42) {
uint8_t ctrl = 0;
read_all_axes_short(ts);
if( fabs((ts[0] - last[1][0]) + (last[1][0] - last[0][0])) < 4 ) {
if(ts[0] > 30)
ctrl |= 0x80;
if(ts[0] < -30)
ctrl |= 0x40;
}
if( fabs((ts[1] - last[1][1]) + (last[1][1] - last[0][1])) < 4 ) {
if(ts[1] > 30)
ctrl |= 0x01;
if(ts[1] < -30)
ctrl |= 0x02;
}
last[0][0] = last[1][0];
last[0][1] = last[1][1];
last[1][0] = ts[0];
last[1][1] = ts[1];
ts[0] = ctrl;
send(ts);
PORTA ^= (1 << PA0);
_delay_ms(100);
}
}
