/*=====================================================================================================*/
void AHRS_Update(void)
{
float tempX = 0, tempY = 0;
float Normalize;
float gx, gy, gz;
// float hx, hy, hz;
// float wx, wy, wz;
// float bx, bz;
float ErrX, ErrY, ErrZ;
float AccX, AccY, AccZ;
float GyrX, GyrY, GyrZ;
// float MegX, MegY, MegZ;
float /*Mq11, Mq12, */Mq13,/* Mq21, Mq22, */Mq23,/* Mq31, Mq32, */Mq33;
static float AngZ_Temp = 0.0f;
static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
// Mq11 = NumQ.q0*NumQ.q0 + NumQ.q1*NumQ.q1 - NumQ.q2*NumQ.q2 - NumQ.q3*NumQ.q3;
// Mq12 = 2.0f*(NumQ.q1*NumQ.q2 + NumQ.q0*NumQ.q3);
Mq13 = 2.0f * (NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
// Mq21 = 2.0f*(NumQ.q1*NumQ.q2 - NumQ.q0*NumQ.q3);
// Mq22 = NumQ.q0*NumQ.q0 - NumQ.q1*NumQ.q1 + NumQ.q2*NumQ.q2 - NumQ.q3*NumQ.q3;
Mq23 = 2.0f * (NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
// Mq31 = 2.0f*(NumQ.q0*NumQ.q2 + NumQ.q1*NumQ.q3);
// Mq32 = 2.0f*(NumQ.q2*NumQ.q3 - NumQ.q0*NumQ.q1);
Mq33 = NumQ.q0 * NumQ.q0 - NumQ.q1 * NumQ.q1 - NumQ.q2 * NumQ.q2 + NumQ.q3 * NumQ.q3;
Normalize = invSqrtf(squa(Acc.TrueX) + squa(Acc.TrueY) + squa(Acc.TrueZ));
AccX = Acc.TrueX * Normalize;
AccY = Acc.TrueY * Normalize;
AccZ = Acc.TrueZ * Normalize;
// Normalize = invSqrtf(squa(Meg.TrueX) + squa(Meg.TrueY) + squa(Meg.TrueZ));
// MegX = Meg.TrueX*Normalize;
// MegY = Meg.TrueY*Normalize;
// MegZ = Meg.TrueZ*Normalize;
gx = Mq13;
gy = Mq23;
gz = Mq33;
// hx = MegX*Mq11 + MegY*Mq21 + MegZ*Mq31;
// hy = MegX*Mq12 + MegY*Mq22 + MegZ*Mq32;
// hz = MegX*Mq13 + MegY*Mq23 + MegZ*Mq33;
// bx = sqrtf(squa(hx) + squa(hy));
// bz = hz;
// wx = bx*Mq11 + bz*Mq13;
// wy = bx*Mq21 + bz*Mq23;
// wz = bx*Mq31 + bz*Mq33;
ErrX = (AccY * gz - AccZ * gy)/* + (MegY*wz - MegZ*wy)*/;
ErrY = (AccZ * gx - AccX * gz)/* + (MegZ*wx - MegX*wz)*/;
ErrZ = (AccX * gy - AccY * gx)/* + (MegX*wy - MegY*wx)*/;
exInt = exInt + ErrX * Ki;
eyInt = eyInt + ErrY * Ki;
ezInt = ezInt + ErrZ * Ki;
GyrX = toRad(Gyr.TrueX);
GyrY = toRad(Gyr.TrueY);
GyrZ = toRad(Gyr.TrueZ);
GyrX = GyrX + Kp * ErrX + exInt;
GyrY = GyrY + Kp * ErrY + eyInt;
GyrZ = GyrZ + Kp * ErrZ + ezInt;
Quaternion_RungeKutta(&NumQ, GyrX, GyrY, GyrZ, SampleRateHelf);
Quaternion_Normalize(&NumQ);
Quaternion_ToAngE(&NumQ, &AngE);
tempX = (Mag.X * arm_cos_f32(Mag.EllipseSita) + Mag.Y * arm_sin_f32(Mag.EllipseSita)) / Mag.EllipseB;
tempY = (-Mag.X * arm_sin_f32(Mag.EllipseSita) + Mag.Y * arm_cos_f32(Mag.EllipseSita)) / Mag.EllipseA;
AngE.Yaw = atan2f(tempX, tempY);
AngE.Pitch = toDeg(AngE.Pitch);
AngE.Roll = toDeg(AngE.Roll);
AngE.Yaw = toDeg(AngE.Yaw) + 180.0f;
/* 互補濾波 Complementary Filter */
#define CF_A 0.9f
#define CF_B 0.1f
AngZ_Temp = AngZ_Temp + GyrZ * SampleRate;
AngZ_Temp = CF_A * AngZ_Temp + CF_B * AngE.Yaw;
if (AngZ_Temp > 360.0f)
AngE.Yaw = AngZ_Temp - 360.0f;
else if (AngZ_Temp < 0.0f)
AngE.Yaw = AngZ_Temp + 360.0f;
else
AngE.Yaw = AngZ_Temp;
}
virtual void operator ()(const cv::Range & r) const {
int width = _ctData.width / 2;
int height = _ctData.height / 2;
//useful image is ellipsoid with ra rb as width and height,
//all black near corners - we loss this "garbage" during rotations - good riddance
int pad = std::max(width, height);
int widthPad = std::ceil((pad - width) / 2.0);
int heightPad = std::ceil((pad - height) / 2.0);
int wPad = width + widthPad;
int hPad = height + heightPad;
std::vector<cv::Mat>rotationMatrix;
std::vector<float>cosTable;
std::vector<float>sinTable;
for (int angle = 0; angle < RADON_DEGREE_RANGE; angle ++) {
rotationMatrix.push_back(cv::getRotationMatrix2D(cv::Point2i((width + widthPad) / 2, (height + heightPad) / 2),
angle, 1.0));
cosTable.push_back(std::cos(toRad(angle)));
sinTable.push_back(std::sin(toRad(angle)));
}
std::vector<float>dhtCoeffs;
float twoPiN = PI_TIMES_2 / wPad;
for (int i = 0; i != wPad; i ++) {
dhtCoeffs.push_back(std::cos(twoPiN * i) + std::sin(twoPiN * i));
}
for (register int i = r.start; i != r.end; ++ i) {
cv::Mat * data = new cv::Mat(_ctData.width, _ctData.height, _ctData.type);
T pixel;
char * bufferImageI = _ctData.buffer + _ctData.offset * i;
for (int y = 0; y < _ctData.height; y ++) {
for (int x = 0; x < _ctData.width; x ++) {
pixel = 0;
if (_ctData.isLittleEndian) {
for (int k = 0; k < _ctData.bytesAllocated; k ++) {
pixel |= (T)*(bufferImageI + k) << (8 * k);
}
}
else {
for (int k = _ctData.bytesAllocated - 1; k > 0 ; k --) {
pixel |= (T)*(bufferImageI + k) << (8 * (_ctData.bytesAllocated - k + 1));
}
}
bufferImageI += _ctData.bytesAllocated;
/* similar as http://code.google.com/p/pydicom/source/browse/source/dicom/contrib/pydicom_PIL.py*/
pixel = _ctData.slope * pixel + _ctData.intercept;
if (pixel <= (_ctData.windowCenter - 0.5 - (_ctData.windowWidth - 1) / 2.0)) {
pixel = (T)_ctData.minValue;
}
else if (pixel > (_ctData.windowCenter - 0.5 + (_ctData.windowWidth - 1) / 2.0)) {
pixel = (T)_ctData.maxValue;
}
else {
pixel = ((pixel - _ctData.windowCenter + 0.5) / (_ctData.windowWidth - 1) + 0.5) *
(_ctData.maxValue - _ctData.minValue);
}
//MONOCHROME2 - high value -> brighter, -1 high -> blacker
if (_ctData.inverseNeeded) {
data->at<T>(y, x) = ~pixel;
}
else {
data->at<T>(y, x) = pixel;
}
}
}
cv::resize(*data, *data, cv::Size(width, height));
//cv::GaussianBlur(*data, *data, cv::Size(9, 9), 5);
//cv::dilate(*data, *data, cv::Mat(3, 3, CV_8UC1));
//cv::Scharr(*data, *data, -1, 1, 0);
_ctData.images->images.at(i) = data;
//cv::ocl::oclMat * oclData = new cv::ocl::oclMat(*data8);
//cv::Mat * sinogram = new cv::Mat(radon(*data, rotationMatrix, RADON_DEGREE_RANGE, width, height, wPad, hPad));
//cv::Mat * fourier1d = new cv::Mat(Fourier1D(*sinogram, dhtCoeffs));
//cv::Mat * backprojection = new cv::Mat(backproject(*sinogram, cosTable, sinTable));
_ctData.images->ctImages.at(i) = data;
_ctData.images->fourier1d.at(i) = data;
_ctData.images->sinograms.at(i) = data;
//_ctData.images->images.at(i) = backprojection;
//cv::medianBlur(*data8, *contourImage, 5);
//cv::Canny(*contourImage, *contourImage, CANNY_LOWER, 3 * CANNY_LOWER, 3);
//cv::threshold(*contourImage, *contourImage, 250, 255, CV_THRESH_OTSU);
//cv::dilate(*contourImage, *contourImage, 19);
//cv::Mat * steger = new cv::Mat();
//stegerEdges(*contourImage, *steger, 5, 0, 10.0);
//cv::adaptiveThreshold(*contourImage, *contourImage, 200, CV_ADAPTIVE_THRESH_GAUSSIAN_C, CV_THRESH_BINARY, 3, 1);
/*
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
cv::findContours(data8, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_TC89_KCOS, cv::Point(0, 0));
for (uint k = 0; k < contours.size(); k ++) {
if (contours.at(k).size() > 300) {
cv::drawContours(*contourImage, contours, k, cv::Scalar(0x00FF), 2, 8, hierarchy, 0, cv::Point());
}
}
*/
//oclData->download(*contourImage);
//delete contourImage;
//delete oclData;
}
}
// Clears the window and draws the torus.
void display() {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
glRotatef(rotate_x, 1.0, 0.0, 0.0);
glRotatef(rotate_y, 0.0, 1.0, 0.0);
glScaled(zoom, zoom, zoom);
// Draw a white torus of outer radius 3, inner radius 0.5 with 15 stacks
// and 30 slices.
glColor3f(1.0, 1.0, 1.0);
glutWireTorus(0.5, 3, 15, 30);
// WireSphere
glPushMatrix ();
glTranslatef (3.0, 3.0, 1.0);
glColor3f(1.0, 1.0, 0.0);
glutWireSphere(0.75, 20, 20);
glPopMatrix ();
// Square
glBegin (GL_LINE_LOOP);
glColor3f(1.0, 0.0, 1.0);
glVertex2iv (square[0]);
glVertex2iv (square[1]);
glVertex2iv (square[2]);
glVertex2iv (square[3]);
glEnd ();
// Top-face
glBegin(GL_QUADS); // of the color cube
glColor3f(0.0f, 1.0f, 0.0f); // green
glVertex3f(1.0f, 1.0f, -1.0f);
glVertex3f(-1.0f, 1.0f, -1.0f);
glVertex3f(-1.0f, 1.0f, 1.0f);
glVertex3f(1.0f, 1.0f, 1.0f);
// Bottom-face
glColor3f(1.0f, 0.5f, 0.0f); // orange
glVertex3f(1.0f, -1.0f, 1.0f);
glVertex3f(-1.0f, -1.0f, 1.0f);
glVertex3f(-1.0f, -1.0f, -1.0f);
glVertex3f(1.0f, -1.0f, -1.0f);
// Front-face
glColor3f(1.0f, 0.0f, 0.0f); // red
glVertex3f(1.0f, 1.0f, 1.0f);
glVertex3f(-1.0f, 1.0f, 1.0f);
glVertex3f(-1.0f, -1.0f, 1.0f);
glVertex3f(1.0f, -1.0f, 1.0f);
// Back-face
glColor3f(1.0f, 1.0f, 0.0f); // yellow
glVertex3f(1.0f, -1.0f, -1.0f);
glVertex3f(-1.0f, -1.0f, -1.0f);
glVertex3f(-1.0f, 1.0f, -1.0f);
glVertex3f(1.0f, 1.0f, -1.0f);
// Left-face
glColor3f(0.0f, 0.0f, 1.0f); // blue
glVertex3f(-1.0f, 1.0f, 1.0f);
glVertex3f(-1.0f, 1.0f, -1.0f);
glVertex3f(-1.0f, -1.0f, -1.0f);
glVertex3f(-1.0f, -1.0f, 1.0f);
// Right-face
glColor3f(1.0f, 0.0f, 1.0f); // magenta
glVertex3f(1.0f, 1.0f, -1.0f);
glVertex3f(1.0f, 1.0f, 1.0f);
glVertex3f(1.0f, -1.0f, 1.0f);
glVertex3f(1.0f, -1.0f, -1.0f);
glEnd(); // of the color cube
// Draw a red x-axis, a green y-axis, and a blue z-axis. Each of the
// axes are ten units long.
glBegin(GL_LINES);
glColor3f(1, 0, 0); glVertex3f(0, 0, 0); glVertex3f(10, 0, 0);
glColor3f(0, 1, 0); glVertex3f(0, 0, 0); glVertex3f(0, 10, 0);
glColor3f(0, 0, 1); glVertex3f(0, 0, 0); glVertex3f(0, 0, 10);
glEnd();
// WireSphere, BOUNDARY REPRESENTATION
glPushMatrix ();
glTranslatef (1.0, -0.5, 2.8);
glutWireSphere (0.7, 2.0, 7.6);
glPopMatrix ();
// Tetrahedron
glPushMatrix ();
glColor3f (0.5, 1.0, 0.0);
glTranslatef (3.4, 2.6, -2.9);
glutWireTetrahedron ();
glPopMatrix ();
// Cylinder using Parametric representation
GLUquadricObj *cylinder;
glPushMatrix ();
glTranslatef (-1.0, 1.2, 2.8);
cylinder = gluNewQuadric ();
glColor3f (1,0.3, 0.3);
gluQuadricDrawStyle (cylinder, GLU_LINE);
gluCylinder (cylinder, 0.6, 0.6, 1.5, 6, 4);
glPopMatrix ();
/* Sweep representation of Solids
* Hour Glass
* */
float d = 1.0;
float theta = 45.0;
glPushMatrix ();
glTranslatef (3.0, 0.0, 0.0);
for(int phi=0 ;phi < 360 ; phi++) {
glBegin(GL_LINES);
glColor3f(1,0,0);
double x = d*sin(toRad(theta))*cos(toRad(phi));
double y = d*cos(toRad(theta));
double z = d*sin(toRad(theta))*sin(toRad(phi));
glVertex3d(x,y,z);
glColor3f(0,1,0);
glVertex3d(-x,-y,-z);
glEnd();
}
glPopMatrix ();
glFlush();
glutSwapBuffers();
}
int main(int argc, char* argv[]) {
char help[] = "--help";
char target_ip[100];
float position[6] = {};
int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
struct sockaddr_in gcAddr;
struct sockaddr_in locAddr;
//struct sockaddr_in fromAddr;
uint8_t buf[BUFFER_LENGTH];
ssize_t recsize;
socklen_t fromlen;
int bytes_sent;
mavlink_message_t msg;
uint16_t len;
int i = 0;
//int success = 0;
unsigned int temp = 0;
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0)) {
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2) {
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) {
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
for (;;) {
printf("heartbeat\n");
/*Send Heartbeat */
mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send Status */
mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send attitude */
mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), toRad(1.2), toRad(1.7), toRad(3.14), 0.01, 0.02, 0.03);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0) {
printf("Something received\n");
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i) {
temp = buf[i];
printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
}
}
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
sleep(1); // Sleep one second
}
}
Vector<dim> degToRad(Vector<dim> deg) {
for (unsigned int i = 0; i < dim; ++i)
deg[i] *= toRad();
return deg;
}
/*!
* \brief Convert degree to radian
* \author Sascha Kaden
* \param[in] deg
* \param[out] rad
* \date 2016-11-16
*/
static double degToRad(const double deg) {
return deg * toRad();
}
void Camera::moveRight() {
float x = cos(toRad(rotation.y())) * movementSpeed;
float z = sin(toRad(rotation.y())) * movementSpeed;
position.add(x, 0, z);
}
void Camera::moveLeft() {
float x = cos(toRad(rotation.y())) * movementSpeed;
float z = sin(toRad(rotation.y())) * movementSpeed;
position.add(-x, 0, -z);
}
sf::Vector2f Angle::toVector() {
const float rad = toRad(value);
return sf::Vector2f((float) cos(rad), (float) sin(rad));
}
geometry_msgs::Quaternion coefsToQuaternionMsg(float a, float b, float c){
float vmod=sqrt(a*a+b*b+c*c);
float pitch = -(1.0*asin(1.0*c/vmod));
float yaw = (a==0?toRad(90):atan(b/a)) + (a>=0?0:toRad(180));
return tf::createQuaternionMsgFromRollPitchYaw(0,pitch,yaw);
}
void display(void)
{
// Buffer clearen
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// View Matrix erstellen
glLoadIdentity();
float x = distance * sin(theta) * cos(phi);
float y = distance * cos(theta);
float z = distance * sin(theta) * sin(phi);
gluLookAt(x, y, z, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
// Teekanne rendern.
glutSolidTeapot(1);
// Den Matrix-Stack sichern.
glPushMatrix();
// Zeichnen der Kugelkreisbahn.
drawCircle(10.0f, 50);
// Zeichnen der Kugel.
// Wenden Sie eine Translation und eine Rotation an, bevor sie die Kugel zeichnen. Sie können die Variable 'angle' für die Rotation verwenden.
// Bedenken Sie dabei die richtige Reihenfolge der beiden Transformationen.
glRotated(angle, 0, 1.0f, 0);
glTranslatef(10.0f, 0, 0);
glutSolidSphere(1.0f, 32, 32);
// Zeichnen der Würfelkreisbahn.
// Hinweis: Der Ursprung des Koordinatensystems befindet sich nun im Zentrum des Würfels.
// Drehen Sie das Koordinatensystem um 90° entlang der Achse, die für die Verschiebung des Würfels genutzt wurde.
// Danach steht die Würfelkreisbahn senkrecht zur Tangentialrichtung der Kugelkreisbahn.
glRotated(90.0f, 1.0f, 0, 0);
drawCircle(5.0f, 50);
// Zeichnen des Würfels.
// Wenden Sie die entsprechende Translation und Rotation an, bevor sie den Würfel zeichnen.
glRotated(angle, 0, 1.0f, 0);
glTranslatef(5.0f, 0, 0);
glutSolidCube(1.0f);
// Zeichnen einer Linie von Würfel zu Kegel.
glDisable(GL_LIGHTING);
glTranslatef(3.0f, 0, 0);
glBegin(GL_LINE_STRIP);
glVertex3f(0, 0, 0);
glVertex3f(-3.0f, 0, 0);
glEnd();
glEnable(GL_LIGHTING);
// Drehung anwenden, sodass Koordinatensystem in Richtung Ursprung orientiert ist. (Hinweis: Implementieren Sie dies zuletzt.)
GLfloat height = 8*cos(toRad(angle-90));
GLfloat d = 8*sin(toRad(angle-90));
GLfloat e = 10 - d;
GLfloat l = pow(pow(height,2) + pow(e,2), 0.5f);
GLfloat alpha = toDeg(acos(e/l)) - 90;
if(static_cast<int>(angle)%360 > 180) alpha = -alpha;
glRotated(alpha, 0, 1.0f, 0);
if(static_cast<int>(angle)%360 > 180)
{
glRotated(180 - angle, 0, 1.0f, 0);
std::cout << alpha + 180 - (int)angle%360 << std::endl;
}
else
{
glRotated(-angle, 0, 1.0f, 0);
std::cout << alpha - (int)angle%360 << std::endl;
}
// Zeichnen der Linie von Kegel zu Urpsrung.
glDisable(GL_LIGHTING);
glBegin(GL_LINE_STRIP);
glVertex3f(0, 0, 0);
glVertex3f(0, 0, l);
glEnd();
glEnable(GL_LIGHTING);
// Zeichnen des Kegels.
glutSolidCone(0.5f, 1.0f, 32, 4);
// Den Matrix-Stack wiederherstellen.
glPopMatrix();
glutSwapBuffers();
angle += 5.0f / 60.0f;
}
