steffen
/
SimpleSkyCam
2
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity

calibration something is working. don't know what and if it is usefull

Browse Source
master
Steffen Pohle 3 years ago
parent f236912d34
commit 8908f0f82d
4 changed files with 108 additions and 12 deletions
Show Stats Download Patch File Download Diff File

6
detect.h
Unescape View File

@ -44,7 +44,10 @@ enum {
POSCTL_CALIB_MODE_FINISH POSCTL_CALIB_MODE_FINISH
}; };
enum {
POSCTL_DEVTYPE_TTY = 0,
POSCTL_DEVTYPE_SIM
};
class PosCtl { class PosCtl {
@ -53,6 +56,7 @@ class PosCtl {
int mode; int mode;
PID pid_axis[2]; PID pid_axis[2];
std::string device; std::string device;
int device_type;
int device_fd; int device_fd;
struct timeval calib_timestamp; struct timeval calib_timestamp;
int calib_mode; int calib_mode;

45
posctl.cc
Unescape View File

@ -21,6 +21,8 @@
#include "error.h" #include "error.h"
extern PosCtl posctl; extern PosCtl posctl;
extern Simulation simulation;
extern GtkBuilder *_builder_; // work around for threads extern GtkBuilder *_builder_; // work around for threads
GtkWidget *posctl_rot_da = NULL; GtkWidget *posctl_rot_da = NULL;
@ -452,6 +454,7 @@ PosCtl::PosCtl() {
calib_axis2max_len = 0.0; calib_axis2max_len = 0.0;
device_fd = -1; device_fd = -1;
device = ""; device = "";
device_type = POSCTL_DEVTYPE_TTY;
}; };
/* /*
@ -542,14 +545,18 @@ void PosCtl::CalibModeDelta(int x, int y) {
OutputWriteStart(); OutputWriteStart();
fp.x = (x - calib_pos.x) / (float)timediff; fp.x = (x - calib_pos.x) / (float)timediff;
fp.y = (y - calib_pos.y) / (float)timediff; fp.y = -(y - calib_pos.y) / (float)timediff;
LockMutex(); LockMutex();
calib_rot_angle = atan2(fp.x, fp.y) * 180.0 / M_PI; calib_rot_angle = atan2(fp.x, fp.y) * 180.0 / M_PI;
while (calib_rot_angle < 0.0) calib_rot_angle += 360.0;
calib_rot_len = sqrt(fp.x * fp.x + fp.y * fp.y); calib_rot_len = sqrt(fp.x * fp.x + fp.y * fp.y);
UnLockMutex(); UnLockMutex();
calib_mode = POSCTL_CALIB_MODE_AXIS1_MI; calib_mode = POSCTL_CALIB_MODE_AXIS1_MI;
calib_pos.x = x;
calib_pos.y = y;
OutputWriteValue(0, a1min); OutputWriteValue(0, a1min);
gettimeofday (&calib_timestamp, NULL); gettimeofday (&calib_timestamp, NULL);
gdk_threads_add_idle(cb_thread_posctl, NULL); gdk_threads_add_idle(cb_thread_posctl, NULL);
@ -561,6 +568,7 @@ void PosCtl::CalibModeAxis(int x, int y) {
struct timeval tv; struct timeval tv;
float timediff; float timediff;
double a1min, a1max, a2min, a2max; double a1min, a1max, a2min, a2max;
position_f_2d fp;
gettimeofday (&tv, NULL); gettimeofday (&tv, NULL);
timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0); timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
@ -571,20 +579,37 @@ void PosCtl::CalibModeAxis(int x, int y) {
pid_axis[0].GetParam(&a1min, &a1max, NULL, NULL, NULL); pid_axis[0].GetParam(&a1min, &a1max, NULL, NULL, NULL);
pid_axis[1].GetParam(&a2min, &a2max, NULL, NULL, NULL); pid_axis[1].GetParam(&a2min, &a2max, NULL, NULL, NULL);
fp.x = (x - calib_pos.x) / (float)timediff;
fp.y = -(y - calib_pos.y) / (float)timediff;
if (calib_mode == POSCTL_CALIB_MODE_AXIS1_MI) { if (calib_mode == POSCTL_CALIB_MODE_AXIS1_MI) {
calib_axis1min_angle = atan2(fp.x, fp.y) * 180.0 / M_PI;
while (calib_axis1min_angle < 0.0) calib_axis1min_angle += 360.0;
calib_axis1min_len = sqrt(fp.x * fp.x + fp.y * fp.y);
OutputWriteValue(0, a1max); OutputWriteValue(0, a1max);
} }
else if (calib_mode == POSCTL_CALIB_MODE_AXIS1_MA) { else if (calib_mode == POSCTL_CALIB_MODE_AXIS1_MA) {
OutputWriteValue(0, a2min); calib_axis1max_angle = atan2(fp.x, fp.y) * 180.0 / M_PI;
while (calib_axis1max_angle < 0.0) calib_axis1max_angle += 360.0;
calib_axis1max_len = sqrt(fp.x * fp.x + fp.y * fp.y);
OutputWriteValue(1, a2min);
} }
else if (calib_mode == POSCTL_CALIB_MODE_AXIS2_MI) { else if (calib_mode == POSCTL_CALIB_MODE_AXIS2_MI) {
OutputWriteValue(0, a2max); calib_axis2min_angle = atan2(fp.x, fp.y) * 180.0 / M_PI;
while (calib_axis2min_angle < 0.0) calib_axis2min_angle += 360.0;
calib_axis2min_len = sqrt(fp.x * fp.x + fp.y * fp.y);
OutputWriteValue(1, a2max);
} }
else if (calib_mode == POSCTL_CALIB_MODE_AXIS2_MA) { else if (calib_mode == POSCTL_CALIB_MODE_AXIS2_MA) {
calib_axis2max_angle = atan2(fp.x, fp.y) * 180.0 / M_PI;
while (calib_axis2max_angle < 0.0) calib_axis2max_angle += 360.0;
calib_axis2max_len = sqrt(fp.x * fp.x + fp.y * fp.y);
OutputWriteStart(); // reset speed output to 50% OutputWriteStart(); // reset speed output to 50%
} }
calib_mode++; calib_mode++;
calib_pos.x = x;
calib_pos.y = y;
gettimeofday (&calib_timestamp, NULL); gettimeofday (&calib_timestamp, NULL);
gdk_threads_add_idle(cb_thread_posctl, NULL); gdk_threads_add_idle(cb_thread_posctl, NULL);
} }
@ -635,8 +660,12 @@ void PosCtl::Loop (int posx, int posy) {
void PosCtl::SetDevice (std::string d) { void PosCtl::SetDevice (std::string d) {
printf ("%s:%d %s new device:%s\n", __FILE__, __LINE__, __FUNCTION__, d.c_str()); printf ("%s:%d %s new device:%s\n", __FILE__, __LINE__, __FUNCTION__, d.c_str());
OutputClose(); OutputClose();
device = d; device = d;
if (d.compare ("SIMULATION") == 0) device_type = POSCTL_DEVTYPE_SIM;
else device_type = POSCTL_DEVTYPE_TTY;
}; };
@ -718,6 +747,11 @@ int PosCtl::OutputWriteValue (int axis, double value) {
char outbuf[255]; char outbuf[255];
printf ("%s:%d %s Axis %d Value:%f\n", __FILE__, __LINE__, __FUNCTION__, axis, value); printf ("%s:%d %s Axis %d Value:%f\n", __FILE__, __LINE__, __FUNCTION__, axis, value);
if (device_type == POSCTL_DEVTYPE_SIM) {
simulation.AxisSetValue(axis, value);
return 0;
}
if (device_fd <= 0) if (OutputOpen() != 0) return -1; if (device_fd <= 0) if (OutputOpen() != 0) return -1;
// //
@ -742,6 +776,11 @@ int PosCtl::OutputWriteStop () {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
if (device_type == POSCTL_DEVTYPE_SIM) {
simulation.AxisStop();
return 0;
}
snprintf (outbuf, 255, ":Q#"); snprintf (outbuf, 255, ":Q#");
if (device_fd <= 0) if (OutputOpen() != 0) return -1; if (device_fd <= 0) if (OutputOpen() != 0) return -1;
WriteTTY(outbuf); WriteTTY(outbuf);

55
videodev-simulation.cc
Unescape View File

@ -99,6 +99,7 @@ int VideoDev_Simulation::CaptureStart() {
if (inframe != NULL) Close(); if (inframe != NULL) Close();
if (Open() != VDEV_STATUS_OK) return VDEV_STATUS_ERROR; if (Open() != VDEV_STATUS_OK) return VDEV_STATUS_ERROR;
ConvertStart(&cdata, V4L2_PIX_FMT_RGB24); ConvertStart(&cdata, V4L2_PIX_FMT_RGB24);
get_cycletime(&lastframetv);
return VDEV_STATUS_OK; return VDEV_STATUS_OK;
}; };
@ -121,7 +122,11 @@ int VideoDev_Simulation::CaptureStop() {
#define SIMULATION_SIZE 15 #define SIMULATION_SIZE 15
int VideoDev_Simulation::Grab(VideoFrame *vf) { int VideoDev_Simulation::Grab(VideoFrame *vf) {
int posx, posy, x ,y; int posx, posy, x ,y;
double r, a; double r, a, dsec;
// try to match a speed of 20Hz
dsec = get_cycletime(&lastframetv);
if (dsec < 0.040) usleep (50000 - dsec*1000.0);
memset (inframe, 0x0, w*h*3); memset (inframe, 0x0, w*h*3);
simulation.GetPos(&posx, &posy); simulation.GetPos(&posx, &posy);
@ -174,15 +179,35 @@ gpointer simulation_threadprocess_wrapper (gpointer data) {
} }
Simulation::Simulation() { Simulation::Simulation() {
int i;
posX = 900.0; posX = 900.0;
posY = 500.0; posY = 500.0;
running = 0; running = 0;
time_t t = time(NULL); time_t t = time(NULL);
srandom (t); srandom (t);
//
// object movement
dAngle = 2.0 * M_PI * (double)random() / (double) RAND_MAX; dAngle = 2.0 * M_PI * (double)random() / (double) RAND_MAX;
dLen = 5.0 + 10.0 * (double)random() / (double) RAND_MAX; dLen = 2.5 + 5.0 * (double)random() / (double) RAND_MAX;
printf ("%s:%d %s dAngle:%f dLen:%f\n", __FILE__, __LINE__, __FUNCTION__, dAngle, dLen); printf ("%s:%d %s dAngle:%f dLen:%f\n", __FILE__, __LINE__, __FUNCTION__, dAngle, dLen);
//
// axis movement
i = random() % 2;
axis[i % 2].defAngle = dAngle + (10.0 * (double)random() / ((double) RAND_MAX)) - 5.0;
axis[i % 2].defLen = dLen + (1.0 * (double)random() / ((double) RAND_MAX) - 0.5);
axis[i % 2].v = 1.0;
axis[i % 2].active = 1;
i++;
axis[i % 2].defAngle = dAngle + (10.0 * (double)random() / ((double) RAND_MAX)) + 85.0;
axis[i % 2].defLen = dLen + (1.0 * (double)random() / ((double) RAND_MAX) - 0.5);
axis[i % 2].v = 1.0;
axis[i % 2].active = 1;
}; };
Simulation::~Simulation() { Simulation::~Simulation() {
@ -218,18 +243,21 @@ void Simulation::ThreadProcess() {
// //
// simulate rotation movement // simulate rotation movement
// add random error to values
// calculate movement // calculate movement
dx = sin(dAngle) * dLen * ms; dx = sin(dAngle) * dLen * ms;
dy = cos(dAngle) * dLen * ms; dy = -cos(dAngle) * dLen * ms;
posX += dx; posX += dx;
posY += dy; posY += dy;
// //
// simulate motor axis movement // simulate motor axis movement
for (int i = 0; i < 2; i++)
if (axis[i].active) {
dx = sin(axis[i].defAngle) * axis[i].defLen * axis[i].v * ms;
dy = -cos(axis[i].defAngle) * axis[i].defLen * axis[i].v * ms;
posX -= dx;
posY -= dy;
}
if (posX < 0) posX = 1920.0 - 1.0; if (posX < 0) posX = 1920.0 - 1.0;
if (posY < 0) posY = 1080.0 - 1.0; if (posY < 0) posY = 1080.0 - 1.0;
@ -250,4 +278,17 @@ void Simulation::Stop() {
} }
void Simulation::AxisSetValue (int a, double v) {
printf ("%s:%d %s Axis:%d Value:%f\n", __FILE__, __LINE__, __FUNCTION__, a, v);
if (a < 0 || a > 1) return;
axis[a].v = v;
axis[a].active = 1;
}
void Simulation::AxisStop() {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
axis[0].active = 1;
axis[1].active = 1;
}

14
videodev-simulation.h
Unescape View File

@ -27,6 +27,14 @@
#include "videodev.h" #include "videodev.h"
struct SimAxisCtl {
int active;
double v;
double defLen;
double defAngle;
};
class Simulation { class Simulation {
private: private:
float posX; float posX;
@ -34,6 +42,7 @@ private:
float dAngle; float dAngle;
float dLen; float dLen;
int running; int running;
struct SimAxisCtl axis[2];
GMutex mutex; GMutex mutex;
public: public:
@ -45,6 +54,9 @@ public:
void Start(); void Start();
void Stop(); void Stop();
void AxisSetValue(int a, double v);
void AxisStop();
void LockMutex() { g_mutex_lock(&mutex); }; void LockMutex() { g_mutex_lock(&mutex); };
void UnLockMutex() { g_mutex_unlock(&mutex); }; void UnLockMutex() { g_mutex_unlock(&mutex); };
}; };
@ -54,7 +66,7 @@ class VideoDev_Simulation: public VideoDev {
private: private:
uint32_t w; uint32_t w;
uint32_t h; uint32_t h;
struct timeval starttv; struct timeval lastframetv;
unsigned char *inframe; unsigned char *inframe;
ConvertData cdata; ConvertData cdata;
GThread *simulation_thread; GThread *simulation_thread;

Write Preview
Loading…
Cancel
Save