void vrpn_LUDL_USBMAC6000::flush_input_from_ludl(void)
{
// Clear the input buffer, read all available characters
// from the endpoint we're supposed to use, then clear it
// again -- throwing away all data that was coming from the device.
_incount = 0;
check_for_data();
_incount = 0;
}
int wait_ack_reply() {
int ret = -ENOMEM;
struct anspass_packet *pkt = (struct anspass_packet*)calloc(1,
sizeof(struct anspass_packet));
if (!pkt)
goto no_pkt_mem;
struct timeval *to = (struct timeval*)calloc(1, sizeof(struct timeval));
if (!to)
goto no_to_mem;
to->tv_sec = 1;
to->tv_usec = 0;
pkt->socket = info.socket;
pkt->to = to;
ret = check_for_data(pkt);
if (!ret)
{
printf("check timed out.\n");
goto no_data;
}
if (ret < 0) {
ret = -errno;
printf("Error checking for data: %d\n", ret);
goto error_data;
}
ret = -EIO;
pkt->ret = -EIO;
if(get_data(pkt))
{
if (ACK == pkt->type)
ret = pkt->ret;
}
error_data:
no_data:
free(to);
no_to_mem:
free(pkt);
no_pkt_mem:
return ret;
}
// The first axis is 1 in this function.
bool vrpn_LUDL_USBMAC6000::ludl_axis_position(unsigned axis, vrpn_int32 *position_return)
{
// Request the position of the axis.
flush_input_from_ludl();
if (!send_usbmac_command(axis, LUDL_GET_LONG_DATA, LUDL_MOTOR_POSITION, 0)) {
REPORT_ERROR("vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not send command 1");
return false;
}
// Read from the device to find the status. We call the check_for_data() method
// to look for a response.
unsigned watchdog = 0;
while (_incount == 0) {
if (!check_for_data()) {
REPORT_ERROR("vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not get report");
return false;
}
// If it has been too long, re-send the request to the stage.
// XXX We should not be losing characters... figure out what is causing us to
// have to resend.
if (++watchdog == 25) { // 25 ms (timeout is 1ms)
if (!send_usbmac_command(axis, LUDL_GET_LONG_DATA, LUDL_MOTOR_POSITION, 0)) {
REPORT_ERROR("vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not resend command 1");
return false;
}
watchdog = 0;
}
}
int position = 0;
int device, command, index;
if (!interpret_usbmac_ascii_response(_inbuffer, &device, &command, &index, &position)) {
REPORT_ERROR("vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not parse report");
return false;
}
_incount = 0; // XXX Should parse more than one report if there is one.
if ( (command != LUDL_GET_LONG_DATA) || (index != LUDL_MOTOR_POSITION) ) {
REPORT_ERROR("vrpn_LUDL_USBMAC6000::ludl_axis_position(): Bad command or index in report");
return false;
}
*position_return = position;
return true;
}
// I got the algorithm for checking if the axis is moving
// from the code in USBMAC6000.cpp from
// the video project, as implemented by Ryan Schubert at UNC.
// The first axis is 1 in this function.
bool vrpn_LUDL_USBMAC6000::ludl_axis_moving(unsigned axis)
{
// Request the status of the axis. In particular, we look at the
// bits telling whether each axis is busy.
flush_input_from_ludl();
if (!send_usbmac_command(LUDL_INTERFACE_ADDRESS, LUDL_GET_LONG_DATA, LUDL_MODULE_BUSY, 0)) {
REPORT_ERROR("vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not send command 1");
return false;
}
// Read from the device to find the status. We call the check_for_data() method
// to look for a response.
unsigned watchdog = 0;
while (_incount == 0) {
if (!check_for_data()) {
REPORT_ERROR("vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not get report");
return false;
}
// If it has been too long, re-send the request to the stage.
// XXX We should not be losing characters... figure out what is causing us to
// have to resend.
if (++watchdog == 25) { // 25 ms (timeout is 1ms)
if (!send_usbmac_command(LUDL_INTERFACE_ADDRESS, LUDL_GET_LONG_DATA, LUDL_MODULE_BUSY, 0)) {
REPORT_ERROR("vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not resend command 1");
return false;
}
watchdog = 0;
}
}
int status = 0;
int device, command, index;
if (!interpret_usbmac_ascii_response(_inbuffer, &device, &command, &index, &status)) {
REPORT_ERROR("vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not parse report");
return false;
}
_incount = 0; // XXX Should parse more than one report if there is one.
int axisMaskBit = 0x0001 << axis;
return (status & axisMaskBit) != 0;
}
void poll_inputs()
{
short changed = 0;
short button_state = 0;
short i;
short *pOldValue = old_sensor_values;
sensor_t *pSensor = &sensors[0];
char packet_available;
throttle_count--;
if( throttle_count == 0){
throttle_count = throttle;
// If we're not polling or someone already has the monitor
// return.
if (!poller || get_monitor_count((&(poller->_super))) != 0)
return;
// We do not have a thread that we can use to grab
// the monitor but that's OK because we are atomic
// anyway.
// Check the sensor canonical values.
for (i = 1<<SENSOR_POS; i<(1<<BUTTON_POS); i <<= 1, pOldValue++, pSensor++)
{
if (*pOldValue != pSensor->value) {
changed |= i;
*pOldValue = pSensor->value;
}
}
// Check the button status
read_buttons (0x3000, &button_state);
button_state <<= BUTTON_POS; // Shift into poll position
changed |= button_state ^ old_button_state;
old_button_state = button_state;
// Check serial status
check_for_data ( &packet_available, null);
if (packet_available) {
changed |= 1 << SERIAL_RECEIVED_POS;
}
// Only wake threads up if things have changed since
// we last looked.
if (changed)
{
// Or in the latest changes. Some threads may not have
// responded to earlier changes yet so we can't
// just overwrite them.
short jword = 0;
store_word((byte*)(&jword), 2, changed);
poller->changed |= jword;
#if DEBUG_POLL
jword = get_word((byte*)&poller->changed, 2);
printf("Poller: poller->changed = 0x%1X\n", jword);
#endif
// poller.notifyAll()
monitor_notify_unchecked(&poller->_super, 1);
}
}
}