mirror of
https://github.com/gtxaspec/wz_mini_hacks.git
synced 2024-09-20 00:11:30 +00:00
69 lines
1.8 KiB
C
69 lines
1.8 KiB
C
#include <pthread.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <sys/time.h>
|
|
|
|
extern int local_sdk_motor_get_position(float *step,float *angle);
|
|
extern int local_sdk_motor_move_abs_angle(float pan, float tilt, int speed, void (*done)(float a, float b), void (*canceled)(void), int mode);
|
|
extern void CommandResponse(int fd, const char *res);
|
|
|
|
int MotorFd = 0;
|
|
struct timeval MotorLastMovedTime = { 0, 0 };
|
|
|
|
static void motor_move_done(float pan, float tilt) {
|
|
|
|
if(MotorFd) {
|
|
static char motorResBuf[256];
|
|
sprintf(motorResBuf, "%f %f\n", pan, tilt);
|
|
CommandResponse(MotorFd, motorResBuf);
|
|
}
|
|
MotorFd = 0;
|
|
struct timeval tv;
|
|
gettimeofday(&MotorLastMovedTime, NULL);
|
|
}
|
|
|
|
static void motor_move_canceled() {
|
|
|
|
if(MotorFd) CommandResponse(MotorFd, "error");
|
|
MotorFd = 0;
|
|
gettimeofday(&MotorLastMovedTime, NULL);
|
|
}
|
|
|
|
char *MotorMove(int fd, char *tokenPtr) {
|
|
|
|
char *p = strtok_r(NULL, " \t\r\n", &tokenPtr);
|
|
if(!p) {
|
|
float pan; // 0-355
|
|
float tilt; // 0-180
|
|
int ret = local_sdk_motor_get_position(&pan, &tilt);
|
|
static char motorResBuf[256];
|
|
if(!ret) {
|
|
sprintf(motorResBuf, "%f %f\n", pan, tilt);
|
|
} else {
|
|
sprintf(motorResBuf, "- -\n");
|
|
}
|
|
return motorResBuf;
|
|
}
|
|
float pan = atof(p); // 0-355
|
|
if((pan < 0.0) || (pan > 355.0)) return "error";
|
|
|
|
p = strtok_r(NULL, " \t\r\n", &tokenPtr);
|
|
if(!p) return "error";
|
|
float tilt = atof(p); // 0-180
|
|
if((tilt < 0.0) || (tilt > 180.0)) return "error";
|
|
|
|
p = strtok_r(NULL, " \t\r\n", &tokenPtr);
|
|
int pri = 2; // 0: high - 3: low
|
|
if(p) pri = atoi(p);
|
|
if(pri < 0) pri = 0;
|
|
if(pri > 3) pri = 3;
|
|
|
|
if(MotorFd) return "error";
|
|
MotorFd = fd;
|
|
|
|
int speed = 9;
|
|
int res = local_sdk_motor_move_abs_angle(pan, tilt, speed, &motor_move_done, &motor_move_canceled, pri);
|
|
return NULL;
|
|
}
|