////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
static void GPS_calc_poshold(void) {
int32_t d;
int32_t target_speed;
uint8_t axis;
for (axis=0;axis<2;axis++) {
target_speed = get_P(error[axis], &posholdPID_PARAM); // calculate desired speed from lat/lon error
target_speed = constrain(target_speed,-100,100); // Constrain the target speed in poshold mode to 1m/s it helps avoid runaways..
rate_error[axis] = target_speed - actual_speed[axis]; // calc the speed error
nav[axis] =
get_P(rate_error[axis], &poshold_ratePID_PARAM)
+get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
d = constrain(d, -2000, 2000);
// get rid of noise
if(fabs(actual_speed[axis]) < 50) d = 0;
nav[axis] +=d;
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
navPID[axis].integrator = poshold_ratePID[axis].integrator;
}
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate the desired nav_lat and nav_lon for distance flying such as RTH
//
static void GPS_calc_nav_rate(uint16_t max_speed) {
float trig[2];
uint8_t axis;
float temp;
// push us towards the original track
GPS_update_crosstrack();
// nav_bearing includes crosstrack
temp = (9000l - nav_bearing) * RADX100;
trig[_X] = cos(temp);
trig[_Y] = sin(temp);
for (axis=0;axis<2;axis++) {
rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
rate_error[axis] = constrain(rate_error[axis], -1000, 1000);
// P + I + D
nav[axis] =
get_P(rate_error[axis], &navPID_PARAM)
+get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM)
+get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM);
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
poshold_ratePID[axis].integrator = navPID[axis].integrator;
}
}
////////////////////////////////////////////////////////////////////////////////////
// Crashpilot NEW PH Logic
// #define GPS_X 1 // #define GPS_Y 0
// #define LON 1 // #define LAT 0
// VelEast; // 1 // VelNorth; // 0
// 0 is NICK part // 1 is ROLL part
// actual_speed[GPS_Y] = VelNorth;
void GPS_calc_posholdCrashpilot(bool overspeed)
{
uint8_t axis;
float p, i, d, rate_error, AbsPosErrorToVel, tmp0, tmp1;
float maxbank100new;
for (axis = 0; axis < 2; axis++)
{
maxbank100new = maxbank100;
if (overspeed)
{
tmp1 = abs(ACC_speed[axis]);
if (tmp1 == 0) tmp1 = 1.0f;
tmp0 = (float)cfg.gps_ph_settlespeed / tmp1;
tmp1 = (float)cfg.gps_ph_minbrakepercent * 0.01f; // this is the minimal break percentage
tmp0 = constrain(sqrt(tmp0), tmp1, 1.0f); // 1 - 100%
maxbank100new = (float)maxbankbrake100 * tmp0;
}
tmp1 = LocError[axis];
if (abs(tmp1) < cfg.gps_ph_abstub && cfg.gps_ph_abstub != 0) // Keep linear Stuff beyond x cm
{
if (tmp1 < 0) tmp0 = -GpsPhAbsTub; // Do flatter response below x cm
else tmp0 = GpsPhAbsTub;
tmp1 = tmp1 * tmp1 * tmp0; // Get a peace around current position
}
AbsPosErrorToVel = get_P(tmp1, &posholdPID_PARAM); // Do absolute position error here, that means transform positionerror to a velocityerror
rate_error = AbsPosErrorToVel - ACC_speed[axis]; // Calculate Rate Error for actual axis
rate_error = constrain(rate_error, -1000, 1000); // +- 10m/s
p = get_P(rate_error, &poshold_ratePID_PARAM);
i = get_I(AbsPosErrorToVel, &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); // Constrained to half max P
d = get_D(rate_error , &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); // Constrained to half max P
nav[axis] = constrain(p + i + d, -maxbank100new, maxbank100new);
}
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
static void GPS_calc_poshold(void)
{
int32_t d;
int32_t target_speed;
int axis;
for (axis = 0; axis < 2; axis++) {
target_speed = get_P(error[axis], &posholdPID_PARAM); // calculate desired speed from lon error
rate_error[axis] = target_speed - actual_speed[axis]; // calc the speed error
nav[axis] = get_P(rate_error[axis], &poshold_ratePID_PARAM) +
get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
d = constrain(d, -2000, 2000);
// get rid of noise
#if defined(GPS_LOW_SPEED_D_FILTER)
if (abs(actual_speed[axis]) < 50)
d = 0;
#endif
nav[axis] += d;
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
navPID[axis].integrator = poshold_ratePID[axis].integrator;
}
}
void GPS_calc_posholdCrashpilot(bool overspeed)
{
uint8_t axis;
float rate_error, tilt, tmp0, tmp1;
float maxbank100new;
for (axis = 0; axis < 2; axis++)
{
maxbank100new = (float)maxbank100;
if (overspeed)
{
tmp1 = fabs(ACC_speed[axis]);
if (!tmp1) tmp1 = 1.0f;
tmp0 = (float)cfg.gps_ph_settlespeed / tmp1;
tmp1 = (float)cfg.gps_ph_minbrakepercent * 0.01f; // this is the minimal break percentage
tmp0 = constrain(sqrtf(tmp0), tmp1, 1.0f); // 1 - 100%
maxbank100new = (float)maxbankbrake100 * tmp0;
}
rate_error = get_P(LocError[axis], &posholdPID_PARAM); // Do absolute position error here, that means transform positionerror to a velocityerror
rate_error += get_I(LocError[axis], &ACCDeltaTimeINS, &posholdPID[axis], &posholdPID_PARAM);
rate_error = constrain(rate_error - ACC_speed[axis], -1000, 1000); // +- 10m/s; // Calculate velocity Error for actual axis
tilt = get_P(rate_error, &poshold_ratePID_PARAM);
tilt += get_D(rate_error, &ACCDeltaTimeINS, &poshold_ratePID[axis], &poshold_ratePID_PARAM); // Constrained to half max P
nav[axis] = constrain(tilt, -maxbank100new, maxbank100new);
}
}
int main (int argc, char** argv) {
Graph *G;
CayleyGraph *C;
G = get_D(6);
C = G -> make_cayley_graph();
std::cout << C -> identify_group() << std::endl;
delete C,G;
G = get_Q8();
C = G -> make_cayley_graph();
std::cout << C -> identify_group() << std::endl;
delete C,G;
Graph *G1, *G2, *G3, *G4;
CayleyGraph *C1,*C2,*C3;
G1 = get_Z(4);
G2 = get_Z(1);
G3 = get_Q8();
G4 = get_Z(1);
C1 = direct_product(*(G1->make_cayley_graph()), *(G2->make_cayley_graph()));
C2 = direct_product(*(G3->make_cayley_graph()), *(G4->make_cayley_graph()));
C3 = direct_product(*C1,*C2);
std::cout << "Direct product 1: " << C1 -> identify_group() << std::endl;
std::cout << "Direct product 2: " << C2 -> identify_group() << std::endl;
std::cout << "Direct product 3: " << C3 -> identify_group() << std::endl;
delete G1,G2,G3,G4,C1,C2,C3;
return 0;
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate the desired nav_lat and nav_lon for distance flying such as RTH
void GPS_calc_nav_rate(uint16_t max_speed)
{
float trig[2], rate_error;
float temp, tiltcomp, trgtspeed;
uint8_t axis;
int32_t crosstrack_error;
if ((abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) && cfg.nav_ctrkgain != 0)// If we are too far off or too close we don't do track following
{
temp = (float)(target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sinf(temp) * (float)wp_distance * cfg.nav_ctrkgain; // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
while (nav_bearing < 0) nav_bearing += 36000; // nav_bearing = wrap_36000(nav_bearing);
while (nav_bearing >= 36000) nav_bearing -= 36000;
}
else nav_bearing = target_bearing;
temp = (float)(9000l - nav_bearing) * RADX100; // nav_bearing and maybe crosstrack
trig[GPS_X] = cosf(temp);
trig[GPS_Y] = sinf(temp);
for (axis = 0; axis < 2; axis++)
{
trgtspeed = (trig[axis] * (float)max_speed); // Target speed
rate_error = trgtspeed - MIX_speed[axis]; // Since my INS Stuff is shit, reduce ACC influence to 50% anyway better than leadfilter
rate_error = constrain(rate_error, -1000, 1000);
nav[axis] = get_P(rate_error, &navPID_PARAM) + // P + I + D
get_I(rate_error, &ACCDeltaTimeINS, &navPID[axis], &navPID_PARAM) +
get_D(rate_error, &ACCDeltaTimeINS, &navPID[axis], &navPID_PARAM);
if (cfg.nav_tiltcomp) // Do the apm 2.9.1 magic tiltcompensation
{
tiltcomp = trgtspeed * trgtspeed * ((float)cfg.nav_tiltcomp * 0.0001f);
if(trgtspeed < 0) tiltcomp = -tiltcomp;
}
else tiltcomp = 0;
nav[axis] = nav[axis] + tiltcomp; // Constrain is done at the end of all GPS stuff
poshold_ratePID[axis].integrator = navPID[axis].integrator;
}
}