void goodix_ts_scan_work(struct work_struct *work)
{
struct goodix_ts *ts= container_of(work, struct goodix_ts, scan_work);
int err = 0;
err = read_position(ts);
//finger up
if (err == NO_TOUCH)
{
DPRINTK("finger up\n");
input_report_key(ts->input_dev,BTN_TOUCH,0);
input_sync(ts->input_dev);
// gpio_irq_enable(TS_INT_PIN);
}
//finger down.
else
{
DPRINTK("finger down\n");
//pr_info("touch pos:(%d, %d)\n", ts->touch_x, ts->touch_y);
input_report_abs(ts->input_dev,ABS_X,ts->touch_x );
input_report_abs(ts->input_dev,ABS_Y,ts->touch_y );
input_report_key(ts->input_dev,BTN_TOUCH,1);
input_sync(ts->input_dev);
//hrtimer_start(&ts->timer,ktime_set(0,TS_POLL_PERIOD),HRTIMER_MODE_REL);
// gpio_irq_enable(TS_INT_PIN);
}
}
block_result block_database::get(const size_t height) const
{
if (height >= index_.size())
return block_result(nullptr);
const position_type position = read_position(height);
const slab_type slab = allocator_.get(position);
return block_result(slab);
}
uint32_t file_input_stream::bytes_available()
{
assert(m_file.is_open());
uint32_t pos = read_position();
assert(pos <= m_filesize);
return m_filesize - pos;
}
block_result block_database::get(const size_t height) const
{
if (height >= index_.count())
return block_result(nullptr);
const auto position = read_position(height);
const auto slab = allocator_.get(position);
return block_result(slab);
}
static ssize_t show_position(struct device *dev, struct device_attribute *attr, char *buf)
{
accel_data x;
accel_data y;
accel_data z;
read_position(dev,&x,&y,&z);
return sprintf(buf, "(%d,%d,%d)", x, y, z);
}
bool FileHandle::at_end_of_stream()
{
uint32_t size = 0u;
auto size_result = stream_size(size);
if (size_result != SUCCESS)
{
return false;
}
return read_position() >= static_cast<int32_t>(size);
}
static int accel_probe(struct hid_device *dev, const struct hid_device_id *id)
{
//struct hid_accel *data = NULL;
int retval = -ENOMEM;
printk(KERN_INFO "Probing device\n");
dev_info(&dev->dev, "Found USB Accelerometer\n");
// Set up HID Report Parser
retval = hid_parse(dev);
if (retval)
goto error;
//data = kzalloc(sizeof(struct hid_accel), GFP_KERNEL);
//if (data == NULL) {
// dev_err(&dev->dev, "Out of memory\n");
// goto error_mem;
//}
retval = device_create_file(&dev->dev, &dev_attr_position);
if (retval)
goto error;
retval = device_create_file(&dev->dev, &dev_attr_calibrate);
if (retval)
goto error;
retval = device_create_file(&dev->dev, &dev_attr_recalibrate);
if (retval)
goto error;
read_position(&dev->dev, &rest_x, &rest_y, &rest_z);
dev_info(&dev->dev, "USB Accelerometer device now attached\n");
return 0;
error:
err_hid("There was an error\n");
device_remove_file(&dev->dev, &dev_attr_position);
device_remove_file(&dev->dev, &dev_attr_calibrate);
device_remove_file(&dev->dev, &dev_attr_recalibrate);
// kfree(dev);
error_mem:
return retval;
}
int main (int argc, char** argv){
// Check for hdaps module
FILE* fd_mod;
char line[BUF_LEN_LONG], mod[BUF_LEN_SHORT];
fd_mod = fopen("/proc/modules", "r");
if(!fd_mod){
perror("open /proc/modules");
exit(EXIT_FAILURE);
}
while(fgets(line, sizeof(line), fd_mod)){
sscanf(line, "%s", mod);
if(strcmp(mod, "hdaps") == 0)
break;
}
fclose(fd_mod);
if(strcmp(mod, "hdaps") != 0){
fprintf(stderr, "Please load the hdaps module first:\n\n# modprobe hdaps\n\n");
exit(EXIT_FAILURE);
}
static int verbose;
int thresh_left = THRESH_LEFT;
int thresh_right = THRESH_RIGHT;
int sleep_interval = SLEEP_INTERVAL;
char *exec = EXECUTE;
int c;
while (1){
static struct option long_options[] = {
{"verbose", no_argument, &verbose, 'v'},
{"threshold-left", required_argument, 0, 'l'},
{"threshold-right", required_argument, 0, 'r'},
{"interval", required_argument, 0, 'i'},
{"execute", required_argument, 0, 'e'},
{0, 0, 0, 0}
};
int option_index = 0;
c = getopt_long(argc, argv, "vl:r:i:e:",
long_options, &option_index);
if(c == -1) break;
switch (c){
case 0:
break;
case 'v':
verbose = 1;
break;
case 'r':
printf ("Threshold right is now %s\n", optarg);
thresh_right = atoi(optarg);
break;
case 'l':
printf ("Threshold left is now %s\n", optarg);
thresh_left = atoi(optarg);
break;
case 'i':
printf ("Sleeping interval is now %s\n", optarg);
sleep_interval = atoi(optarg);
break;
case 'e':
printf("Execute command is now %s\n", optarg);
exec = optarg;
break;
default:
printf("Usage: %s [OPTIONS]\n", argv[0]);
printf("Options:\n");
printf("\t-v, --verbose\t\t\tPrint out thresholds and measurements\n");
printf("\t-l, --threshold-left=I\t\tThreshold for rotating left\n");
printf("\t-r, --threshold-right=I\t\tThreshold for rotating right\n");
printf("\t-i, --interval=i\t\tPolling interval\n");
printf("\t-e, --execute=SCRIPT\t\tScript to execute on rotation. The script will be passed one parameter: left, right or normal\n");
exit(EXIT_FAILURE);
}
}
int ret, x, y;
orientation_t rot = NORMAL;
char buf[256];
if(verbose) printf("Left threshold\tMeasurement\tRight threshold\n");
/* main loop */
while(1){
ret = read_position(&x, &y);
if(ret < 0)
return 1;
if(x <= thresh_left){
if(rot != LEFT){
rot = LEFT;
snprintf(buf, 256, "%s %s", exec, "left");
system(buf);
}
} else if(x >= thresh_right){
if(rot != RIGHT){
rot = RIGHT;
snprintf(buf, 256, "%s %s", exec, "right");
system(buf);
}
} else {
if(rot != NORMAL){
rot = NORMAL;
snprintf(buf, 256, "%s %s", exec, "normal");
system(buf);
}
}
if(verbose){
switch(rot){
case LEFT:
printf("[%i]\t%i\t%i\n", THRESH_LEFT, x, THRESH_RIGHT);
break;
case RIGHT:
printf("%i\t%i\t[%i]\n", THRESH_LEFT, x, THRESH_RIGHT);
break;
default:
printf("%i\t[%i]\t%i\n", THRESH_LEFT, x, THRESH_RIGHT);
break;
}
}
nanosleep((struct timespec[]){{0, sleep_interval * 1000000}}, NULL);
}
IOResult FileHandle::read(uint32_t count, uint8_t *buffer)
{
auto result = _fs->read(_entry_id, read_position(), count, buffer);
change_read_position(result.bytes());
return result;
}
/** Find an arbitrary number of delimited strings at the end of a file. This
* function provides the core of our last-lines-loader implementation.
*
* @param fd File descriptor to operate on. This must be opened for reading.
*
* @param bytes Delimiter sequence.
*
* @param bytes_length Number of significant bytes in @p bytes.
*
* @param count Number of tail sequences to find.
*
* @param out Destination string array.
*/
static inline size_t
find_tail_sequences(int fd, const char* bytes, size_t bytes_length, size_t count, wxArrayString& out)
{
size_t count_added ( 0 );
/* We overlap the file reads a little to avoid splitting (and thus missing) the
delimiter sequence. */
const size_t read_overlap ( bytes_length - 1 );
const size_t read_size ( BUFSIZ );
const off_t log_length ( lseek(fd, 0, SEEK_END) );
bool have_last_pos ( false );
char buf[read_size];
off_t last_found_pos ( 0 );
off_t last_read_position ( log_length + read_overlap );
/* We read `read_size'-byte blocks of the file, starting at the end and working backwards. */
while ( count_added < count && last_read_position > 0 ) {
off_t read_position ( next_read_position(last_read_position, read_size, read_overlap) );
size_t bytes_read ( pread(fd, buf, std::min(static_cast<off_t>(read_size), last_read_position - read_position), read_position) );
/* In each block, we search for `bytes', starting at the end. */
for ( ssize_t i = bytes_read - read_overlap - 1; i >= 0; i-- ) {
if ( !strncmp((buf + i), bytes, bytes_length) ) {
off_t this_found_pos ( read_position + i );
if ( have_last_pos && count_added < count ) {
size_t line_length ( last_found_pos - this_found_pos - bytes_length );
if ( line_length > 0 ) {
char* source ( NULL );
if ( last_found_pos >= read_position + (off_t) bytes_read ) {
source = new char[ line_length + 1];
memset(source, 0, line_length + 1);
if ( pread(fd, source, line_length, this_found_pos + bytes_length) < (ssize_t) line_length ) {
wxLogWarning(_T("ChatLog::find_tail_sequences: Read-byte count less than expected"));
}
} else {
source = buf + i + bytes_length;
}
if ( strncmp(source, "##", 2) != 0 ) {
out.Insert(wxString(L'\0', 0), 0);
wxLogMessage(_T("ChatLog::find_tail_sequences: fetching write buffer for %lu bytes"), sizeof(wxChar) * (line_length + 1));
#if !defined(HAVE_WX28) || defined(SL_QT_MODE)
wxStringBufferLength outzero_buf(out[0], sizeof(wxChar) * (line_length + 1));
wxConvUTF8.ToWChar(outzero_buf, line_length, source);
outzero_buf.SetLength(line_length);
#else
wxConvUTF8.MB2WC(out[0].GetWriteBuf(sizeof(wxChar) * (line_length + 1)), source, line_length);
out[0].UngetWriteBuf(line_length);
#endif
++count_added;
}
if ( last_found_pos >= read_position + (off_t) bytes_read )
delete[] source;
if ( count_added >= count )
i = -1; /* short-circuit the `for' loop. */
}
}
last_found_pos = this_found_pos;
have_last_pos = true;
i -= bytes_length - 1;
}
}
last_read_position = read_position;
}
return count_added;
}
int main(int argc, char **argv)
{
std::string dash_m = "-m";
std::string dash_tty = "-tty";
std::string dash_baud = "-baud";
if (argc<2) {
ROS_INFO("using default motor_id %d, baud code %d, via /dev/ttyUSB%d",motor_id,baudnum,ttynum);
ROS_INFO("may run with command args, e.g.: -m 2 -tty 1 for motor_id=2 on /dev/ttyUSB1");
}
else {
std::vector <std::string> sources;
for (int i = 1; i < argc; ++i) { // argv[0] is the path to the program, we want from argv[1] onwards
sources.push_back(argv[i]);
}
for (int i=0;i<argc-2;i++) { // if have a -m or -tty, MUST have at least 2 args
std::cout<<sources[i]<<std::endl;
if (sources[i]==dash_m) {
std::cout<<"found dash_m"<<std::endl;
motor_id = atoi(sources[i+1].c_str());
}
if (sources[i]==dash_tty) {
std::cout<<"found dash_tty"<<std::endl;
ttynum = atoi(sources[i+1].c_str());
}
if (sources[i]==dash_baud) {
std::cout<<"found dash_baud"<<std::endl;
baudnum = atoi(sources[i+1].c_str());
}
}
ROS_INFO("using motor_id %d at baud code %d via /dev/ttyUSB%d",motor_id,baudnum,ttynum);
}
char node_name[50];
char in_topic_name[50];
char out_topic_name[50];
sprintf(node_name,"dynamixel_motor%d",motor_id);
ROS_INFO("node name: %s",node_name);
sprintf(in_topic_name,"dynamixel_motor%d_cmd",motor_id);
ROS_INFO("input command topic: %s",in_topic_name);
sprintf(out_topic_name,"dynamixel_motor%d_ang",motor_id);
ROS_INFO("output topic: %s",out_topic_name);
char in_topic_toggle[50];
sprintf(in_topic_toggle,"dynamixel_motor%d_mode",motor_id);
ros::init(argc,argv,node_name); //name this node
ros::NodeHandle n; // need this to establish communications with our new node
ros::Publisher pub_jnt = n.advertise<std_msgs::Int16>(out_topic_name, 1);
double dt= 0.01; // 100Hz
ROS_INFO("attempting to open /dev/ttyUSB%d",ttynum);
bool open_success = open_dxl(ttynum,baudnum);
if (!open_success) {
ROS_WARN("could not open /dev/ttyUSB%d; check permissions?",ttynum);
return 0;
}
ROS_INFO("attempting communication with motor_id %d at baudrate code %d",motor_id,baudnum);
ros::Subscriber subscriber = n.subscribe(in_topic_name,1,dynamixelCB);
ros::Subscriber toggle_subscriber = n.subscribe(in_topic_toggle,1,toggleCB);
std_msgs::Int16 motor_ang_msg;
short int sensed_motor_ang=0;
torque_control_toggle(motor_id,0); // set motor to torque mode during initialization
// set position/torque limits during initialization
set_torque_max(motor_id,torque_max);
set_dynamixel_CW_limit(motor_id,CW_limit);
set_dynamixel_CCW_limit(motor_id,CCW_limit);
while(ros::ok()) {
sensed_motor_ang = read_position(motor_id);
if (sensed_motor_ang>4096) {
ROS_WARN("read error from Dynamixel: ang value %d at cmd %d",sensed_motor_ang-4096,g_goal_cmd);
}
motor_ang_msg.data = sensed_motor_ang;
pub_jnt.publish(motor_ang_msg);
ros::Duration(dt).sleep();
ros::spinOnce();
}
dxl_terminate();
ROS_INFO("goodbye");
return 0; // should never get here, unless roscore dies
}
static ssize_t set_recalibrate(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
{
if (*buf == '1')
read_position(dev, &rest_x, &rest_y, &rest_z);
return count;
}
