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
};
enum {
POSCTL_DEVTYPE_TTY = 0,
POSCTL_DEVTYPE_SIM
};
class PosCtl {
@ -53,6 +56,7 @@ class PosCtl {
int mode;
PID pid_axis[2];
std::string device;
int device_type;
int device_fd;
struct timeval calib_timestamp;
int calib_mode;

45
posctl.cc
Unescape View File

@ -21,6 +21,8 @@
#include "error.h"
extern PosCtl posctl;
extern Simulation simulation;
extern GtkBuilder *_builder_; // work around for threads
GtkWidget *posctl_rot_da = NULL;
@ -452,6 +454,7 @@ PosCtl::PosCtl() {
calib_axis2max_len = 0.0;
device_fd = -1;
device = "";
device_type = POSCTL_DEVTYPE_TTY;
};
/*
@ -542,14 +545,18 @@ void PosCtl::CalibModeDelta(int x, int y) {
OutputWriteStart();
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();
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);
UnLockMutex();
calib_mode = POSCTL_CALIB_MODE_AXIS1_MI;
calib_pos.x = x;
calib_pos.y = y;
OutputWriteValue(0, a1min);
gettimeofday (&calib_timestamp, NULL);
gdk_threads_add_idle(cb_thread_posctl, NULL);
@ -561,6 +568,7 @@ void PosCtl::CalibModeAxis(int x, int y) {
struct timeval tv;
float timediff;
double a1min, a1max, a2min, a2max;
position_f_2d fp;
gettimeofday (&tv, NULL);
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[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) {
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);
}
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) {
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) {
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%
}
calib_mode++;
calib_pos.x = x;
calib_pos.y = y;
gettimeofday (&calib_timestamp, 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) {
printf ("%s:%d %s new device:%s\n", __FILE__, __LINE__, __FUNCTION__, d.c_str());
OutputClose();
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];
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;
//
@ -742,6 +776,11 @@ int PosCtl::OutputWriteStop () {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
if (device_type == POSCTL_DEVTYPE_SIM) {
simulation.AxisStop();
return 0;
}
snprintf (outbuf, 255, ":Q#");
if (device_fd <= 0) if (OutputOpen() != 0) return -1;
WriteTTY(outbuf);

55
videodev-simulation.cc
Unescape View File

@ -99,6 +99,7 @@ int VideoDev_Simulation::CaptureStart() {
if (inframe != NULL) Close();
if (Open() != VDEV_STATUS_OK) return VDEV_STATUS_ERROR;
ConvertStart(&cdata, V4L2_PIX_FMT_RGB24);
get_cycletime(&lastframetv);
return VDEV_STATUS_OK;
};
@ -121,7 +122,11 @@ int VideoDev_Simulation::CaptureStop() {
#define SIMULATION_SIZE 15
int VideoDev_Simulation::Grab(VideoFrame *vf) {
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);
simulation.GetPos(&posx, &posy);
@ -174,15 +179,35 @@ gpointer simulation_threadprocess_wrapper (gpointer data) {
}
Simulation::Simulation() {
int i;
posX = 900.0;
posY = 500.0;
running = 0;
time_t t = time(NULL);
srandom (t);
//
// object movement
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);
//
// 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() {
@ -218,18 +243,21 @@ void Simulation::ThreadProcess() {
//
// simulate rotation movement
// add random error to values
// calculate movement
dx = sin(dAngle) * dLen * ms;
dy = cos(dAngle) * dLen * ms;
dy = -cos(dAngle) * dLen * ms;
posX += dx;
posY += dy;
//
// 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 (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"
struct SimAxisCtl {
int active;
double v;
double defLen;
double defAngle;
};
class Simulation {
private:
float posX;
@ -34,6 +42,7 @@ private:
float dAngle;
float dLen;
int running;
struct SimAxisCtl axis[2];
GMutex mutex;
public:
@ -45,6 +54,9 @@ public:
void Start();
void Stop();
void AxisSetValue(int a, double v);
void AxisStop();
void LockMutex() { g_mutex_lock(&mutex); };
void UnLockMutex() { g_mutex_unlock(&mutex); };
};
@ -54,7 +66,7 @@ class VideoDev_Simulation: public VideoDev {
private:
uint32_t w;
uint32_t h;
struct timeval starttv;
struct timeval lastframetv;
unsigned char *inframe;
ConvertData cdata;
GThread *simulation_thread;

Write Preview
Loading…
Cancel
Save