#include "mbed.h"
#include "rtos.h"
#include "BaseUsbHost.h"
#include "UvcCam.h"
#include "BaseJpegDecode.h"
#include "Terminal.h"
#include "MyThread.h"
#define WIDTH 160
#define HEIGHT 120
#define THRESHOLD 200
DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
Terminal term(USBTX, USBRX);
class CalcCenter : public MyThread, public BaseJpegDecode {
public:
int y_center, x_center;
int m_x_sum, m_y_sum, m_sum;
uint32_t EOI_count;
int16_t buf_Cb[WIDTH/16*HEIGHT/8];
int16_t buf_Cr[WIDTH/16*HEIGHT/8];
CalcCenter(BaseUvc* cam) {
m_cam = cam;
m_cam->setOnResult(this, &CalcCenter::callback_motion_jpeg);
EOI_count = 0;
}
protected:
void callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len) {
input(buf+12, len-12);
if (buf[1]&1) {
led1 = !led1;
}
}
virtual void outputDC(int mcu, int block, int value) {
if (mcu >= (WIDTH/16*HEIGHT/8)) {
return;
}
if (block == 2) {
buf_Cb[mcu] = value * qt[1][0];
} else if (block == 3) {
buf_Cr[mcu] = value * qt[1][0];
value *= qt[1][0];
if (value >= THRESHOLD) {
m_x_sum += value*(mcu%(WIDTH/16));
m_y_sum += value*(mcu/(WIDTH/16));
m_sum += value;
}
}
}
virtual void outputAC(int mcu, int block, int scan, int value){};
virtual void outputMARK(uint8_t c){
if (c == 0xd9) {
if(m_sum == 0) {
x_center = y_center = -1;
} else {
x_center = m_x_sum / m_sum;
y_center = m_y_sum / m_sum;
}
m_x_sum = m_y_sum = m_sum = 0;
EOI_count++;
led3 = !led3;
}
}
virtual void run() {
while(true) {
if (m_cam) {
m_cam->poll();
}
}
}
BaseUvc* m_cam;
};
BaseUvc* cam = NULL;
CalcCenter* calc = NULL;
class display_Thread : public MyThread {
virtual void run() {
term.cls();
int fg, old_fg = 0xffffff;
while(1) {
int column = 0;
for(int y = 0; y < HEIGHT/8; y++) {
term.locate(0, column++);
for(int x = 0; x < WIDTH/16; x++) {
int value = calc->buf_Cr[y*WIDTH/16+x];
if (value >= THRESHOLD) {
fg = 0xff0000;
} else {
fg = 0xffffff;
}
if (fg != old_fg) {
term.foreground(fg);
old_fg = fg;
}
term.printf("%+4d,", value);
Thread::yield();
}
}
term.locate(0, column++);
term.printf("Cr:(%d,%d)", calc->x_center, calc->y_center);
term.locate(0, column++);
term.printf("width=%d height=%d yblock=%d EOI: %u]\nCC:",
calc->width, calc->height, calc->yblock, calc->EOI_count);
for(int i = 0; i < 16; i++) {
term.printf(" %u", cam->report_cc_count[i]);
}
term.printf("]\nPS:");
for(int i = 0; i < 16; i++) {
term.printf(" %u", cam->report_ps_cc_count[i]);
}
term.printf("]\n");
Thread::wait(150);
led2 = !led2;
}
}
};
#define KP 80
void QcamOrbitAF_Control(BaseUvc* cam, CalcCenter* calc) {
Thread::wait(10*1000);
int unit = 9;
int cs = 1;
struct PanTiltR {
int16_t pan;
int16_t tilt;
} ptr = {0, 0};
while(1) {
if (calc->x_center != (-1)) {
ptr.pan = ((calc->width/16/2) - calc->x_center) * KP;
int rc = cam->Control(SET_CUR, cs, unit<<8, reinterpret_cast<uint8_t*>(&ptr), sizeof(PanTiltR));
}
Thread::wait(50);
led4 = !led4;
}
}
void no_memory () {
error("Failed to allocate memory!\n");
}
int main() {
term.baud(921600);
term.printf("%s\n", __FILE__);
set_new_handler(no_memory);
BaseUsbHost* UsbHost = new BaseUsbHost;
ControlEp* ctlEp = new ControlEp;
if (UsbHub::check(ctlEp)) {
UsbHub* hub = new UsbHub(ctlEp);
ctlEp = hub->search<UvcCam>();
}
if (!UvcCam::check(ctlEp)) {
error("UVC camera is not connected\n");
}
cam = new UvcCam(UVC_MJPEG, UVC_160x120, _15FPS, ctlEp);
calc = new CalcCenter(cam);
calc->start();
display_Thread* display_th = new display_Thread;
display_th->start(osPriorityBelowNormal);
QcamOrbitAF_Control(cam, calc);
}