int main(void){
_delay_ms(100);
USB_ConfigureClock();
PORTR.DIRSET = 1 << 1;
USB_Init();
// Enable USB interrupts
USB.INTCTRLA = USB_BUSEVIE_bm | USB_INTLVL_MED_gc;
USB.INTCTRLB = USB_TRNIE_bm | USB_SETUPIE_bm;
PMIC.CTRL = PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm;
sei();
// setup TWI bus for master-mode I2C comms
TWIC.MASTER.BAUD = TWI_BAUD;
TWIC.MASTER.CTRLA = TWI_MASTER_ENABLE_bm;
TWIC.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc;
// setup TCC0 for sample timing
TCC0.CTRLA = TC_CLKSEL_DIV256_gc;
TCC0.CTRLB = TC0_CCAEN_bm | TC_WGMODE_SINGLESLOPE_gc;
TCC0.INTCTRLB = TC_CCAINTLVL_LO_gc;
TCC0.CCA = 120;
TCC0.PER = 0;
// config PORTE for serial transmission
PORTE.DIRSET = 1 << 3;
USARTE0.BAUDCTRLA = 0x01;
USARTE0.CTRLC = USART_PMODE_EVEN_gc | USART_CHSIZE_8BIT_gc;
USARTE0.CTRLB = USART_TXEN_bm | USART_CLK2X_bm;
// configure general DMA settings
DMA.CTRL = DMA_ENABLE_bm | DMA_DBUFMODE_DISABLED_gc | DMA_PRIMODE_RR0123_gc;
// use DMA CH0 for transmitting data after a frame snapshot
// reload SRC address register every transaction
// increment SRC every packet
// don't reload the destination address
// the destination address is fixed
DMA.CH0.ADDRCTRL = DMA_CH_SRCRELOAD_TRANSACTION_gc | DMA_CH_SRCDIR_INC_gc | DMA_CH_DESTRELOAD_NONE_gc | DMA_CH_DESTDIR_FIXED_gc;
// DMA.CH1.ADDRCTRL = DMA_CH_SRCRELOAD_NONE_gc | DMA_CH_SRCDIR_FIXED_gc | DMA_CH_DESTRELOAD_TRANSACTION_gc | DMA_CH_DESTDIR_INC_gc;
// trigger DMA transfer on data ready event - USARTE0.DATA is ready to get another byte
DMA.CH0.TRIGSRC = DMA_CH_TRIGSRC_USARTE0_DRE_gc;
// DMA.CH1.TRIGSRC = DMA_CH_TRIGSRC_USARTE0_RXC_gc;
// eww.
DMA.CH0.SRCADDR0 = ((uint32_t)(&sensorData) >> (8*0)) & 0xFF;
DMA.CH0.SRCADDR1 = ((uint32_t)(&sensorData) >> (8*1)) & 0xFF;
DMA.CH0.SRCADDR2 = ((uint32_t)(&sensorData) >> (8*2)) & 0xFF;
DMA.CH0.DESTADDR0 = ((uint32_t)(&USARTE0.DATA) >> (8*0)) & 0xFF;
DMA.CH0.DESTADDR1 = ((uint32_t)(&USARTE0.DATA) >> (8*1)) & 0xFF;
DMA.CH0.DESTADDR2 = ((uint32_t)(&USARTE0.DATA) >> (8*2)) & 0xFF;
// enable CH0, set to one byte bursts
DMA.CH0.CTRLA = DMA_CH_ENABLE_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc;
getAlive();
getCalibrationData();
PORTR.OUTSET = 1 << 1;
for (;;){}
}
int main() {
// ## Make these inputs to the function ##
const char* camDataFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/PS3Eye_out_camera_data.yml";
const char* objPointsFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/LEDPos copy2.txt";
const char* imageFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/Test Images/Test3.jpg";
const char* imagePointsFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/imagePts3RANDOM"
".txt";
// Open the correct image
std::cout << imageFilename << std::endl << imagePointsFilename << std::endl;
cv::Mat img = cv::imread(imageFilename);
cv::namedWindow("Window");
cv::imshow("Window", img);
// Get the camera matrix and distortion coefficients
cv::Mat cameraMatrix, distCoeffs;
getCalibrationData(camDataFilename, cameraMatrix, distCoeffs);
// Solve pose estimation problem
// get the object points (3d) and image points (2d)
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point2f> imagePoints;
std::cout << "Object Points:" << std::endl;
readPoints(objPointsFilename, objectPoints);
std::cout << "\nImage Points:" << std::endl;
readPoints(imagePointsFilename, imagePoints);
// sort the image points
ledFinder(imagePoints,imagePoints,1);
std::cout << "\nSorted Image Points:" << std::endl;
std::cout << imagePoints << std::endl;
// Provide an initial guess: (give an approximate attitude--assume following from behind)
// (OpenCV reference frame) NOTE: rvec has weird quaternion convention, but [0,0,0]
// corresponds to following direclty behind
cv::Mat rvec(3,1,CV_64F);
cv::Mat tvec(3,1,CV_64F);
rvec.at<double>(0,0) = 0*DEG2RAD;
rvec.at<double>(1,0) = 0*DEG2RAD;
rvec.at<double>(2,0) = 0*DEG2RAD;
tvec.at<double>(0,0) = 0*IN2MM;
tvec.at<double>(1,0) = 0*IN2MM;
tvec.at<double>(2,0) = 100*IN2MM;
//
// solve for the pose that minimizes reprojection error
// UNITS: (objectPoints = mm, imagePoints = pixels, ... , rvec = radians, tvec = mm)
clock_t t;
t = clock();
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, 1, CV_EPNP);
t = clock() - t;
std::cout << "\nTime To Estimate Pose: " << ((float)t/CLOCKS_PER_SEC*1000) << " ms" << std::endl;
// compute the theta and r parts of the Euler rotation
/*
double rvec_theta = cv::norm(rvec, cv::NORM_L2);
cv::Mat rvec_r = rvec/rvec_theta;
*/
// get rotation matrix
cv::Mat rotMat, CV2B(3,3,CV_64F,cv::Scalar(0)), B2CV;
CV2B.at<double>(0,2) = 1.0;
CV2B.at<double>(1,0) = 1.0;
CV2B.at<double>(2,1) = 1.0;
cv::transpose(CV2B,B2CV); // CV2B and B2CV convert between OpenCV
cv::Rodrigues(rvec,rotMat); // frame convention and typical body frame
// extract phi, theta, psi (NOTE: these are for typical body frame)
double phi, theta, psi;
rotMat = CV2B * rotMat * B2CV; // change the rotation matrix from CV convention to RHS body frame
theta = asin(-rotMat.at<double>(2,0)); // get the Euler angles
psi = acos(rotMat.at<double>(0,0) / cos(theta));
phi = asin(rotMat.at<double>(2,1) / cos(theta));
// add changes to the projection for troubleshooting
phi += 0*DEG2RAD; // phi, theta, psi in body frame
theta += 0*DEG2RAD;
psi += 0*DEG2RAD;
tvec.at<double>(0,0) += 0*IN2MM; //tvec in OpenCV frame
tvec.at<double>(1,0) += 0*IN2MM;
tvec.at<double>(2,0) += 0*IN2MM;
// reconstruct rotation matrix: from object frame to camera frame
rotMat.at<double>(0,0) = cos(theta)*cos(psi);
rotMat.at<double>(0,1) = sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi);
rotMat.at<double>(0,2) = cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi);
rotMat.at<double>(1,0) = cos(theta)*sin(psi);
rotMat.at<double>(1,1) = sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi);
rotMat.at<double>(1,2) = cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi);
rotMat.at<double>(2,0) = -sin(theta);
rotMat.at<double>(2,1) = sin(phi)*cos(theta);
rotMat.at<double>(2,2) = cos(phi)*cos(theta);
// rewrite the rvec from rotation matrix
rotMat = B2CV * rotMat * CV2B; // convert RHS body rotation matrix to OpenCV rotation matrix
cv::Rodrigues(rotMat,rvec);
// print to standard output
cv::Mat tvec_body;
tvec_body.push_back(tvec.at<double>(2,0)); // get tvec in body coordinates to display to
tvec_body.push_back(tvec.at<double>(0,0)); // standard output
tvec_body.push_back(tvec.at<double>(1,0));
std::cout << "\nPhi, Theta, Psi: " << (phi*RAD2DEG) << ", " << (theta*RAD2DEG) <<
", " << (psi*RAD2DEG) << std::endl;
std::cout << "\ntvec:\n" << (tvec_body*MM2IN) << std::endl;
std::cout << "\nTotal Distance: " << (cv::norm(tvec,cv::NORM_L2)*MM2IN) << std::endl;
// compute the (re)projected points
cv::vector<cv::Point3f> axesPoints; // create points for coordinate axes
axesPoints.push_back(cv::Point3f(0,0,0));
axesPoints.push_back(cv::Point3f(AXES_LN,0,0));
axesPoints.push_back(cv::Point3f(0,AXES_LN,0));
axesPoints.push_back(cv::Point3f(0,0,AXES_LN));
cv::vector<cv::Point2f> projImagePoints, projAxesPoints;
cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projImagePoints);
cv::projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, projAxesPoints);
std::cout << "\nProjected Image Points:\n" << projImagePoints << std::endl;
// ## Need to compute the projected error to determine feasibility Decide on a threshold
double reprojErr = scaledReprojError(imagePoints, projImagePoints);
std::cout << "\nReprojection Error " << reprojErr << std::endl << std::endl;
// Plot the LED's and reprojected positions on the image
for (int i=0; i<NO_LEDS; i++) {
cv::circle(img,imagePoints[i], 3, cv::Scalar(0,0,255), 2);
cv::circle(img,projImagePoints[i], 3, cv::Scalar(0,255,0), 2);
cv::line(img,projAxesPoints[0], projAxesPoints[1], cv::Scalar(0,0,255), 2);
cv::line(img,projAxesPoints[0], projAxesPoints[2], cv::Scalar(0,255,0), 2);
cv::line(img,projAxesPoints[0], projAxesPoints[3], cv::Scalar(255,0,0), 2);
}
cv::imshow("Window",img);
//cv::imwrite("/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/VisionOut5.jpg",img);
cv::waitKey(0);
std::cout << "DONE!\n" << std::endl;
return 0;
}
/** Event handler for the library USB Control Request reception event. */
bool EVENT_USB_Device_ControlRequest(USB_Request_Header_t* req){
// zero out ep0_buf_in
for (uint8_t i = 0; i < 64; i++) ep0_buf_in[i] = 0;
usb_cmd = 0;
if ((req->bmRequestType & CONTROL_REQTYPE_TYPE) == REQTYPE_VENDOR){
switch(req->bRequest){
case 0x00: // Info
if (req->wIndex == 0){
USB_ep0_send_progmem((uint8_t*)hwversion, sizeof(hwversion));
}else if (req->wIndex == 1){
USB_ep0_send_progmem((uint8_t*)fwversion, sizeof(fwversion));
}
return true;
// bother a specified I2C address, return '1' if address ACKs, '0' if NACK
// mnemonic - 0xBotherAddress
case 0xBA:
ep0_buf_in[0] = botherAddress(req->wIndex, req->wValue);
USB_ep0_send(1);
return true;
// start sampling
// mnemonic - 0xConfigure7imer
case 0xC7:
usb_pipe_reset(&ep_in);
if (req->wIndex != 0) {
startConversion();
ep0_buf_in[0] = 1;
timeout_or_sampling_no_longer_enabled = 0;
TCC0.CCA = req->wValue;
TCC0.PER = 1 << 15;
}
else {
TCC0.PER = 0;
ep0_buf_in[0] = 0;
timeout_or_sampling_no_longer_enabled = 1;
}
TCC0.CNT = 0;
USB_ep0_send(1);
return true;
// return a bitmap of alive cells
// mnemonic - 0x5Can
case 0x5C:
getAlive();
_delay_ms(5);
for (uint8_t i = 0; i < 8; i++) {ep0_buf_in[i] = bitmap[i];}
USB_ep0_send(8);
return true;
// return calibration information
// mnemonic - 0x6etCalibration
case 0x6C: {
getCalibrationData();
_delay_ms(5);
uint8_t offset = 40*req->wIndex+8*req->wValue;
for (uint8_t i = 0; i < 8; i++) {ep0_buf_in[i] = calibrationData[offset+i];}
USB_ep0_send(8);
return true;
}
// disconnect from USB, jump to bootloader
case 0xBB:
USB_enter_bootloader();
return true;
}
}
return false;
}
int fd_runtime_connectdevice(FDRuntime* self)
{
FDDeviceClass *deviceclass = FD_DEVICE_GET_CLASS(FD_DEVICE(self->device));
char* out = g_malloc0(256 * sizeof(char));
pthread_mutex_lock(&FD_DEVICE(self->device)->deviceMutex);
deviceclass->search(FD_DEVICE(self->device), &out); //search for device, grab a usb handle and model number.
pthread_mutex_unlock(&FD_DEVICE(self->device)->deviceMutex);
g_message("%s\n",out); //output of search function.
g_free(out);
switch (FD_DEVICE(self->device)->model)
{
case MODEL_DSO2090:
case MODEL_DSO2100:
case MODEL_DSO2150:
case MODEL_DSO2250:
self->gaincount = 9;
double tempgains1[9] = {0.010, 0.020, 0.050, 0.100, 0.200, 0.500, 1.0, 2.0, 5.0};
self->gains = g_malloc(sizeof(double)*self->gaincount);
memcpy(self->gains, tempgains1, sizeof(double)*self->gaincount);
self->sampleratecount = 16;
unsigned long int tempsr1[16] = {100e6, 50e6, 25e6, 10e6, 5e6, 25e5, 1e6, 500e3, 250e3, 100e3, 50e3, 25e3, 10e3, 5e3, 25e2, 1e3};
self->samplerates = g_malloc(sizeof(unsigned long int)*self->sampleratecount);
memcpy(self->samplerates, tempsr1, sizeof(unsigned long int)*self->sampleratecount);
break;
case MODEL_DSO5200:
case MODEL_DSO5200A:
self->gaincount = 10;
double tempgains2[10] = {0.010, 0.020, 0.050, 0.100, 0.200, 0.500, 1.0, 2.0, 5.0, 10.0};
self->gains = g_malloc(sizeof(double)*self->gaincount);
memcpy(self->gains, tempgains2, sizeof(double)*self->gaincount);
self->sampleratecount = 17;
unsigned long int tempsr2[17] = {250e6, 100e6, 50e6, 25e6, 10e6, 5e6, 25e5, 1e6, 500e3, 250e3, 100e3, 50e3, 25e3, 10e3, 5e3, 25e2, 1e3};
self->samplerates = g_malloc(sizeof(unsigned long int)*self->sampleratecount);
memcpy(self->samplerates, tempsr2, sizeof(unsigned long int)*self->sampleratecount);
unsigned short int tempRanges[10] = {186, 370, 458, 916, 366, 450, 900, 368, 458, 908};
self->ranges = g_malloc(sizeof(unsigned short int)*self->gaincount);
memcpy(self->ranges, tempRanges, sizeof(unsigned short int)*self->gaincount);
self->cal5200Data = g_malloc(sizeof(unsigned char)*6);
break;
case MODEL_UNKNOWN:
default:
self->gaincount = 6;
double tempgains3[6] = {0.100, 0.200, 0.500, 1.0, 2.0, 5.0};
self->gains = g_malloc(sizeof(double)*self->gaincount);
memcpy(self->gains, tempgains3, sizeof(double)*self->gaincount);
self->sampleratecount = 13;
unsigned long int tempsr3[13] = {10e6, 5e6, 25e5, 1e6, 500e3, 250e3, 100e3, 50e3, 25e3, 10e3, 5e3, 25e2, 1e3};
self->samplerates = g_malloc(sizeof(unsigned long int)*self->sampleratecount);
memcpy(self->samplerates, tempsr3, sizeof(unsigned long int)*self->sampleratecount);
break;
}
populateSamplerateComboBox(FD_UI(self->ui), self->samplerates, self->sampleratecount);
populateGainComboBoxs(FD_UI(self->ui), self->gains, self->gaincount);
self->samplerateFastMax = self->samplerates[0];
self->samplerateChannelMax = self->samplerates[1];
self->calibrationData = g_malloc(((sizeof(unsigned short int)*self->gaincount)*HANTEK_CHANNELS)*OFFSET_COUNT);
getCalibrationData(self); //gets the channel level calibration data.
printCalibrationData(self);
self->calibration2Data = g_malloc(sizeof(unsigned char)*4);
getCalibration2Data(self); //gets another calibration
if (self->cal5200Data)
getCal5200Data(self);
self->currentAveraging[0] = 0;
self->currentAveraging[1] = 0;
fd_runtime_setdeviceisconnected(self, TRUE);
fd_runtime_settriggersource(self, FALSE, 0);
fd_runtime_settriggerposition(self, 0.5);
fd_runtime_settriggerlevel(self, 0, 0.0);
fd_runtime_settriggerslope(self, SLOPE_POSITIVE);
fd_runtime_setbuffersize(self, 10240);
fd_runtime_setchannelused(self, 1, FALSE);
fd_runtime_setchannelused(self, 0, TRUE);
fd_runtime_setoffset(self, 0, 0.5);
fd_runtime_setoffset(self, 1, 0.5);
setDefaultParameters(self);
return 0;
}
