void APSPthread::pthreadTask() {
setrunning(true);
if (_isLoop) {
while (true) {
_task();
pthread_testcancel();
}
} else {
_task();
}
setrunning(false);
}
boolean VC0706::setMotionDetect(boolean flag)
{
if (! setMotionStatus(VC0706_MOTIONCONTROL, VC0706_UARTMOTION, VC0706_ACTIVATEMOTION))
return false;
uint8_t cmd[] = {VC0706_COMM_MOTION_CTRL, 0x01, flag};
_task(cmd, sizeof(cmd), 5);
}
uint8_t VC0706::getImageSize(void)
{
uint8_t cmd[] = {VC0706_READ_DATA, 0x04, 0x04, 0x01, 0x00, 0x19};
if (!_task(cmd, sizeof(cmd), 6))
return -1;
return vcBuff[5];
}
boolean VC0706::getMotionDetect(void)
{
uint8_t cmd[] = {VC0706_COMM_MOTION_STATUS, 0x00};
if (! _task(cmd, sizeof(cmd), 5))
return false;
return vcBuff[5];
}
uint8_t VC0706::getDownsize(void)
{
uint8_t cmd[] = {VC0706_DOWNSIZE_STATUS, 0x0};
if (! _task(cmd, sizeof(cmd), 6))
return -1;
return vcBuff[5];
}
void Thread::_thread_func() {
t_thread_info = make_shared_ref<ThreadInfo>(std::this_thread::get_id(), _name);
_logger.info() << get_own_info() << " spawned.";
GUM_TRY_LEVEL("Uncaught exception from client thread function", LogLevel::Error, _task(_cancellation_token));
}
uint8_t* VC0706::readPicture(uint8_t length)
{
#if TRANSFER_BY_SPI
digitalWrite(SLAVE_PIN, LOW);
bufferLen = 0;
uint8_t tmp = SPI.transfer(0);
while(bufferLen < length){
vcBuff[bufferLen++] = SPI.transfer(0);
}
digitalWrite(SLAVE_PIN, HIGH);
return vcBuff;
#else // transfer by serial
uint8_t cmd[] = {VC0706_READ_FBUF, 0x0C, 0x0, 0x0A, 0, 0, framePosition >> 8,
framePosition & 0xFF, 0, 0, 0, length, CAMERADELAY >> 8, CAMERADELAY & 0xFF};
if (!_task(cmd, sizeof(cmd), 5, DEFAULT_TIMEOUT/4, false))
return 0;
if (_read(length+5, CAMERADELAY) == 0)
return 0;
framePosition += length;
return vcBuff;
#endif
}
uint32_t VC0706::getFrameLength(void)
{
uint8_t cmd[] = {VC0706_GET_FBUF_LEN, 0x01, 0x00};
if (!_task(cmd, sizeof(cmd), 9))
return 0;
uint32_t length = (vcBuff[5]<<24)|(vcBuff[6]<<16)|(vcBuff[7]<<8)|vcBuff[8];
return length;
}
void timer_task::task_handler(const boost::system::error_code& e)
{
if (!e) {
_task();
_timer.expires_from_now(boost::posix_time::milliseconds(_period));
_timer.async_wait(boost::bind(&timer_task::task_handler, this, boost::asio::placeholders::error));
}
}
bool Thread::_run()
{
if (!_task) {
return false;
}
_task();
return true;
}
bool the::timed_task::invoke(float dt)
{
_current += dt;
if(_current >= _delay)
{
_current = 0;
_task();
return !_looped;
}
return false;
}
bool PeriodicThread::_run()
{
uint64_t next_run_usec = AP_HAL::micros64() + _period_usec;
while (true) {
uint64_t dt = next_run_usec - AP_HAL::micros64();
if (dt > _period_usec) {
// we've lost sync - restart
next_run_usec = AP_HAL::micros64();
} else {
Scheduler::from(hal.scheduler)->microsleep(dt);
}
next_run_usec += _period_usec;
_task();
}
return true;
}
boolean VC0706::resetBaudrate(VC0706_BaudRate rate)
{
uint8_t S1RELH;
uint8_t S1RELL;
switch (rate){
case BaudRate_9600:
S1RELH = 0xAE;
S1RELL = 0xC8;
baud = 9600;
break;
case BaudRate_19200:
S1RELH = 0x56;
S1RELL = 0xE4;
baud = 19200;
break;
case BaudRate_38400:
S1RELH = 0x2A;
S1RELL = 0xF2;
baud = 38400;
break;
case BaudRate_57600:
S1RELH = 0x1C;
S1RELL = 0x4C;
baud = 57600;
break;
case BaudRate_115200:
S1RELH = 0x0D;
S1RELL = 0xA6;
baud = 115200;
break;
default : //19200
S1RELH = 0x56;
S1RELL = 0xE4;
baud = 19200;
break;
}
uint8_t cmd[] = {VC0706_SET_PORT,0x03,0x01,S1RELH,S1RELL};
return _task(cmd, sizeof(cmd), 5);
}
boolean VC0706::resumeVideo(void)
{
uint8_t cmd[] = {VC0706_FBUF_CTRL, 0x01, VC0706_RESUMEFRAME};
return _task(cmd, sizeof(cmd), 5);
}
double operator()(std::size_t node_id)
{
return _task(*_msh.getNode(node_id));
}
boolean VC0706::enableTVOutput(bool enable)
{
uint8_t cmd[] = {VC0706_TV_OUT_CTRL, 0x01, enable};
return _task(cmd, sizeof(cmd), 5);
}
boolean VC0706::setDownsize(uint8_t size)
{
uint8_t cmd[] = {VC0706_DOWNSIZE_CTRL, 0x01, size};
return _task(cmd, sizeof(cmd), 5);
}
boolean VC0706::resetSystem(void)
{
uint8_t cmd[] = {VC0706_SYSTEM_RESET, 0x00};//0x56+0x00+0x26+0x00
return _task(cmd, sizeof(cmd), 5);//0x76+0x00+0x26+0x00+0x00
}
boolean VC0706::setImageSize(uint8_t size)
{
uint8_t cmd[] = {VC0706_WRITE_DATA, 0x05, 0x04, 0x01, 0x00, 0x19, size};
return _task(cmd, sizeof(cmd), 5);
}
uint8_t VC0706::getMotionStatus(uint8_t x)
{
uint8_t cmd[] = {VC0706_MOTION_STATUS, 0x01, x};
return _task(cmd, sizeof(cmd), 5);
}
uint8_t VC0706::setMotionStatus(uint8_t x, uint8_t d1, uint8_t d2)
{
uint8_t cmd[] = {VC0706_MOTION_CTRL, 0x03, x, d1, d2};
return _task(cmd, sizeof(cmd), 5);
}
boolean VC0706::takePicture(void)
{
uint8_t cmd[] = {VC0706_FBUF_CTRL, 0x01, VC0706_STOPCURRENTFRAME};
framePosition = 0;
return _task(cmd, sizeof(cmd), 5);
}
