void Vectorizer::process_triangle(int iv0, int iv1, int iv2, int level,
scalar* xval, scalar* yval, double* phx, double* phy, int* idx)
{
if (level < LIN_MAX_LEVEL)
{
int i;
if (!(level & 1))
{
// obtain solution values and physical element coordinates
xsln->set_quad_order(1, xitem);
ysln->set_quad_order(1, yitem);
xval = xsln->get_values(xia, xib);
yval = ysln->get_values(yia, yib);
for (i = 0; i < lin_np_tri[1]; i++) {
double m = getmag(i);
if (finite(m) && fabs(m) > max) max = fabs(m);
}
if (curved)
{
RefMap* refmap = xsln->get_refmap();
phx = refmap->get_phys_x(1);
phy = refmap->get_phys_y(1);
}
idx = tri_indices[0];
}
// obtain linearized values and coordinates at the midpoints
double midval[4][3];
for (i = 0; i < 4; i++)
{
midval[i][0] = (verts[iv0][i] + verts[iv1][i])*0.5;
midval[i][1] = (verts[iv1][i] + verts[iv2][i])*0.5;
midval[i][2] = (verts[iv2][i] + verts[iv0][i])*0.5;
};
// determine whether or not to split the element
bool split;
if (eps >= 1.0)
{
//if eps > 1, the user wants a fixed number of refinements (no adaptivity)
split = (level < eps);
}
else
{
// calculate the approximate error of linearizing the normalized solution
double err = fabs(getmag(idx[0]) - midmag(0)) +
fabs(getmag(idx[1]) - midmag(1)) +
fabs(getmag(idx[2]) - midmag(2));
split = !finite(err) || err > max*3*eps;
// do the same for the curvature
if (curved && !split)
{
double cerr = 0.0, cden = 0.0; // fixme
for (i = 0; i < 3; i++)
{
cerr += fabs(phx[idx[i]] - midval[0][i]) + fabs(phy[idx[i]] - midval[1][i]);
cden += fabs(phx[idx[i]]) + fabs(phy[idx[i]]);
}
split = (cerr > cden*2.5e-4);
}
// do extra tests at level 0, so as not to miss some functions with zero error at edge midpoints
if (level == 0 && !split)
{
split = (fabs(getmag(8) - 0.5*(midmag(0) + midmag(1))) +
fabs(getmag(9) - 0.5*(midmag(1) + midmag(2))) +
fabs(getmag(4) - 0.5*(midmag(2) + midmag(0)))) > max*3*eps;
}
}
// split the triangle if the error is too large, otherwise produce a linear triangle
if (split)
{
if (curved)
for (i = 0; i < 3; i++) {
midval[0][i] = phx[idx[i]];
midval[1][i] = phy[idx[i]];
}
// obtain mid-edge vertices
int mid0 = get_vertex(iv0, iv1, midval[0][0], midval[1][0], getvalx(idx[0]), getvaly(idx[0]));
int mid1 = get_vertex(iv1, iv2, midval[0][1], midval[1][1], getvalx(idx[1]), getvaly(idx[1]));
int mid2 = get_vertex(iv2, iv0, midval[0][2], midval[1][2], getvalx(idx[2]), getvaly(idx[2]));
// recur to sub-elements
push_transform(0); process_triangle(iv0, mid0, mid2, level+1, xval, yval, phx, phy, tri_indices[1]); pop_transform();
push_transform(1); process_triangle(mid0, iv1, mid1, level+1, xval, yval, phx, phy, tri_indices[2]); pop_transform();
push_transform(2); process_triangle(mid2, mid1, iv2, level+1, xval, yval, phx, phy, tri_indices[3]); pop_transform();
push_transform(3); process_triangle(mid1, mid2, mid0, level+1, xval, yval, phx, phy, tri_indices[4]); pop_transform();
return;
}
}
// no splitting: output a linear triangle
add_triangle(iv0, iv1, iv2);
}
int main(void){
int integration[3] = {0,0,0};
char lostsignalcnt = 0;
int pry[] = {0,0,0};
int paceCounter = 0;
int pidValues13[3] = {6,20,24};
int pidValuesDen13[3] = {16,1,1};
int pidValues24[3] = {6,20,24};
int pidValuesDen24[3] = {16,1,1};
char pidRotUp[3] = {0,0,20};
char pidRotDenUp[3] = {42,1,1};
char pidRotDown[3] = {9,0,20};
char pidRotDenDown[3] = {42,1,1};
char pidRot[] = {5,0,20};
int throttledif = 0;
int throttleavr = 0;
/*counting var, for for loops*/
int i;
/*Start memory location for Accel and Gyro reads, should be moved
to gyro and accel read functions*/
uint8_t accelstartbyte = 0x30;
uint8_t gyrostartbyte = 0x1A;
/*Joystick Axis buffer
[0] - X axis tilt
[1] - Y axis tilt
[2] - Throttle
[3] - Rotation about Z axis
*/
int joyaxis[] = {0,0,0,0,0};
char joyin[] = {0,0,0,0,0};
int joytrim[] = {0,0,0,0,0};
int joydif[] = {0,0};
int joyavr[] = {0,0};
int motorSpeeds[4];
/*Var to allow increase in motor speed nonrelative to the throttle
during flight*/
int motorup = 0;
/*Vars for new input raw data (cache) and filtered data (int) from
imu*/
int gyrocache[3] = {0,0,0};
int accelcache[3] = {0,0,0};
int magcache[3] = {0,0,0};
int magfacing = 0;
int roterr = 0;
int target[] = {0,0,0};
int accelint[] = {0, 0, 0};
int gyroint[] = {0, 0, 0};
int gyrocounter[] = {0,0,0};
/*Standard values for accel and gyro (when level), set during offset*/
int accelnorm[3] = {28,-20,468};
char gyronorm[3] = {16,42,0};
/*Buffer for sending data through the xbee*/
char xbeebuffer[100];
CLK.CTRL = 0b00000011;
CLK.PSCTRL = 0b00010100;
/*Initialize PORTD to output on pins 0-3 from Timer counter pwm at
50Hz*/
PORTD.DIR = 0x2F;
TCD0.CTRLA = TC_CLKSEL_DIV1_gc;
TCD0.CTRLB = TC_WGMODE_SS_gc | TC0_CCCEN_bm | TC0_CCAEN_bm |TC0_CCBEN_bm | TC0_CCDEN_bm;
TCD0.PER = 8000;
/*Initialize Timer counter C0 for pacing,RATE Hz*/
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 2000000 / RATE;
/*Set on board LED pins to output*/
PORTF.DIR = 0x03;
/*Set PORTC to power IMU, PIN 3 3.3V, pin 2 ground*/
PORTC.DIR = 0b00001100;
PORTC.OUT = 0b00001000;
/*Enable global interrupts of all priority levels, should be made
more relevant*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
/*Set pwm duty cycle to stop motors, stop them from beeping
annoyingly*/
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
/*Set Xbee Uart transmit pin 3 to output*/
PORTE.DIR = 0x08;
/*Initialize USARTE0 as the module used by the Xbee*/
uartInitiate(&xbee, &USARTE0);
/*Initialize USARTE1 as the module used by atmega328p */
uartInitiate(&atmega328p, &USARTE1);
/*Initialize imu to use Two wire interface on portC*/
twiInitiate(&imu, &TWIC);
itg3200Init(&imu, RATE);
adxl345Init(&imu);
lsm303dlhInit(&imu);
/*Send string to indicate startup, python doesn't like return carriage
(/r) character*/
sprintf(xbeebuffer, "starting\n");
sendstring(&xbee, xbeebuffer);
/*Start of flight control state machine loop*/
while(1){
/*Check for new packet from atmega328p*/
if(IRDataAvailable)
{
sprintf(xbeebuffer, "Altitude: %d\n", (irdata[1]*256) + irdata[2]);
sendstring(&xbee, xbeebuffer);
}
/*Check for new packet from xbee each time*/
if(readdata){
readdata = 0;
lostsignalcnt = 0;
/*For Joystick packet reads*/
joytrim[2] = 0;
for(i = 0; i < 5; i++){
joyin[i] = -input[3 + i] + 126;
joyaxis[i] = joyin[i];
joyaxis[i] += joytrim[i];
}
throttleavr = ((throttleavr) + (joyaxis[2]))/2;
throttledif = joyaxis[2] - throttleavr;
joyaxis[2] += throttledif * THROTTLEJOYDIF;
for(i = 0; i < 2; i++){
joyavr[i] = (joyavr[i] + joyaxis[i])/2;
joydif[i] = joyaxis[i] - joyavr[i];
}
joyaxis[1] += joydif[1] * PRJOYDIF13;
joyaxis[0] += joydif[0] * PRJOYDIF24;
/*
yawavr = ((yawavr) + joyaxis[3])/2;
yawdif = joyaxis[3] - yawavr;
joyaxis[3] += yawdif * YAWJOYDIF;
*/
//Input 7 is the button buffer
if(input[8] == 4){
state = stopped;
//sprintf(xbeebuffer, "stopped %d\n", input[7]);
sprintf(xbeebuffer, "%4d %4d %4d %4d\n", joyaxis[0], joyaxis[1], joyaxis[2], joyaxis[3]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 0){
joytrim[0] += joyin[0];
joytrim[1] += joyin[1];
joytrim[3] += joyin[3];
}
else if(input[8] == 1){
state = running;
sprintf(xbeebuffer, "running %d\n", input[7]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 10){
state = offset;
}
else if(input[8] == 5){
//motorup += 5;
pidRot[2] ++;
sprintf(xbeebuffer, "D up %d\n", pidRot[2]);
//pidRot[2] ++;
//sprintf(xbeebuffer, "D up %d\n", pidValues24[2]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 6){
pidRot[2] --;
sprintf(xbeebuffer, "D down %d\n", pidRot[2]);
//pidRot[2] --;
//sprintf(xbeebuffer, "D down %d\n", pidValues24[2]);
sendstring(&xbee, xbeebuffer);
//motorup -= 5;
}
else if(input[8] == 7){
pidRot[0] ++;
sprintf(xbeebuffer, "P up %d\n", pidRot[0]);
//pidRot[0] ++;
//sprintf(xbeebuffer, "P up %d\n", pidValues24[0]);
sendstring(&xbee, xbeebuffer);
}
else if(input[8] == 8){
pidRot[0] --;
sprintf(xbeebuffer, "P down %d\n", pidRot[0]);
//pidRot[0] --;
//sprintf(xbeebuffer, "P down %d\n", pidValues24[0]);
sendstring(&xbee, xbeebuffer);
/*
getmag(magcache, &imu);
sprintf(xbeebuffer, "%4d %4d %4d\n", magcache[0], magcache[1], magcache[2]);
sendstring(&xbee, xbeebuffer);
*/
}
else if(input[8] == 2){
sprintf(xbeebuffer, "descending\n");
sendstring(&xbee, xbeebuffer);
motorup = -50;
}
xbeecounter = 0;
for(i=0;i<3;i++){
pidRotUp[i] = pidRot[i] * 3/4;
pidRotDown[i] = pidRot[i] * 1;
}
if(state == running){
//sprintf(xbeebuffer, "%d %d\n", joyaxis[2], throttledif);
//sprintf(xbeebuffer, "%d %d %d \n", joyaxis[0], joyaxis[1], joyaxis[3]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", pry[0], pry[1], pry[2]);
//sprintf(xbeebuffer, "%3d %3d\n", gyroint[2], joyaxis[3]);
//sprintf(xbeebuffer, "%4d %4d %4d %4d\n", motorSpeeds[0], motorSpeeds[1], motorSpeeds[2], motorSpeeds[3]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", accelint[0], accelint[1], accelint[2]);
//sprintf(xbeebuffer, "%4d %4d %4d\n", magcache[0], magcache[1], magcache[2]);
sprintf(xbeebuffer, "%4d\n", roterr);
sendstring(&xbee, xbeebuffer);
}
}
switch(state){
/*Stopped state keeps motors stopped but not beeping*/
case stopped:
TCD0.CCA = 2000;
TCD0.CCB = 2000;
TCD0.CCC = 2000;
TCD0.CCD = 2000;
break;
/*Offset gets standard value for gyro's and accel's*/
case offset:
getgyro(gyrocache, &imu, &gyrostartbyte);
getaccel(accelcache, &imu, &accelstartbyte);
getmag(magcache, &imu);
target[2] = arctan2(magcache[0], magcache[1]);
for(i = 0; i < 3; i ++){
gyronorm[i] = gyrocache[i];
//accelnorm[i] = accelcache[i];
accelcache[i] = 0;
gyrocache[i] = 0;
}
sprintf(xbeebuffer, "offset %d %d %d %d %d %d\n", gyronorm[0], gyronorm[1], gyronorm[2], accelnorm[0], accelnorm[1], accelnorm[2]);
sendstring(&xbee, xbeebuffer);
state = stopped;
break;
case running:
/*Ensure loop doesn't go faster than 50Hz*/
while(!(TCC0.INTFLAGS & 0x01));
TCC0.INTFLAGS = 0x01;
/*Get gyro data
substract stationary offset
filter for stability
*/
getgyro(gyrocache, &imu, &gyrostartbyte);
for(i = 0; i < 3; i ++){
gyrocache[i] -= gyronorm[i];
if((gyrocache[i] <= 1) && (gyrocache[i] >= -1)){
gyrocache[i] = 0;
}
gyrocounter[i] += gyrocache[i];
}
for(i = 0; i < 3; i += 2){
gyroint[i] = gyrocounter[i]/DEGREE;
gyrocounter[i] %= DEGREE;
pry[i] += gyroint[i];
if(pry[i] > PI){
pry[i] = -PI + (pry[i] - PI);
}
else if(pry[i] < -PI){
pry[i] = PI + (pry[i] + PI);
}
}
gyroint[1] = -(gyrocounter[1]/DEGREE);
gyrocounter[1] %= DEGREE;
pry[1] += gyroint[1];
paceCounter ++;
//Slower Operations at 50Hz
if(paceCounter == (RATE / 20)){
paceCounter = 0;
lostsignalcnt ++;
sendbyte(&atmega328p, 'r'); //ask for IR data
getaccel(accelcache, &imu, &accelstartbyte);
getmag(magcache, &imu);
magfacing = arctan2(magcache[0], magcache[1]);
if((4900 - abs(pry[2]) - abs(target[2])) < abs(pry[2] - target[2])){
if(target > 0){
roterr = 4900 - abs(target[2]) - abs(pry[2]);
}
else{
roterr = -(4900 - abs(target[2]) - abs(pry[2]));
}
}
else{
roterr = target[2] - pry[2];
}
for(i = 0; i < 3; i ++){
accelcache[i] -= accelnorm[i];
/*
if(accelcache[i] > (accelint[i] + 40)){
accelcache[i] = accelint[i] + 40;
}
else if(accelcache[i] < (accelint[i] - 40)){
accelcache[i] = accelint[i] - 40;
}
*/
}
accelint[0] = ((ACCELINT * accelint[0]) + ((24 - ACCELINT) * accelcache[0]))/24;
accelint[1] = ((ACCELINT * accelint[1]) + ((24 - ACCELINT) * accelcache[1]))/24;
if(accelint[1] > (pry[0] + 15)){
accelint[1] = pry[0] + 15;
}
else if(accelint[1] < (pry[0] - 15)){
accelint[1] = pry[0] - 15;
}
if(accelint[0] > (pry[1] + 15)){
accelint[0] = pry[1] + 15;
}
else if(accelint[0] < (pry[1] - 15)){
accelint[0] = pry[1] - 15;
}
pry[0] = ((AWEIGHT * accelint[1]) + (GWEIGHT * pry[0])) / (AWEIGHT + GWEIGHT);
pry[1] = ((AWEIGHT * accelint[0]) + (GWEIGHT * pry[1])) / (AWEIGHT + GWEIGHT);
/*reset cache values to 0, should be made unnecessary by modding gyro and
accel read functions*/
for(i = 0; i < 3; i ++){
accelcache[i] = 0;
}
}
if(gyroint[0] > 6){
gyroint[0] = 6;
}
else if(gyroint[0] < -6){
gyroint[0] = -6;
}
if(gyroint[1] > 6){
gyroint[1] = 6;
}
else if(gyroint[1] < -6){
gyroint[1] = -6;
}
motorSpeed(pry, integration ,gyroint, joyaxis, motorSpeeds, pidValues13, pidValues24, pidValuesDen13, pidValuesDen24);
yawCorrect(motorSpeeds, gyroint, &roterr,pidRotUp,pidRotDenUp,pidRotDown,pidRotDenDown);
if(lostsignalcnt > 10){
for(i = 0; i < 4; i ++){
motorSpeeds[i] -= 50;
}
}
while(!((TCD0.CNT > 5000) || (TCD0.CNT < 2500)));
TCD0.CCA = motorSpeeds[0] + motorup;// - motordif13;
TCD0.CCC = motorSpeeds[2] + motorup;// + motordif13;
TCD0.CCB = motorSpeeds[1] + motorup;// + motordif24;
TCD0.CCD = motorSpeeds[3] + motorup;// - motordif24;
PORTD.OUT ^= 0b00100000;
}
}
}
void Vectorizer::process_quad(int iv0, int iv1, int iv2, int iv3, int level,
scalar* xval, scalar* yval, double* phx, double* phy, int* idx)
{
// try not to split through the vertex with the largest value
int a = (magvert(iv0) > magvert(iv1)) ? iv0 : iv1;
int b = (magvert(iv2) > magvert(iv3)) ? iv2 : iv3;
a = (magvert(a) > magvert(b)) ? a : b;
int flip = (a == iv1 || a == iv3) ? 1 : 0;
if (level < LIN_MAX_LEVEL)
{
int i;
if (!(level & 1))
{
// obtain solution values and physical element coordinates
xsln->set_quad_order(1, xitem);
ysln->set_quad_order(1, yitem);
xval = xsln->get_values(xia, xib);
yval = ysln->get_values(yia, yib);
for (i = 0; i < lin_np_quad[1]; i++) {
double m = getmag(i);
if (finite(m) && fabs(m) > max) max = fabs(m);
}
if (curved)
{
RefMap* refmap = xsln->get_refmap();
phx = refmap->get_phys_x(1);
phy = refmap->get_phys_y(1);
}
idx = quad_indices[0];
}
// obtain linearized values and coordinates at the midpoints
double midval[4][5];
for (i = 0; i < 4; i++)
{
midval[i][0] = (verts[iv0][i] + verts[iv1][i]) * 0.5;
midval[i][1] = (verts[iv1][i] + verts[iv2][i]) * 0.5;
midval[i][2] = (verts[iv2][i] + verts[iv3][i]) * 0.5;
midval[i][3] = (verts[iv3][i] + verts[iv0][i]) * 0.5;
midval[i][4] = (midval[i][0] + midval[i][2]) * 0.5;
};
// determine whether or not to split the element
bool split;
if (eps >= 1.0)
{
//if eps > 1, the user wants a fixed number of refinements (no adaptivity)
split = (level < eps);
}
else
{
// the value of the middle point is not the average of the four vertex values, since quad == 2 triangles
midval[2][4] = flip ? (verts[iv0][2] + verts[iv2][2]) * 0.5 : (verts[iv1][2] + verts[iv3][2]) * 0.5;
midval[3][4] = flip ? (verts[iv0][3] + verts[iv2][3]) * 0.5 : (verts[iv1][3] + verts[iv3][3]) * 0.5;
// calculate the approximate error of linearizing the normalized solution
double err = fabs(getmag(idx[0]) - midmag(0)) +
fabs(getmag(idx[1]) - midmag(1)) +
fabs(getmag(idx[2]) - midmag(2)) +
fabs(getmag(idx[3]) - midmag(3)) +
fabs(getmag(idx[4]) - midmag(4));
split = !finite(err) || err > max*4*eps;
// do the same for the curvature
if (curved && !split)
{
double cerr = 0.0, cden = 0.0; // fixme
for (i = 0; i < 5; i++)
{
cerr += fabs(phx[idx[i]] - midval[0][i]) + fabs(phy[idx[i]] - midval[1][i]);
cden += fabs(phx[idx[i]]) + fabs(phy[idx[i]]);
}
split = (cerr > cden*2.5e-4);
}
// do extra tests at level 0, so as not to miss functions with zero error at edge midpoints
if (level == 0 && !split)
{
err = fabs(getmag(13) - 0.5*(midmag(0) + midmag(1))) +
fabs(getmag(17) - 0.5*(midmag(1) + midmag(2))) +
fabs(getmag(20) - 0.5*(midmag(2) + midmag(3))) +
fabs(getmag(9) - 0.5*(midmag(3) + midmag(0)));
split = !finite(err) || (err) > max*2*eps; //?
}
}
// split the quad if the error is too large, otherwise produce two linear triangles
if (split)
{
if (curved)
for (i = 0; i < 5; i++) {
midval[0][i] = phx[idx[i]];
midval[1][i] = phy[idx[i]];
}
// obtain mid-edge and mid-element vertices
int mid0 = get_vertex(iv0, iv1, midval[0][0], midval[1][0], getvalx(idx[0]), getvaly(idx[0]));
int mid1 = get_vertex(iv1, iv2, midval[0][1], midval[1][1], getvalx(idx[1]), getvaly(idx[1]));
int mid2 = get_vertex(iv2, iv3, midval[0][2], midval[1][2], getvalx(idx[2]), getvaly(idx[2]));
int mid3 = get_vertex(iv3, iv0, midval[0][3], midval[1][3], getvalx(idx[3]), getvaly(idx[3]));
int mid4 = get_vertex(mid0, mid2, midval[0][4], midval[1][4], getvalx(idx[4]), getvaly(idx[4]));
// recur to sub-elements
push_transform(0); process_quad(iv0, mid0, mid4, mid3, level+1, xval, yval, phx, phy, quad_indices[1]); pop_transform();
push_transform(1); process_quad(mid0, iv1, mid1, mid4, level+1, xval, yval, phx, phy, quad_indices[2]); pop_transform();
push_transform(2); process_quad(mid4, mid1, iv2, mid2, level+1, xval, yval, phx, phy, quad_indices[3]); pop_transform();
push_transform(3); process_quad(mid3, mid4, mid2, iv3, level+1, xval, yval, phx, phy, quad_indices[4]); pop_transform();
return;
}
}
// output two linear triangles,
if (!flip)
{
add_triangle(iv3, iv0, iv1);
add_triangle(iv1, iv2, iv3);
}
else
{
add_triangle(iv0, iv1, iv2);
add_triangle(iv2, iv3, iv0);
}
}
