diff --git a/flight/libraries/inc/paths.h b/flight/libraries/inc/paths.h index 053b9b474..a588430cc 100644 --- a/flight/libraries/inc/paths.h +++ b/flight/libraries/inc/paths.h @@ -34,6 +34,6 @@ struct path_status { float correction_vector[3]; }; -void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status); +void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D); #endif diff --git a/flight/libraries/inc/plans.h b/flight/libraries/inc/plans.h index 478de25af..d6341c578 100644 --- a/flight/libraries/inc/plans.h +++ b/flight/libraries/inc/plans.h @@ -63,11 +63,6 @@ void plan_setup_returnToBase(); */ void plan_setup_land(); -/** - * @brief execute land - */ -void plan_run_land(); - /** * @brief setup pathplanner/pathfollower for braking */ @@ -78,11 +73,29 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred); #define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN 2 #define PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT 3 +#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH 0 +#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST 1 +#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN 2 +#define PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED 3 + +#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH 0 +#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST 1 +#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN 2 +#define PATHDESIRED_MODEPARAMETER_LAND_OPTIONS 3 + +#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE 0 +#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH 1 + +#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND 0 +#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1 1 +#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2 2 +#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3 3 /** * @brief setup pathfollower for positionvario */ void plan_setup_CourseLock(); void plan_setup_PositionRoam(); +void plan_setup_VelocityRoam(); void plan_setup_HomeLeash(); void plan_setup_AbsolutePosition(); @@ -91,6 +104,7 @@ void plan_setup_AbsolutePosition(); */ void plan_run_CourseLock(); void plan_run_PositionRoam(); +void plan_run_VelocityRoam(); void plan_run_HomeLeash(); void plan_run_AbsolutePosition(); diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index d014babb1..cb41469c8 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -33,25 +33,26 @@ #include #include #include +#include // constants/macros/typdefs #define NUMX 13 // number of states, X is the state vector #define NUMW 9 // number of plant noise inputs, w is disturbance noise vector #define NUMV 10 // number of measurements, v is the measurement noise vector #define NUMU 6 // number of deterministic inputs, U is the input vector - +#pragma GCC optimize "O3" // Private functions void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX]); -void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], - float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], - uint16_t SensorsUsed); -void RungeKutta(float X[NUMX], float U[NUMU], float dT); -void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); -void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], - float G[NUMX][NUMW]); -void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); -void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); +static void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], + uint16_t SensorsUsed); +static void RungeKutta(float X[NUMX], float U[NUMU], float dT); +static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); +static void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], + float G[NUMX][NUMW]); +static void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); +static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); // Private variables @@ -60,29 +61,29 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); // LinearizeFG() and LinearizeH(): // // usage F: usage G: usage H: -// 0123456789abc 012345678 0123456789abc +// -0123456789abc 012345678 0123456789abc // 0...X......... ......... X............ // 1....X........ ......... .X........... // 2.....X....... ......... ..X.......... // 3......XXXX... ...XXX... ...X......... // 4......XXXX... ...XXX... ....X........ // 5......XXXX... ...XXX... .....X....... -// 6.......XXXXXX XXX...... ......XXXX... -// 7......X.XXXXX XXX...... ......XXXX... -// 8......XX.XXXX XXX...... ......XXXX... -// 9......XXX.XXX XXX...... ..X.......... -// a............. ......X.. -// b............. .......X. -// c............. ........X +// 6.....ooXXXXXX XXX...... ......XXXX... +// 7.....oXoXXXXX XXX...... ......XXXX... +// 8.....oXXoXXXX XXX...... ......XXXX... +// 9.....oXXXoXXX XXX...... ..X.......... +// a............. ......Xoo +// b............. ......oXo +// c............. ......ooX -static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6, 13, 13, 13 }; -static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 }; +static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 13, 13, 13 }; +static int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 }; -static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 }; -static const int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 }; +static int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 }; +static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 }; -static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 }; -static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 }; +static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 }; +static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 }; static struct EKFData { // linearized system matrices @@ -182,8 +183,8 @@ void INSGetP(float PDiag[NUMX]) uint8_t i; // retrieve diagonal elements (aka state variance) - for (i = 0; i < NUMX; i++) { - if (PDiag != 0) { + if (PDiag != 0) { + for (i = 0; i < NUMX; i++) { PDiag[i] = ekf.P[i][i]; } } @@ -289,7 +290,7 @@ void INSSetMagNorth(float B[3]) void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) { float U[6]; - float qmag; + float invqmag; // rate gyro inputs in units of rad/s U[0] = gyro_data[0]; @@ -304,11 +305,11 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) // EKF prediction step LinearizeFG(ekf.X, U, ekf.F, ekf.G); RungeKutta(ekf.X, U, dT); - qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); - ekf.X[6] /= qmag; - ekf.X[7] /= qmag; - ekf.X[8] /= qmag; - ekf.X[9] /= qmag; + invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + ekf.X[6] *= invqmag; + ekf.X[7] *= invqmag; + ekf.X[8] *= invqmag; + ekf.X[9] *= invqmag; // CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P); // Update Nav solution structure @@ -373,8 +374,8 @@ void VelBaroCorrection(float Vel[3], float BaroAlt) void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed) { - float Z[10], Y[10]; - float Bmag, qmag; + float Z[10] = { 0 }; + float Y[10] = { 0 }; // GPS Position in meters and in local NED frame Z[0] = Pos[0]; @@ -385,14 +386,15 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], Z[3] = Vel[0]; Z[4] = Vel[1]; Z[5] = Vel[2]; - // magnetometer data in any units (use unit vector) and in body frame - Bmag = - sqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + - mag_data[2] * mag_data[2]); - Z[6] = mag_data[0] / Bmag; - Z[7] = mag_data[1] / Bmag; - Z[8] = mag_data[2] / Bmag; + + + if (SensorsUsed & MAG_SENSORS) { + float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]); + Z[6] = mag_data[0] * invBmag; + Z[7] = mag_data[1] * invBmag; + Z[8] = mag_data[2] * invBmag; + } // barometric altimeter in meters and in local NED frame Z[9] = BaroAlt; @@ -401,12 +403,12 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], LinearizeH(ekf.X, ekf.Be, ekf.H); MeasurementEq(ekf.X, ekf.Be, Y); SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed); - qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); - ekf.X[6] /= qmag; - ekf.X[7] /= qmag; - ekf.X[8] /= qmag; - ekf.X[9] /= qmag; + float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + ekf.X[6] *= invqmag; + ekf.X[7] *= invqmag; + ekf.X[8] *= invqmag; + ekf.X[9] *= invqmag; // Update Nav solution structure Nav.Pos[0] = ekf.X[0]; Nav.Pos[1] = ekf.X[1]; @@ -434,60 +436,78 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], // The first Method is very specific to this implementation // ************************************************ -__attribute__((optimize("O3"))) void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX]) { // Pnew = (I+F*T)*P*(I+F*T)' + (T^2)*G*Q*G' = (T^2)[(P/T + F*P)*(I/T + F') + G*Q*G')] - float dT1 = 1.0f / dT; // multiplication is faster than division on fpu. - float dTsq = dT * dT; + const float dT1 = 1.0f / dT; // multiplication is faster than division on fpu. + const float dTsq = dT * dT; float Dummy[NUMX][NUMX]; int8_t i; + int8_t k; for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P) - float *Firow = F[i]; - float *Pirow = P[i]; - float *Dirow = Dummy[i]; - int8_t Fistart = FrowMin[i]; - int8_t Fiend = FrowMax[i]; + float *Firow = F[i]; + float *Pirow = P[i]; + float *Dirow = Dummy[i]; + const int8_t Fistart = FrowMin[i]; + const int8_t Fiend = FrowMax[i]; int8_t j; + for (j = 0; j < NUMX; j++) { Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ... - int8_t k; - for (k = Fistart; k <= Fiend; k++) { + } + for (k = Fistart; k <= Fiend; k++) { + for (j = 0; j < NUMX; j++) { Dirow[j] += Firow[k] * P[k][j]; // [] + F * P } } } for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G'] - float *Dirow = Dummy[i]; - float *Girow = G[i]; - float *Pirow = P[i]; - int8_t Gistart = GrowMin[i]; - int8_t Giend = GrowMax[i]; + float *Dirow = Dummy[i]; + float *Girow = G[i]; + float *Pirow = P[i]; + const int8_t Gistart = GrowMin[i]; + const int8_t Giend = GrowMax[i]; int8_t j; - for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular - float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ... - { - float *Fjrow = F[j]; - int8_t Fjstart = FrowMin[j]; - int8_t Fjend = FrowMax[j]; - int8_t k; - for (k = Fjstart; k <= Fjend; k++) { - Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ... - } + + for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular + float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ... + + const float *Fjrow = F[j]; + int8_t Fjstart = FrowMin[j]; + int8_t Fjend = FrowMax[j]; + k = Fjstart; + + while (k <= Fjend - 3) { + Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ... + Ptmp += Dirow[k + 1] * Fjrow[k + 1]; + Ptmp += Dirow[k + 2] * Fjrow[k + 2]; + Ptmp += Dirow[k + 3] * Fjrow[k + 3]; + k += 4; + } + while (k <= Fjend) { + Ptmp += Dirow[k] * Fjrow[k]; + k++; } - { - float *Gjrow = G[j]; - int8_t Gjstart = MAX(Gistart, GrowMin[j]); - int8_t Gjend = MIN(Giend, GrowMax[j]); - int8_t k; - for (k = Gjstart; k <= Gjend; k++) { - Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ... + float *Gjrow = G[j]; + const int8_t Gjstart = MAX(Gistart, GrowMin[j]); + const int8_t Gjend = MIN(Giend, GrowMax[j]); + k = Gjstart; + while (k <= Gjend - 2) { + Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ... + Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1]; + Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2]; + k += 3; + } + if (k <= Gjend) { + Ptmp += Q[k] * Girow[k] * Gjrow[k]; + if (k <= Gjend - 1) { + Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1]; } } @@ -511,7 +531,6 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], // The SensorsUsed variable is a bitwise mask indicating which sensors // should be used in the update. // ************************************************ - void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], uint16_t SensorsUsed) @@ -524,7 +543,10 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], if (SensorsUsed & (0x01 << m)) { // use this sensor for update for (j = 0; j < NUMX; j++) { // Find Hp = H*P HP[j] = 0; - for (k = HrowMin[m]; k <= HrowMax[m]; k++) { + } + + for (k = HrowMin[m]; k <= HrowMax[m]; k++) { + for (j = 0; j < NUMX; j++) { // Find Hp = H*P HP[j] += H[m][k] * P[k][j]; } } @@ -532,14 +554,13 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], for (k = HrowMin[m]; k <= HrowMax[m]; k++) { HPHR += HP[k] * H[m][k]; } - + float invHPHR = 1.0f / HPHR; for (k = 0; k < NUMX; k++) { - Km[k] = HP[k] / HPHR; // find K = HP/HPHR + Km[k] = HP[k] * invHPHR; // find K = HP/HPHR } for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP for (j = i; j < NUMX; j++) { - P[i][j] = P[j][i] = - P[i][j] - Km[i] * HP[j]; + P[i][j] = P[j][i] = P[i][j] - Km[i] * HP[j]; } } @@ -560,8 +581,8 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], void RungeKutta(float X[NUMX], float U[NUMU], float dT) { - float dT2 = - dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX]; + const float dT2 = dT / 2.0f; + float K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX]; uint8_t i; for (i = 0; i < NUMX; i++) { @@ -585,7 +606,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT) for (i = 0; i < NUMX; i++) { X[i] = Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] + - K4[i]) / 6.0f; + K4[i]) * (1.0f / 6.0f); } } @@ -608,7 +629,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT) // H is output of LinearizeH(), all elements not set should be zero // ************************************************ -void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]) +static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]) { float ax, ay, az, wx, wy, wz, q0, q1, q2, q3; diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index 891bcbe03..e03e6e97b 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -143,3 +143,95 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) pid->d = d; pid->iLim = iLim; } + + +/** + * Configure the settings for a pid2 structure + * @param[out] pid The PID2 structure to configure + * @param[in] kp proportional gain + * @param[in] ki integral gain. Time constant Ti = kp/ki + * @param[in] kd derivative gain. Time constant Td = kd/kp + * @param[in] Tf filtering time = (kd/k)/N, N is in the range of 2 to 20 + * @param[in] kt tracking gain for anti-windup. Tt = √TiTd and Tt = (Ti + Td)/2 + * @param[in] dt delta time increment + * @param[in] beta setpoint weight on setpoint in P component. beta=1 error feedback. beta=0 smoothes out response to changes in setpoint + * @param[in] u0 initial output for r=y at activation to achieve bumpless transfer + * @param[in] va constant for compute of actuator output for check against limits for antiwindup + * @param[in] vb multiplier for compute of actuator output for check against limits for anti-windup + */ +void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb) +{ + pid->reconfigure = true; + pid->u0 = u0; + pid->va = va; + pid->vb = vb; + pid->kp = kp; + pid->beta = beta; // setpoint weight on proportional term + + pid->bi = ki * dT; + pid->br = kt * dT / vb; + + pid->ad = Tf / (Tf + dT); + pid->bd = kd / (Tf + dT); +} + +/** + * Achieve a bumpless transfer and trigger initialisation of I term + * @param[out] pid The PID structure to configure + * @param[in] u0 initial output for r=y at activation to achieve bumpless transfer + */ +void pid2_transfer(struct pid2 *pid, float u0) +{ + pid->reconfigure = true; + pid->u0 = u0; +} + +/** + * pid controller with setpoint weighting, anti-windup, with a low-pass filtered derivative on the process variable + * See "Feedback Systems" for an explanation + * @param[out] pid The PID structure to configure + * @param[in] r setpoint + * @param[in] y process variable + * @param[in] ulow lower limit on actuator + * @param[in] uhigh upper limit on actuator + */ +float pid2_apply( + struct pid2 *pid, + const float r, + const float y, + const float ulow, + const float uhigh) +{ + // on reconfigure ensure bumpless transfer + // http://www.controlguru.com/2008/021008.html + if (pid->reconfigure) { + pid->reconfigure = false; + + // initialise derivative terms + pid->yold = y; + pid->D = 0.0f; + + // t=0, u=u0, y=y0, v=u + pid->I = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y); + } + + // compute proportional part + pid->P = pid->kp * (pid->beta * r - y); + + // update derivative part + pid->D = pid->ad * pid->D - pid->bd * (y - pid->yold); + + // compute temporary output + float v = pid->va + pid->vb * (pid->P + pid->I + pid->D); + + // simulate actuator saturation + float u = boundf(v, ulow, uhigh); + + // update integral + pid->I = pid->I + pid->bi * (r - y) + pid->br * (u - v); + + // update old process output + pid->yold = y; + + return u; +} diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h index 0031fb212..9b81cebad 100644 --- a/flight/libraries/math/pid.h +++ b/flight/libraries/math/pid.h @@ -44,7 +44,25 @@ struct pid { float lastDer; }; -typedef struct pid_scaler { +// pid2 structure for a PID+setpoint weighting, anti-windup and filtered derivative control +struct pid2 { + float u0; + float va; + float vb; + float kp; + float bi; + float ad; + float bd; + float br; + float beta; + float yold; + float P; + float I; + float D; + uint8_t reconfigure; +}; + +typedef struct pid_scaler_s { float p; float i; float d; @@ -57,4 +75,9 @@ void pid_zero(struct pid *pid); void pid_configure(struct pid *pid, float p, float i, float d, float iLim); void pid_configure_derivative(float cutoff, float gamma); +// Methods for use with pid2 structure +void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb); +void pid2_transfer(struct pid2 *pid, float u0); +float pid2_apply(struct pid2 *pid, const float r, const float y, float ulow, float uhigh); + #endif /* PID_H */ diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index cdfa27f04..071d4b112 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -44,33 +44,27 @@ static void path_circle(PathDesiredData *path, float *cur_point, struct path_sta * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status) +void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D) { switch (path->Mode) { - case PATHDESIRED_MODE_BRAKE: // should never get here... - case PATHDESIRED_MODE_FLYVECTOR: - return path_vector(path, cur_point, status, true); + case PATHDESIRED_MODE_BRAKE: + case PATHDESIRED_MODE_FOLLOWVECTOR: + return path_vector(path, cur_point, status, mode3D); break; - case PATHDESIRED_MODE_DRIVEVECTOR: - return path_vector(path, cur_point, status, false); + case PATHDESIRED_MODE_CIRCLERIGHT: + return path_circle(path, cur_point, status, mode3D); break; - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - return path_circle(path, cur_point, status, 1); + case PATHDESIRED_MODE_CIRCLELEFT: + return path_circle(path, cur_point, status, mode3D); break; - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - return path_circle(path, cur_point, status, 0); + case PATHDESIRED_MODE_GOTOENDPOINT: + return path_endpoint(path, cur_point, status, mode3D); break; - case PATHDESIRED_MODE_FLYENDPOINT: - return path_endpoint(path, cur_point, status, true); - - break; - case PATHDESIRED_MODE_DRIVEENDPOINT: + case PATHDESIRED_MODE_LAND: default: // use the endpoint as default failsafe if called in unknown modes return path_endpoint(path, cur_point, status, false); diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index bf43d3d17..650a0cc78 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -40,6 +40,7 @@ #include #include #include +#include #include #define UPDATE_EXPECTED 0.02f @@ -47,6 +48,31 @@ #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f + +static float applyExpo(float value, float expo); + + +static float applyExpo(float value, float expo) +{ + // note: fastPow makes a small error, therefore result needs to be bound + float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f); + + // magic number scales expo + // so that + // expo=100 yields value**10 + // expo=0 yields value**1 + // expo=-100 yields value**(1/10) + // (pow(2.0,1/100)~=1.00695) + if (value > 0.0f) { + return boundf(fastPow(value, exp), 0.0f, 1.0f); + } else if (value < -0.0f) { + return boundf(-fastPow(-value, exp), -1.0f, 0.0f); + } else { + return 0.0f; + } +} + + /** * @brief initialize UAVOs and structs used by this library */ @@ -61,6 +87,7 @@ void plan_initialize() ManualControlCommandInitialize(); VelocityStateInitialize(); VtolPathFollowerSettingsInitialize(); + StabilizationBankInitialize(); } /** @@ -85,7 +112,7 @@ void plan_setup_positionHold() pathDesired.Start.Down = positionState.Down; pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT; PathDesiredSet(&pathDesired); } @@ -126,49 +153,61 @@ void plan_setup_returnToBase() pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + + uint8_t ReturnToBaseNextCommand; + FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand); + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1] = 0.0f; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2] = 0.0f; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3] = 0.0f; + pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT; PathDesiredSet(&pathDesired); } -static PiOSDeltatimeConfig landdT; + +static void plan_setup_land_helper(PathDesiredData *pathDesired) +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + float velocity_down; + + FlightModeSettingsLandingVelocityGet(&velocity_down); + + pathDesired->Start.North = positionState.North; + pathDesired->Start.East = positionState.East; + pathDesired->Start.Down = positionState.Down; + pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f; + pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST] = 0.0f; + pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN] = velocity_down; + + pathDesired->End.North = positionState.North; + pathDesired->End.East = positionState.East; + pathDesired->End.Down = positionState.Down; + + pathDesired->StartingVelocity = 0.0f; + pathDesired->EndingVelocity = 0.0f; + pathDesired->Mode = PATHDESIRED_MODE_LAND; + pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH; +} + void plan_setup_land() { - float descendspeed; - - plan_setup_positionHold(); - - FlightModeSettingsLandingVelocityGet(&descendspeed); PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.StartingVelocity = descendspeed; - pathDesired.EndingVelocity = descendspeed; + + plan_setup_land_helper(&pathDesired); PathDesiredSet(&pathDesired); - PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); } -/** - * @brief execute land - */ -void plan_run_land() +static void plan_setup_land_from_velocityroam() { - float downPos, descendspeed; - PathDesiredEndData pathDesiredEnd; - - PositionStateDownGet(&downPos); // current down position - PathDesiredEndGet(&pathDesiredEnd); // desired position - PathDesiredEndingVelocityGet(&descendspeed); - - // desired position is updated to match the desired descend speed but don't run ahead - // too far if the current position can't keep up. This normaly means we have landed. - if (pathDesiredEnd.Down - downPos < 10) { - pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT); - } - - PathDesiredEndSet(&pathDesiredEnd); + plan_setup_land(); + FlightStatusAssistedControlStateOptions assistedControlFlightMode; + assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD; + FlightStatusAssistedControlStateSet(&assistedControlFlightMode); } - /** * @brief positionvario functionality */ @@ -197,6 +236,14 @@ void plan_setup_PositionRoam() plan_setup_PositionVario(); } +void plan_setup_VelocityRoam() +{ + vario_control_lowpass[0] = 0.0f; + vario_control_lowpass[1] = 0.0f; + vario_control_lowpass[2] = 0.0f; + AttitudeStateYawGet(&vario_course); +} + void plan_setup_HomeLeash() { plan_setup_PositionVario(); @@ -340,6 +387,9 @@ static void plan_run_PositionVario(vario_type type) pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = pathDesired.End.East; pathDesired.Start.Down = pathDesired.End.Down; + + // set mode explicitly + PathDesiredSet(&pathDesired); } } else { @@ -381,6 +431,106 @@ static void plan_run_PositionVario(vario_type type) } } +void plan_run_VelocityRoam() +{ + // float alpha; + PathDesiredData pathDesired; + FlightStatusAssistedControlStateOptions assistedControlFlightMode; + FlightStatusFlightModeOptions flightMode; + + PathDesiredGet(&pathDesired); + FlightModeSettingsPositionHoldOffsetData offset; + FlightModeSettingsPositionHoldOffsetGet(&offset); + FlightStatusAssistedControlStateGet(&assistedControlFlightMode); + FlightStatusFlightModeGet(&flightMode); + StabilizationBankData stabSettings; + StabilizationBankGet(&stabSettings); + + ManualControlCommandData cmd; + ManualControlCommandGet(&cmd); + + cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll); + cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch); + cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw); + + bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f); + + if (!flagRollPitchHasInput) { + // no movement desired, re-enter positionHold at current start-position + if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY) { + // initiate braking and change assisted control flight mode to braking + if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { + // avoid brake then hold sequence to continue descent. + plan_setup_land_from_velocityroam(); + } else { + plan_setup_assistedcontrol(false); + } + } + // otherwise nothing to do in braking/hold modes + } else { + PositionStateData positionState; + PositionStateGet(&positionState); + + // Revert assist control state to primary, which in this case implies + // we are in roaming state (a GPS vector assisted velocity roam) + assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY; + + // Calculate desired velocity in each direction + float angle; + AttitudeStateYawGet(&angle); + angle = DEG2RAD(angle); + float cos_angle = cosf(angle); + float sine_angle = sinf(angle); + float rotated[2] = { + -cmd.Pitch * cos_angle - cmd.Roll * sine_angle, + -cmd.Pitch * sine_angle + cmd.Roll * cos_angle + }; + // flip pitch to have pitch down (away) point north + float horizontalVelMax; + float verticalVelMax; + VtolPathFollowerSettingsHorizontalVelMaxGet(&horizontalVelMax); + VtolPathFollowerSettingsVerticalVelMaxGet(&verticalVelMax); + float velocity_north = rotated[0] * horizontalVelMax; + float velocity_east = rotated[1] * horizontalVelMax; + float velocity_down = 0.0f; + + if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { + FlightModeSettingsLandingVelocityGet(&velocity_down); + } + + float velocity = velocity_north * velocity_north + velocity_east * velocity_east; + velocity = sqrtf(velocity); + + // if one stick input (pitch or roll) should we use fly by vector? set arbitrary distance of say 20m after which we + // expect new stick input + // if two stick input pilot is fighting wind manually and we use fly by velocity + // in reality setting velocity desired to zero will fight wind anyway. + + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH] = velocity_north; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST] = velocity_east; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN] = velocity_down; + + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + + pathDesired.StartingVelocity = velocity; + pathDesired.EndingVelocity = velocity; + pathDesired.Mode = PATHDESIRED_MODE_VELOCITY; + if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { + pathDesired.Mode = PATHDESIRED_MODE_LAND; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE; + } else { + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED] = 0.0f; + } + PathDesiredSet(&pathDesired); + FlightStatusAssistedControlStateSet(&assistedControlFlightMode); + } +} + void plan_run_CourseLock() { plan_run_PositionVario(COURSE); @@ -437,7 +587,7 @@ void plan_setup_AutoCruise() pathDesired.Start.Down = pathDesired.End.Down; pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT; PathDesiredSet(&pathDesired); @@ -528,22 +678,27 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred) PositionStateGet(&positionState); PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); FlightStatusAssistedControlStateOptions assistedControlFlightMode; FlightStatusAssistedControlStateGet(&assistedControlFlightMode); if (timeout_occurred) { - pathDesired.End.North = positionState.North; - pathDesired.End.East = positionState.East; - pathDesired.End.Down = positionState.Down; - pathDesired.Start.North = positionState.North; - pathDesired.Start.East = positionState.East; - pathDesired.Start.Down = positionState.Down; - pathDesired.StartingVelocity = 0.0f; - pathDesired.EndingVelocity = 0.0f; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD; + FlightStatusFlightModeOptions flightMode; + FlightStatusFlightModeGet(&flightMode); + if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { + plan_setup_land_helper(&pathDesired); + } else { + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.StartingVelocity = 0.0f; + pathDesired.EndingVelocity = 0.0f; + pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT; + } + assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD; } else { VelocityStateData velocityState; VelocityStateGet(&velocityState); diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index a55a05a70..7f16cc1c1 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -118,7 +118,7 @@ int32_t configuration_check() ADDSEVERITY(navCapableFusion); } - switch (modes[i]) { + switch ((FlightModeSettingsFlightModePositionOptions)modes[i]) { case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL: ADDSEVERITY(!gps_assisted); ADDSEVERITY(!multirotor); @@ -143,24 +143,18 @@ int32_t configuration_check() break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: { - // Revo supports PathPlanner and that must be OK or we are not sane - // PathPlan alarm is uninitialized if not running - // PathPlan alarm is warning or error if the flightplan is invalid - SystemAlarmsAlarmData alarms; - SystemAlarmsAlarmGet(&alarms); - ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK); ADDSEVERITY(!gps_assisted); } case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND: ADDSEVERITY(!coptercontrol); ADDSEVERITY(navCapableFusion); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK: - case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION: - case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE: diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index e6789f7dd..881fd69c4 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -45,6 +45,11 @@ #include "cameradesired.h" #include "manualcontrolcommand.h" #include "taskinfo.h" +#include +#include +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES +#include +#endif #undef PIOS_INCLUDE_INSTRUMENTATION #ifdef PIOS_INCLUDE_INSTRUMENTATION #include @@ -76,26 +81,33 @@ static int8_t counter; // Private variables static xQueueHandle queue; static xTaskHandle taskHandle; +static FrameType_t frameType = FRAME_TYPE_MULTIROTOR; +static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE; static float lastResult[MAX_MIX_ACTUATORS] = { 0 }; static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0 }; static uint8_t pinsMode[MAX_MIX_ACTUATORS]; // used to inform the actuator thread that actuator update rate is changed -static volatile bool actuator_settings_updated; +static ActuatorSettingsData actuatorSettings; +static bool spinWhileArmed; + // used to inform the actuator thread that mixer settings are changed -static volatile bool mixer_settings_updated; +static MixerSettingsData mixerSettings; +static int mixer_settings_count = 2; // Private functions static void actuatorTask(void *parameters); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); -static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings); -static float MixerCurve(const float throttle, const float *curve, uint8_t elements); -static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings); -static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update); +static void setFailsafe(); +static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements); +static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements); +static bool set_channel(uint8_t mixer_channel, uint16_t value); +static void actuator_update_rate_if_changed(bool force_update); static void MixerSettingsUpdatedCb(UAVObjEvent *ev); static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); +static void SettingsUpdatedCb(UAVObjEvent *ev); float ProcessMixer(const int index, const float curve1, const float curve2, - const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, + ActuatorDesiredData *desired, const float period); // this structure is equivalent to the UAVObjects for one mixer. @@ -116,6 +128,9 @@ int32_t ActuatorStart() #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR); #endif + SettingsUpdatedCb(NULL); + MixerSettingsUpdatedCb(NULL); + ActuatorSettingsUpdatedCb(NULL); return 0; } @@ -149,6 +164,13 @@ int32_t ActuatorInitialize() MixerStatusInitialize(); #endif +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + VtolPathFollowerSettingsInitialize(); + VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); +#endif + SystemSettingsInitialize(); + SystemSettingsConnectCallback(&SettingsUpdatedCb); + return 0; } MODULE_INITCALL(ActuatorInitialize, ActuatorStart); @@ -178,7 +200,6 @@ static void actuatorTask(__attribute__((unused)) void *parameters) ActuatorDesiredData desired; MixerStatusData mixerStatus; FlightStatusData flightStatus; - SystemSettingsThrustControlOptions thrustType; float throttleDesired; float collectiveDesired; @@ -186,21 +207,17 @@ static void actuatorTask(__attribute__((unused)) void *parameters) counter = PIOS_Instrumentation_CreateCounter(0xAC700001); #endif /* Read initial values of ActuatorSettings */ - ActuatorSettingsData actuatorSettings; - actuator_settings_updated = false; ActuatorSettingsGet(&actuatorSettings); /* Read initial values of MixerSettings */ - MixerSettingsData mixerSettings; - mixer_settings_updated = false; MixerSettingsGet(&mixerSettings); /* Force an initial configuration of the actuator update rates */ - actuator_update_rate_if_changed(&actuatorSettings, true); + actuator_update_rate_if_changed(true); // Go to the neutral (failsafe) values until an ActuatorDesired update is received - setFailsafe(&actuatorSettings, &mixerSettings); + setFailsafe(); // Main task loop lastSysTime = xTaskGetTickCount(); @@ -214,20 +231,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters) #ifdef PIOS_INCLUDE_INSTRUMENTATION PIOS_Instrumentation_TimeStart(counter); #endif - /* Process settings updated events even in timeout case so we always act on the latest settings */ - if (actuator_settings_updated) { - actuator_settings_updated = false; - ActuatorSettingsGet(&actuatorSettings); - actuator_update_rate_if_changed(&actuatorSettings, false); - } - if (mixer_settings_updated) { - mixer_settings_updated = false; - MixerSettingsGet(&mixerSettings); - } if (rc != pdTRUE) { /* Update of ActuatorDesired timed out. Go to failsafe */ - setFailsafe(&actuatorSettings, &mixerSettings); + setFailsafe(); continue; } @@ -240,7 +247,6 @@ static void actuatorTask(__attribute__((unused)) void *parameters) FlightStatusGet(&flightStatus); ActuatorDesiredGet(&desired); ActuatorCommandGet(&command); - SystemSettingsThrustControlGet(&thrustType); // read in throttle and collective -demultiplex thrust switch (thrustType) { @@ -258,12 +264,15 @@ static void actuatorTask(__attribute__((unused)) void *parameters) } bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; + bool activeThrottle = (throttleDesired < -0.001f || throttleDesired > 0.001f); // for ground and reversible motors + bool positiveThrottle = (throttleDesired > 0.00f); // safety settings if (!armed) { throttleDesired = 0; } - if (throttleDesired <= 0.00f || !armed) { + + if ((frameType == FRAME_TYPE_GROUND && !activeThrottle) || (frameType != FRAME_TYPE_GROUND && throttleDesired <= 0.00f) || !armed) { // force set all other controls to zero if throttle is cut (previously set in Stabilization) if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { desired.Roll = 0; @@ -279,46 +288,47 @@ static void actuatorTask(__attribute__((unused)) void *parameters) #ifdef DIAG_MIXERSTATUS MixerStatusGet(&mixerStatus); #endif - int nMixers = 0; - Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; - for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { - if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) { - nMixers++; - } - } - if ((nMixers < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers. - setFailsafe(&actuatorSettings, &mixerSettings); // So that channels like PWM buzzer keep working + + if ((mixer_settings_count < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers. + setFailsafe(); continue; } AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - bool activeThrottle = (throttleDesired < 0.00f || throttleDesired > 0.00f); - bool positiveThrottle = (throttleDesired > 0.00f); - bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; + float curve1 = 0.0f; + float curve2 = 0.0f; - float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM); + // Interpolate curve 1 from throttleDesired as input. + // assume reversible motor/mixer initially. We can later reverse this. The difference is simply that -ve throttleDesired values + // map differently + curve1 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM); // The source for the secondary curve is selectable - float curve2 = 0; AccessoryDesiredData accessory; - switch (mixerSettings.Curve2Source) { + uint8_t curve2Source = mixerSettings.Curve2Source; + switch (curve2Source) { case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: - curve2 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + // assume reversible motor/mixer initially + curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_ROLL: - curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + // Throttle curve contribution the same for +ve vs -ve roll + curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_PITCH: - curve2 = MixerCurve(desired.Pitch, mixerSettings.ThrottleCurve2, - MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + // Throttle curve contribution the same for +ve vs -ve pitch + curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2, + MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_YAW: - curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + // Throttle curve contribution the same for +ve vs -ve yaw + curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: - curve2 = MixerCurve(collectiveDesired, mixerSettings.ThrottleCurve2, - MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + // assume reversible motor/mixer initially + curve2 = MixerCurveFullRangeProportional(collectiveDesired, mixerSettings.ThrottleCurve2, + MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: @@ -327,14 +337,19 @@ static void actuatorTask(__attribute__((unused)) void *parameters) case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) { - curve2 = MixerCurve(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + // Throttle curve contribution the same for +ve vs -ve accessory....maybe not want we want. + curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); } else { - curve2 = 0; + curve2 = 0.0f; } break; + default: + curve2 = 0.0f; + break; } - float *status = (float *)&mixerStatus; // access status objects as an array of floats + float *status = (float *)&mixerStatus; // access status objects as an array of floats + Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { // During boot all camera actuators should be completely disabled (PWM pulse = 0). @@ -343,20 +358,24 @@ static void actuatorTask(__attribute__((unused)) void *parameters) // Setting it to 1 by default means "Rescale this channel and enable PWM on its output". command.Channel[ct] = 1; - if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { + uint8_t mixer_type = mixers[ct].type; + + if (mixer_type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { // Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us status[ct] = -1; continue; } - if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) { - status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds); - } else { - status[ct] = -1; - } - - // Motors have additional protection for when to be on - if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) { + float nonreversible_curve1 = curve1; + float nonreversible_curve2 = curve2; + if (nonreversible_curve1 < 0.0f) { + nonreversible_curve1 = 0.0f; + } + if (nonreversible_curve2 < 0.0f) { + nonreversible_curve2 = 0.0f; + } + status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, dTSeconds); // If not armed or motors aren't meant to spin all the time if (!armed || (!spinWhileArmed && !positiveThrottle)) { @@ -369,57 +388,60 @@ static void actuatorTask(__attribute__((unused)) void *parameters) (status[ct] < 0)) { status[ct] = 0; } - } - - // Reversable Motors are like Motors but go to neutral instead of minimum - if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { + } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { + status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds); + // Reversable Motors are like Motors but go to neutral instead of minimum // If not armed or motor is inactive - no "spinwhilearmed" for this engine type if (!armed || !activeThrottle) { filterAccumulator[ct] = 0; lastResult[ct] = 0; status[ct] = 0; // force neutral throttle } - } + } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) { + status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds); + } else { + status[ct] = -1; - // If an accessory channel is selected for direct bypass mode - // In this configuration the accessory channel is scaled and mapped - // directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING - // these also will not be updated in failsafe mode. I'm not sure what - // the correct behavior is since it seems domain specific. I don't love - // this code - if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && - (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) { - if (AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) { - status[ct] = accessory.AccessoryVal; - } else { - status[ct] = -1; - } - } - - if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) && - (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) { - CameraDesiredData cameraDesired; - if (CameraDesiredGet(&cameraDesired) == 0) { - switch (mixers[ct].type) { - case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1: - status[ct] = cameraDesired.RollOrServo1; - break; - case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2: - status[ct] = cameraDesired.PitchOrServo2; - break; - case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW: - status[ct] = cameraDesired.Yaw; - break; - default: - break; + // If an accessory channel is selected for direct bypass mode + // In this configuration the accessory channel is scaled and mapped + // directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING + // these also will not be updated in failsafe mode. I'm not sure what + // the correct behavior is since it seems domain specific. I don't love + // this code + if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && + (mixer_type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) { + if (AccessoryDesiredInstGet(mixer_type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) { + status[ct] = accessory.AccessoryVal; + } else { + status[ct] = -1; } - } else { - status[ct] = -1; } - // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot - if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) { - command.Channel[ct] = 0; + if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) && + (mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) { + CameraDesiredData cameraDesired; + if (CameraDesiredGet(&cameraDesired) == 0) { + switch (mixer_type) { + case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1: + status[ct] = cameraDesired.RollOrServo1; + break; + case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2: + status[ct] = cameraDesired.PitchOrServo2; + break; + case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW: + status[ct] = cameraDesired.Yaw; + break; + default: + break; + } + } else { + status[ct] = -1; + } + + // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot + if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) { + command.Channel[ct] = 0; + } } } } @@ -454,7 +476,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) bool success = true; for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - success &= set_channel(n, command.Channel[n], &actuatorSettings); + success &= set_channel(n, command.Channel[n]); } PIOS_Servo_Update(); @@ -475,10 +497,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters) * Process mixing for one actuator */ float ProcessMixer(const int index, const float curve1, const float curve2, - const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, const float period) + ActuatorDesiredData *desired, const float period) { static float lastFilteredResult[MAX_MIX_ACTUATORS]; - const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects + const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects const Mixer_t *mixer = &mixers[index]; float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) + @@ -495,18 +517,18 @@ float ProcessMixer(const int index, const float curve1, const float curve2, // feed forward float accumulator = filterAccumulator[index]; - accumulator += (result - lastResult[index]) * mixerSettings->FeedForward; + accumulator += (result - lastResult[index]) * mixerSettings.FeedForward; lastResult[index] = result; result += accumulator; if (period > 0.0f) { if (accumulator > 0.0f) { - float invFilter = period / mixerSettings->AccelTime; + float invFilter = period / mixerSettings.AccelTime; if (invFilter > 1) { invFilter = 1; } accumulator -= accumulator * invFilter; } else { - float invFilter = period / mixerSettings->DecelTime; + float invFilter = period / mixerSettings.DecelTime; if (invFilter > 1) { invFilter = 1; } @@ -518,7 +540,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2, // acceleration limit float dt = result - lastFilteredResult[index]; - float maxDt = mixerSettings->MaxAccel * period; + float maxDt = mixerSettings.MaxAccel * period; if (dt > maxDt) { // we are accelerating too hard result = lastFilteredResult[index] + maxDt; } @@ -530,21 +552,43 @@ float ProcessMixer(const int index, const float curve1, const float curve2, /** - * Interpolate a throttle curve. Throttle input should be in the range 0 to 1. - * Output is in the range 0 to 1. + * Interpolate a throttle curve + * Full range input (-1 to 1) for yaw, roll, pitch + * Output range (-1 to 1) reversible motor/throttle curve + * + * Input of -1 -> -lookup(1) + * Input of 0 -> lookup(0) + * Input of 1 -> lookup(1) */ -static float MixerCurve(const float throttle, const float *curve, uint8_t elements) +static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements) { - float scale = throttle * (float)(elements - 1); - int idx1 = scale; + float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements); + + if (input < 0.0f) { + return -unsigned_value; + } else { + return unsigned_value; + } +} + +/** + * Interpolate a throttle curve + * Full range input (-1 to 1) for yaw, roll, pitch + * Output range (0 to 1) non-reversible motor/throttle curve + * + * Input of -1 -> lookup(1) + * Input of 0 -> lookup(0) + * Input of 1 -> lookup(1) + */ +static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements) +{ + float abs_input = fabsf(input); + float scale = abs_input * (float)(elements - 1); + int idx1 = scale; scale -= (float)idx1; // remainder if (curve[0] < -1) { - return throttle; - } - if (idx1 < 0) { - idx1 = 0; // clamp to lowest entry in table - scale = 0; + return abs_input; } int idx2 = idx1 + 1; if (idx2 >= elements) { @@ -553,7 +597,9 @@ static float MixerCurve(const float throttle, const float *curve, uint8_t elemen idx1 = elements - 1; } } - return curve[idx1] * (1.0f - scale) + curve[idx2] * scale; + + float unsigned_value = curve[idx1] * (1.0f - scale) + curve[idx2] * scale; + return unsigned_value; } @@ -593,21 +639,22 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr /** * Set actuator output to the neutral values (failsafe) */ -static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings) +static void setFailsafe() { /* grab only the parts that we are going to use */ int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM]; ActuatorCommandChannelGet(Channel); - const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects + const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects // Reset ActuatorCommand to safe values for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { - Channel[n] = actuatorSettings->ChannelMin[n]; + Channel[n] = actuatorSettings.ChannelMin[n]; } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { - Channel[n] = actuatorSettings->ChannelNeutral[n]; + // reversible motors need calibration wizard that allows channel neutral to be the 0 velocity point + Channel[n] = actuatorSettings.ChannelNeutral[n]; } else { Channel[n] = 0; } @@ -618,7 +665,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe // Update servo outputs for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - set_channel(n, Channel[n], actuatorSettings); + set_channel(n, Channel[n]); } // Send the updated command PIOS_Servo_Update(); @@ -713,39 +760,39 @@ static inline bool buzzerState(buzzertype type) #if defined(ARCH_POSIX) || defined(ARCH_WIN32) -static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings) +static bool set_channel(uint8_t mixer_channel, uint16_t value) { return true; } #else -static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings) +static bool set_channel(uint8_t mixer_channel, uint16_t value) { - switch (actuatorSettings->ChannelType[mixer_channel]) { + switch (actuatorSettings.ChannelType[mixer_channel]) { case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], - buzzerState(BUZZ_BUZZER) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); + PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], + buzzerState(BUZZ_BUZZER) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]); return true; case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], - buzzerState(BUZZ_ARMING) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); + PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], + buzzerState(BUZZ_ARMING) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]); return true; case ACTUATORSETTINGS_CHANNELTYPE_INFOLED: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], - buzzerState(BUZZ_INFO) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); + PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], + buzzerState(BUZZ_INFO) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]); return true; case ACTUATORSETTINGS_CHANNELTYPE_PWM: { - uint8_t mode = pinsMode[actuatorSettings->ChannelAddr[mixer_channel]]; + uint8_t mode = pinsMode[actuatorSettings.ChannelAddr[mixer_channel]]; switch (mode) { case ACTUATORSETTINGS_BANKMODE_ONESHOT125: // Remap 1000-2000 range to 125-250 - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE); + PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE); break; default: - PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value); + PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value); break; } return true; @@ -770,12 +817,12 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet /** * @brief Update the servo update rate */ -static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update) +static void actuator_update_rate_if_changed(bool force_update) { static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM]; static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM]; - bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings->BankMode, sizeof(prevBankMode)) != 0); - bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings->BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0); + bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0); + bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0); // check if any setting is changed if (updateMode || updateFreq) { @@ -784,15 +831,15 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM]; uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 }; for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) { - if (force_update || (actuatorSettings->BankMode[i] != prevBankMode[i])) { + if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) { PIOS_Servo_SetBankMode(i, - actuatorSettings->BankMode[i] == + actuatorSettings.BankMode[i] == ACTUATORSETTINGS_BANKMODE_PWM ? PIOS_SERVO_BANK_MODE_PWM : PIOS_SERVO_BANK_MODE_SINGLE_PULSE ); } - switch (actuatorSettings->BankMode[i]) { + switch (actuatorSettings.BankMode[i]) { case ACTUATORSETTINGS_BANKMODE_ONESHOT125: freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 2MHz timer clock @@ -802,37 +849,73 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator clock[i] = ACTUATOR_PWM_CLOCK; break; default: // PWM - freq[i] = actuatorSettings->BankUpdateFreq[i]; + freq[i] = actuatorSettings.BankUpdateFreq[i]; clock[i] = ACTUATOR_PWM_CLOCK; break; } } memcpy(prevBankMode, - actuatorSettings->BankMode, + actuatorSettings.BankMode, sizeof(prevBankMode)); PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM); memcpy(prevBankUpdateFreq, - actuatorSettings->BankUpdateFreq, + actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)); // retrieve mode from related bank for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) { uint8_t bank = PIOS_Servo_GetPinBank(i); - pinsMode[i] = actuatorSettings->BankMode[bank]; + pinsMode[i] = actuatorSettings.BankMode[bank]; } } } static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - actuator_settings_updated = true; + ActuatorSettingsGet(&actuatorSettings); + spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; + if (frameType == FRAME_TYPE_GROUND) { + spinWhileArmed = false; + } + actuator_update_rate_if_changed(false); } static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - mixer_settings_updated = true; + MixerSettingsGet(&mixerSettings); + mixer_settings_count = 0; + Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; + for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { + if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) { + mixer_settings_count++; + } + } +} +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + frameType = GetCurrentFrameType(); +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + uint8_t TreatCustomCraftAs; + VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs); + + if (frameType == FRAME_TYPE_CUSTOM) { + switch (TreatCustomCraftAs) { + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: + frameType = FRAME_TYPE_FIXED_WING; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: + frameType = FRAME_TYPE_MULTIROTOR; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND: + frameType = FRAME_TYPE_GROUND; + break; + } + } +#endif + + SystemSettingsThrustControlGet(&thrustType); } /** diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index b17ce8334..2013c7580 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -37,7 +37,8 @@ #include // Private constants -#define ARMED_THRESHOLD 0.50f +#define ARMED_THRESHOLD 0.50f +#define GROUND_LOW_THROTTLE 0.01f // Private types typedef enum { @@ -62,7 +63,7 @@ static bool forcedDisArm(void); * @input: ManualControlCommand, AccessoryDesired * @output: FlightStatus.Arming */ -void armHandler(bool newinit) +void armHandler(bool newinit, FrameType_t frameType) { static ArmState_t armState; @@ -82,7 +83,12 @@ void armHandler(bool newinit) bool lowThrottle = cmd.Throttle < 0; - bool armSwitch = false; + if (frameType == FRAME_TYPE_GROUND) { + // Deadbanding applied in receiver.c typically at 2% but we don't assume its enabled. + lowThrottle = fabsf(cmd.Throttle) < GROUND_LOW_THROTTLE; + } + + bool armSwitch = false; switch (settings.Arming) { case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: @@ -304,6 +310,8 @@ static bool okToArm(void) return true; break; + case FLIGHTSTATUS_FLIGHTMODE_LAND: + return false; default: return false; diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index 6fce4ecf9..a52264443 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -32,6 +32,7 @@ #include #include +#include typedef void (*handlerFunc)(bool newinit); @@ -45,7 +46,7 @@ typedef struct controlHandlerStruct { * @input: ManualControlCommand, AccessoryDesired * @output: FlightStatus.Arming */ -void armHandler(bool newinit); +void armHandler(bool newinit, FrameType_t frameType); /** * @brief Handler to control Manual flightmode - input directly steers actuators diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index cf65a9328..70b872296 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -108,6 +108,7 @@ static float thrustHi = 0.0f; #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ // Private variables static DelayedCallbackInfo *callbackHandle; +static FrameType_t frameType = FRAME_TYPE_MULTIROTOR; // Private functions static void configurationUpdatedCb(UAVObjEvent *ev); @@ -116,6 +117,7 @@ static void manualControlTask(void); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings); #endif +static void SettingsUpdatedCb(UAVObjEvent *ev); #define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode) @@ -135,11 +137,14 @@ int32_t ManualControlStart() // clear alarms AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); + SettingsUpdatedCb(NULL); + // Make sure unarmed on power up - armHandler(true); + armHandler(true, frameType); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES takeOffLocationHandlerInit(); + #endif // Start main task PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); @@ -164,6 +169,8 @@ int32_t ManualControlInitialize() #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES VtolSelfTuningStatsInitialize(); VtolPathFollowerSettingsInitialize(); + VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + SystemSettingsConnectCallback(&SettingsUpdatedCb); #endif callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES); @@ -171,13 +178,37 @@ int32_t ManualControlInitialize() } MODULE_INITCALL(ManualControlInitialize, ManualControlStart); +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + frameType = GetCurrentFrameType(); +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + uint8_t TreatCustomCraftAs; + VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs); + + + if (frameType == FRAME_TYPE_CUSTOM) { + switch (TreatCustomCraftAs) { + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: + frameType = FRAME_TYPE_FIXED_WING; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: + frameType = FRAME_TYPE_MULTIROTOR; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND: + frameType = FRAME_TYPE_GROUND; + break; + } + } +#endif +} + /** * Module task */ static void manualControlTask(void) { // Process Arming - armHandler(false); + armHandler(false, frameType); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES takeOffLocationHandler(); #endif @@ -366,9 +397,15 @@ static void manualControlTask(void) break; #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + // During development the assistedcontrol implementation is optional and set + // set in stabi settings. Once if we decide to always have this on, it can + // can be directly set here...i.e. set the flight mode assist as required. case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: + case FLIGHTSTATUS_FLIGHTMODE_LAND: newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings); if (newFlightModeAssist) { + // Set the default thrust state switch (newFlightModeAssist) { case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST: newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; @@ -381,28 +418,14 @@ static void manualControlTask(void) newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None break; } - - switch (newAssistedControlState) { - case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE: - newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE; - break; - case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY: - newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE; - break; - case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD: - newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD; - break; - } } handler = &handler_PATHFOLLOWER; break; case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK: - case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_POI: case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: handler = &handler_PATHFOLLOWER; @@ -497,11 +520,15 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight thrustMode = modeSettings->Stabilization6Settings.Thrust; break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - // we hard code the "GPS Assisted" PostionHold to use alt-vario which + case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: + // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which // is a more appropriate throttle mode. "GPSAssist" adds braking // and a better throttle management to the standard Position Hold. thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO; break; + case FLIGHTSTATUS_FLIGHTMODE_LAND: + thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL; + break; // other modes will use cruisecontrol as default } diff --git a/flight/modules/ManualControl/pathfollowerhandler.c b/flight/modules/ManualControl/pathfollowerhandler.c index fb0420317..1a0818b68 100644 --- a/flight/modules/ManualControl/pathfollowerhandler.c +++ b/flight/modules/ManualControl/pathfollowerhandler.c @@ -57,7 +57,9 @@ void pathFollowerHandler(bool newinit) uint8_t flightMode; uint8_t assistedControlFlightMode; + uint8_t flightModeAssist; FlightStatusFlightModeGet(&flightMode); + FlightStatusFlightModeAssistGet(&flightModeAssist); FlightStatusAssistedControlStateGet(&assistedControlFlightMode); if (newinit) { @@ -67,8 +69,9 @@ void pathFollowerHandler(bool newinit) plan_setup_returnToBase(); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) { - // Just initiated braking after returning from stabi control + if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) && + (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) { + // Switch from primary (just entered this PH flight mode) into brake plan_setup_assistedcontrol(false); } else { plan_setup_positionHold(); @@ -78,7 +81,11 @@ void pathFollowerHandler(bool newinit) plan_setup_CourseLock(); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: - plan_setup_PositionRoam(); + if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) { + plan_setup_PositionRoam(); + } else { + plan_setup_VelocityRoam(); + } break; case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: plan_setup_HomeLeash(); @@ -88,7 +95,11 @@ void pathFollowerHandler(bool newinit) break; case FLIGHTSTATUS_FLIGHTMODE_LAND: - plan_setup_land(); + if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) { + plan_setup_land(); + } else { + plan_setup_VelocityRoam(); + } break; case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: plan_setup_AutoCruise(); @@ -117,7 +128,11 @@ void pathFollowerHandler(bool newinit) plan_run_CourseLock(); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: - plan_run_PositionRoam(); + if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) { + plan_run_PositionRoam(); + } else { + plan_run_VelocityRoam(); + } break; case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: plan_run_HomeLeash(); @@ -126,7 +141,9 @@ void pathFollowerHandler(bool newinit) plan_run_AbsolutePosition(); break; case FLIGHTSTATUS_FLIGHTMODE_LAND: - plan_run_land(); + if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) { + plan_run_VelocityRoam(); + } break; case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: plan_run_AutoCruise(); diff --git a/flight/modules/PathFollower/fixedwingflycontroller.cpp b/flight/modules/PathFollower/fixedwingflycontroller.cpp new file mode 100644 index 000000000..ed47d0069 --- /dev/null +++ b/flight/modules/PathFollower/fixedwingflycontroller.cpp @@ -0,0 +1,618 @@ +/* + ****************************************************************************** + * + * @file FixedWingFlyController.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Fixed wing fly controller implementation + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +// C++ includes +#include "fixedwingflycontroller.h" + +// Private constants + +// pointer to a singleton instance +FixedWingFlyController *FixedWingFlyController::p_inst = 0; + +FixedWingFlyController::FixedWingFlyController() + : fixedWingSettings(NULL), mActive(false), mMode(0), indicatedAirspeedStateBias(0.0f) +{} + +// Called when mode first engaged +void FixedWingFlyController::Activate(void) +{ + if (!mActive) { + mActive = true; + SettingsUpdated(); + resetGlobals(); + mMode = pathDesired->Mode; + } +} + +uint8_t FixedWingFlyController::IsActive(void) +{ + return mActive; +} + +uint8_t FixedWingFlyController::Mode(void) +{ + return mMode; +} + +// Objective updated in pathdesired +void FixedWingFlyController::ObjectiveUpdated(void) +{} + +void FixedWingFlyController::Deactivate(void) +{ + if (mActive) { + mActive = false; + resetGlobals(); + } +} + + +void FixedWingFlyController::SettingsUpdated(void) +{ + // fixed wing PID only + pid_configure(&PIDposH[0], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f); + pid_configure(&PIDposH[1], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f); + pid_configure(&PIDposV, fixedWingSettings->VerticalPosP, 0.0f, 0.0f, 0.0f); + + pid_configure(&PIDcourse, fixedWingSettings->CoursePI.Kp, fixedWingSettings->CoursePI.Ki, 0.0f, fixedWingSettings->CoursePI.ILimit); + pid_configure(&PIDspeed, fixedWingSettings->SpeedPI.Kp, fixedWingSettings->SpeedPI.Ki, 0.0f, fixedWingSettings->SpeedPI.ILimit); + pid_configure(&PIDpower, fixedWingSettings->PowerPI.Kp, fixedWingSettings->PowerPI.Ki, 0.0f, fixedWingSettings->PowerPI.ILimit); +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t FixedWingFlyController::Initialize(FixedWingPathFollowerSettingsData *ptr_fixedWingSettings) +{ + PIOS_Assert(ptr_fixedWingSettings); + + fixedWingSettings = ptr_fixedWingSettings; + + resetGlobals(); + + return 0; +} + +/** + * reset integrals + */ +void FixedWingFlyController::resetGlobals() +{ + pid_zero(&PIDposH[0]); + pid_zero(&PIDposH[1]); + pid_zero(&PIDposV); + pid_zero(&PIDcourse); + pid_zero(&PIDspeed); + pid_zero(&PIDpower); + pathStatus->path_time = 0.0f; +} + +void FixedWingFlyController::UpdateAutoPilot() +{ + uint8_t result = updateAutoPilotFixedWing(); + + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + + PathStatusSet(pathStatus); +} + +/** + * fixed wing autopilot: + * straight forward: + * 1. update path velocity for limited motion crafts + * 2. update attitude according to default fixed wing pathfollower algorithm + */ +uint8_t FixedWingFlyController::updateAutoPilotFixedWing() +{ + updatePathVelocity(fixedWingSettings->CourseFeedForward, true); + return updateFixedDesiredAttitude(); +} + +/** + * Compute desired velocity from the current position and path + */ +void FixedWingFlyController::updatePathVelocity(float kFF, bool limited) +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + VelocityDesiredData velocityDesired; + + const float dT = fixedWingSettings->UpdatePeriod / 1000.0f; + + // look ahead kFF seconds + float cur[3] = { positionState.North + (velocityState.North * kFF), + positionState.East + (velocityState.East * kFF), + positionState.Down + (velocityState.Down * kFF) }; + struct path_status progress; + path_progress(pathDesired, cur, &progress, true); + + // calculate velocity - can be zero if waypoints are too close + velocityDesired.North = progress.path_vector[0]; + velocityDesired.East = progress.path_vector[1]; + velocityDesired.Down = progress.path_vector[2]; + + if (limited && + // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) + // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector + // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) + // leading to an S-shape snake course the wrong way + // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't + // turn steep unless there is enough space complete the turn before crossing the flightpath + // in this case the plane effectively needs to be turned around + // indicators: + // difference between correction_direction and velocitystate >90 degrees and + // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything ) + // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) + // calculating angles < 90 degrees through dot products + (vector_lengthf(progress.path_vector, 2) > 1e-6f) && + ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) && + ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) { + ; + } else { + // calculate correction + velocityDesired.North += pid_apply(&PIDposH[0], progress.correction_vector[0], dT); + velocityDesired.East += pid_apply(&PIDposH[1], progress.correction_vector[1], dT); + } + velocityDesired.Down += pid_apply(&PIDposV, progress.correction_vector[2], dT); + + // update pathstatus + pathStatus->error = progress.error; + pathStatus->fractional_progress = progress.fractional_progress; + pathStatus->path_direction_north = progress.path_vector[0]; + pathStatus->path_direction_east = progress.path_vector[1]; + pathStatus->path_direction_down = progress.path_vector[2]; + + pathStatus->correction_direction_north = progress.correction_vector[0]; + pathStatus->correction_direction_east = progress.correction_vector[1]; + pathStatus->correction_direction_down = progress.correction_vector[2]; + + + VelocityDesiredSet(&velocityDesired); +} + + +/** + * Compute desired attitude from the desired velocity for fixed wing craft + */ +uint8_t FixedWingFlyController::updateFixedDesiredAttitude() +{ + uint8_t result = 1; + + const float dT = fixedWingSettings->UpdatePeriod / 1000.0f; + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + FixedWingPathFollowerStatusData fixedWingPathFollowerStatus; + AirspeedStateData airspeedState; + SystemSettingsData systemSettings; + + float groundspeedProjection; + float indicatedAirspeedState; + float indicatedAirspeedDesired; + float airspeedError; + + float pitchCommand; + + float descentspeedDesired; + float descentspeedError; + float powerCommand; + + float airspeedVector[2]; + float fluidMovement[2]; + float courseComponent[2]; + float courseError; + float courseCommand; + + FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus); + + VelocityStateGet(&velocityState); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeStateGet(&attitudeState); + AirspeedStateGet(&airspeedState); + SystemSettingsGet(&systemSettings); + + /** + * Compute speed error and course + */ + // missing sensors for airspeed-direction we have to assume within + // reasonable error that measured airspeed is actually the airspeed + // component in forward pointing direction + // airspeedVector is normalized + airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw); + airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw); + + // current ground speed projected in forward direction + groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; + + // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement, + // but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction + // than airspeed and gps sensors alone + indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias; + + // fluidMovement is a vector describing the aproximate movement vector of + // the surrounding fluid in 2d space (aka wind vector) + fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]); + fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]); + + // calculate the movement vector we need to fly to reach velocityDesired - + // taking fluidMovement into account + courseComponent[0] = velocityDesired.North - fluidMovement[0]; + courseComponent[1] = velocityDesired.East - fluidMovement[1]; + + indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]), + fixedWingSettings->HorizontalVelMin, + fixedWingSettings->HorizontalVelMax); + + // if we could fly at arbitrary speeds, we'd just have to move towards the + // courseComponent vector as previously calculated and we'd be fine + // unfortunately however we are bound by min and max air speed limits, so + // we need to recalculate the correct course to meet at least the + // velocityDesired vector direction at our current speed + // this overwrites courseComponent + bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired); + + // Error condition: wind speed too high, we can't go where we want anymore + fixedWingPathFollowerStatus.Errors.Wind = 0; + if ((!valid) && + fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Wind = 1; + result = 0; + } + + // Airspeed error + airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; + + // Vertical speed error + descentspeedDesired = boundf( + velocityDesired.Down, + -fixedWingSettings->VerticalVelMax, + fixedWingSettings->VerticalVelMax); + descentspeedError = descentspeedDesired - velocityState.Down; + + // Error condition: plane too slow or too fast + fixedWingPathFollowerStatus.Errors.Highspeed = 0; + fixedWingPathFollowerStatus.Errors.Lowspeed = 0; + if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) { + fixedWingPathFollowerStatus.Errors.Overspeed = 1; + result = 0; + } + if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) { + fixedWingPathFollowerStatus.Errors.Highspeed = 1; + result = 0; + } + if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) { + fixedWingPathFollowerStatus.Errors.Lowspeed = 1; + result = 0; + } + if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) { + fixedWingPathFollowerStatus.Errors.Stallspeed = 1; + result = 0; + } + + /** + * Compute desired thrust command + */ + + // Compute the cross feed from vertical speed to pitch, with saturation + float speedErrorToPowerCommandComponent = boundf( + (airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp, + -fixedWingSettings->AirspeedToPowerCrossFeed.Max, + fixedWingSettings->AirspeedToPowerCrossFeed.Max + ); + + // Compute final thrust response + powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) + + speedErrorToPowerCommandComponent; + + // Output internal state to telemetry + fixedWingPathFollowerStatus.Error.Power = descentspeedError; + fixedWingPathFollowerStatus.ErrorInt.Power = PIDpower.iAccumulator; + fixedWingPathFollowerStatus.Command.Power = powerCommand; + + // set thrust + stabDesired.Thrust = boundf(fixedWingSettings->ThrustLimit.Neutral + powerCommand, + fixedWingSettings->ThrustLimit.Min, + fixedWingSettings->ThrustLimit.Max); + + // Error condition: plane cannot hold altitude at current speed. + fixedWingPathFollowerStatus.Errors.Lowpower = 0; + if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum + velocityState.Down > 0.0f && // we ARE going down + descentspeedDesired < 0.0f && // we WANT to go up + airspeedError > 0.0f && // we are too slow already + fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Lowpower = 1; + result = 0; + } + // Error condition: plane keeps climbing despite minimum thrust (opposite of above) + fixedWingPathFollowerStatus.Errors.Highpower = 0; + if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum + velocityState.Down < 0.0f && // we ARE going up + descentspeedDesired > 0.0f && // we WANT to go down + airspeedError < 0.0f && // we are too fast already + fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Highpower = 1; + result = 0; + } + + /** + * Compute desired pitch command + */ + // Compute the cross feed from vertical speed to pitch, with saturation + float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp, + -fixedWingSettings->VerticalToPitchCrossFeed.Max, + fixedWingSettings->VerticalToPitchCrossFeed.Max + ); + + // Compute the pitch command as err*Kp + errInt*Ki + X_feed. + pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent; + + fixedWingPathFollowerStatus.Error.Speed = airspeedError; + fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator; + fixedWingPathFollowerStatus.Command.Speed = pitchCommand; + + stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand, + fixedWingSettings->PitchLimit.Min, + fixedWingSettings->PitchLimit.Max); + + // Error condition: high speed dive + fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0; + if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up + velocityState.Down > 0.0f && // we ARE going down + descentspeedDesired < 0.0f && // we WANT to go up + airspeedError < 0.0f && // we are too fast already + fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1; + result = 0; + } + + /** + * Compute desired roll command + */ + courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; + + if (courseError < -180.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f) { + courseError -= 360.0f; + } + + // overlap calculation. Theres a dead zone behind the craft where the + // counter-yawing of some craft while rolling could render a desired right + // turn into a desired left turn. Making the turn direction based on + // current roll angle keeps the plane committed to a direction once chosen + if (courseError < -180.0f + (fixedWingSettings->ReverseCourseOverlap * 0.5f) + && attitudeState.Roll > 0.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f - (fixedWingSettings->ReverseCourseOverlap * 0.5f) + && attitudeState.Roll < 0.0f) { + courseError -= 360.0f; + } + + courseCommand = pid_apply(&PIDcourse, courseError, dT); + + fixedWingPathFollowerStatus.Error.Course = courseError; + fixedWingPathFollowerStatus.ErrorInt.Course = PIDcourse.iAccumulator; + fixedWingPathFollowerStatus.Command.Course = courseCommand; + + stabDesired.Roll = boundf(fixedWingSettings->RollLimit.Neutral + + courseCommand, + fixedWingSettings->RollLimit.Min, + fixedWingSettings->RollLimit.Max); + + // TODO: find a check to determine loss of directional control. Likely needs some check of derivative + + + /** + * Compute desired yaw command + */ + // TODO implement raw control mode for yaw and base on Accels.Y + stabDesired.Yaw = 0.0f; + + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + + StabilizationDesiredSet(&stabDesired); + + FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus); + + return result; +} + + +/** + * Function to calculate course vector C based on airspeed s, fluid movement F + * and desired movement vector V + * parameters in: V,F,s + * parameters out: C + * returns true if a valid solution could be found for V,F,s, false if not + * C will be set to a best effort attempt either way + */ +bool FixedWingFlyController::correctCourse(float *C, float *V, float *F, float s) +{ + // Approach: + // Let Sc be a circle around origin marking possible movement vectors + // of the craft with airspeed s (all possible options for C) + // Let Vl be a line through the origin along movement vector V where fr any + // point v on line Vl v = k * (V / |V|) = k' * V + // Let Wl be a line parallel to Vl where for any point v on line Vl exists + // a point w on WL with w = v - F + // Then any intersection between circle Sc and line Wl represents course + // vector which would result in a movement vector + // V' = k * ( V / |V|) = k' * V + // If there is no intersection point, S is insufficient to compensate + // for F and we can only try to fly in direction of V (thus having wind drift + // but at least making progress orthogonal to wind) + + s = fabsf(s); + float f = vector_lengthf(F, 2); + + // normalize Cn=V/|V|, |V| must be >0 + float v = vector_lengthf(V, 2); + if (v < 1e-6f) { + // if |V|=0, we aren't supposed to move, turn into the wind + // (this allows hovering) + C[0] = -F[0]; + C[1] = -F[1]; + // if desired airspeed matches fluidmovement a hover is actually + // intended so return true + return fabsf(f - s) < 1e-3f; + } + float Vn[2] = { V[0] / v, V[1] / v }; + + // project F on V + float fp = F[0] * Vn[0] + F[1] * Vn[1]; + + // find component Fo of F that is orthogonal to V + // (which is exactly the distance between Vl and Wl) + float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) }; + float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1]; + + // find k where k * Vn = C - Fo + // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo + // so k^2 + fo^2 = s^2 (since |Vn|=1) + float k2 = s * s - fo2; + if (k2 <= -1e-3f) { + // there is no solution, we will be drifted off either way + // fallback: fly stupidly in direction of V and hope for the best + C[0] = V[0]; + C[1] = V[1]; + return false; + } else if (k2 <= 1e-3f) { + // there is exactly one solution: -Fo + C[0] = -Fo[0]; + C[1] = -Fo[1]; + return true; + } + // we have two possible solutions k positive and k negative as there are + // two intersection points between Wl and Sc + // which one is better? two criteria: + // 1. we MUST move in the right direction, if any k leads to -v its invalid + // 2. we should minimize the speed error + float k = sqrt(k2); + float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] }; + float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] }; + // project C+F on Vn to find signed resulting movement vector length + float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1]; + float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1]; + if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) { + // in this case the angle between course and resulting movement vector + // is greater than 90 degrees - so we actually fly backwards + C[0] = C1[0]; + C[1] = C1[1]; + return true; + } + C[0] = C2[0]; + C[1] = C2[1]; + if (vp2 >= 0.0f) { + // in this case the angle between course and movement vector is less than + // 90 degrees, but we do move in the right direction + return true; + } else { + // in this case we actually get driven in the opposite direction of V + // with both solutions for C + // this might be reached in headwind stronger than maximum allowed + // airspeed. + return false; + } +} + + +void FixedWingFlyController::AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + AirspeedStateData airspeedState; + VelocityStateData velocityState; + + AirspeedStateGet(&airspeedState); + VelocityStateGet(&velocityState); + float airspeedVector[2]; + float yaw; + AttitudeStateYawGet(&yaw); + airspeedVector[0] = cos_lookup_deg(yaw); + airspeedVector[1] = sin_lookup_deg(yaw); + // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement + float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; + + indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection; + // note - we do fly by Indicated Airspeed (== calibrated airspeed) however + // since airspeed is updated less often than groundspeed, we use sudden + // changes to groundspeed to offset the airspeed by the same measurement. + // This has a side effect that in the absence of any airspeed updates, the + // pathfollower will fly using groundspeed. +} diff --git a/flight/modules/PathFollower/grounddrivecontroller.cpp b/flight/modules/PathFollower/grounddrivecontroller.cpp new file mode 100644 index 000000000..3397a630e --- /dev/null +++ b/flight/modules/PathFollower/grounddrivecontroller.cpp @@ -0,0 +1,303 @@ +/* + ****************************************************************************** + * + * @file grounddrivecontroller.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Ground drive controller + * the required PathDesired LAND mode. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +// C++ includes +#include "grounddrivecontroller.h" + +// Private constants + +// pointer to a singleton instance +GroundDriveController *GroundDriveController::p_inst = 0; + +GroundDriveController::GroundDriveController() + : groundSettings(0), mActive(false), mMode(0) +{} + +// Called when mode first engaged +void GroundDriveController::Activate(void) +{ + if (!mActive) { + mActive = true; + SettingsUpdated(); + controlNE.Activate(); + mMode = pathDesired->Mode; + } +} + +uint8_t GroundDriveController::IsActive(void) +{ + return mActive; +} + +uint8_t GroundDriveController::Mode(void) +{ + return mMode; +} + +// Objective updated in pathdesired +void GroundDriveController::ObjectiveUpdated(void) +{} + +void GroundDriveController::Deactivate(void) +{ + if (mActive) { + mActive = false; + controlNE.Deactivate(); + } +} + + +void GroundDriveController::SettingsUpdated(void) +{ + const float dT = groundSettings->UpdatePeriod / 1000.0f; + + controlNE.UpdatePositionalParameters(groundSettings->HorizontalPosP); + controlNE.UpdateParameters(groundSettings->SpeedPI.Kp, + groundSettings->SpeedPI.Ki, + groundSettings->SpeedPI.Kd, + groundSettings->SpeedPI.Beta, + dT, + groundSettings->HorizontalVelMax); + + + // max/min are NE command values equivalent to thrust but must be symmetrical as this is NE not forward/reverse. + controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward); +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t GroundDriveController::Initialize(GroundPathFollowerSettingsData *ptr_groundSettings) +{ + PIOS_Assert(ptr_groundSettings); + + groundSettings = ptr_groundSettings; + + return 0; +} + +void GroundDriveController::UpdateAutoPilot() +{ + uint8_t result = updateAutoPilotGround(); + + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + + PathStatusSet(pathStatus); +} + +/** + * fixed wing autopilot: + * straight forward: + * 1. update path velocity for limited motion crafts + * 2. update attitude according to default fixed wing pathfollower algorithm + */ +uint8_t GroundDriveController::updateAutoPilotGround() +{ + updatePathVelocity(groundSettings->CourseFeedForward); + return updateGroundDesiredAttitude(); +} + +/** + * Compute desired velocity from the current position and path + */ +void GroundDriveController::updatePathVelocity(float kFF) +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + VelocityDesiredData velocityDesired; + controlNE.UpdateVelocityState(velocityState.North, velocityState.East); + + // look ahead kFF seconds + float cur[3] = { positionState.North + (velocityState.North * kFF), + positionState.East + (velocityState.East * kFF), + positionState.Down + (velocityState.Down * kFF) }; + struct path_status progress; + path_progress(pathDesired, cur, &progress, false); + + // GOTOENDPOINT: correction_vector is distance array to endpoint, path_vector is velocity vector + // FOLLOWVECTOR: correct_vector is distance to vector path, path_vector is the desired velocity vector + + // Calculate the desired velocity from the lateral vector path errors (correct_vector) and the desired velocity vector (path_vector) + controlNE.ControlPositionWithPath(&progress); + float north, east; + controlNE.GetVelocityDesired(&north, &east); + velocityDesired.North = north; + velocityDesired.East = east; + velocityDesired.Down = 0.0f; + + // update pathstatus + pathStatus->error = progress.error; + pathStatus->fractional_progress = progress.fractional_progress; + // FOLLOWVECTOR: desired velocity vector + pathStatus->path_direction_north = progress.path_vector[0]; + pathStatus->path_direction_east = progress.path_vector[1]; + pathStatus->path_direction_down = progress.path_vector[2]; + + // FOLLOWVECTOR: correction distance to vector path + pathStatus->correction_direction_north = progress.correction_vector[0]; + pathStatus->correction_direction_east = progress.correction_vector[1]; + pathStatus->correction_direction_down = progress.correction_vector[2]; + + VelocityDesiredSet(&velocityDesired); +} + +/** + * Compute desired attitude for ground vehicles + */ +uint8_t GroundDriveController::updateGroundDesiredAttitude() +{ + StatusGroundDriveData statusGround; + VelocityStateData velocityState; + + VelocityStateGet(&velocityState); + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + statusGround.State.Yaw = attitudeState.Yaw; + statusGround.State.Velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East); + + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + + // estimate a north/east command value to control the velocity error. + // ThrustLimits.Max(+-) limits the range. Think of this as a command unit vector + // of the ultimate direction to reduce lateral error and achieve the target direction (desired angle). + float northCommand, eastCommand; + controlNE.GetNECommand(&northCommand, &eastCommand); + + // Get current vehicle orientation + float angle_radians = DEG2RAD(attitudeState.Yaw); // (+-pi) + float cos_angle = cosf(angle_radians); + float sine_angle = sinf(angle_radians); + + float courseCommand = 0.0f; + float speedCommand = 0.0f; + float lateralCommand = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max); + float forwardCommand = boundf(northCommand * cos_angle + eastCommand * sine_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max); + // +ve facing correct direction, lateral command should just correct angle, + if (forwardCommand > 0.0f) { + // if +ve forward command, -+ lateralCommand drives steering to manage lateral error and angular error + + courseCommand = boundf(lateralCommand, -1.0f, 1.0f); + speedCommand = boundf(forwardCommand, groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.Max); + + // reinstate max thrust + controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward); + + statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_ONTRACK; + } else { + // -ve facing opposite direction, lateral command irrelevant, need to turn to change direction and do so slowly. + + // Reduce steering angle based on current velocity + float steeringReductionFactor = 1.0f; + if (stabDesired.Thrust > 0.3f) { + steeringReductionFactor = (groundSettings->HorizontalVelMax - statusGround.State.Velocity) / groundSettings->HorizontalVelMax; + steeringReductionFactor = boundf(steeringReductionFactor, 0.05f, 1.0f); + } + + // should we turn left or right? + if (lateralCommand >= 0.1f) { + courseCommand = 1.0f * steeringReductionFactor; + statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDRIGHT; + } else { + courseCommand = -1.0f * steeringReductionFactor; + statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDLEFT; + } + + // Impose limits to slow down. + controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.SlowForward, 0.0f); + + speedCommand = groundSettings->ThrustLimit.SlowForward; + } + + stabDesired.Roll = 0.0f; + stabDesired.Pitch = 0.0f; + stabDesired.Yaw = courseCommand; + + // Mix yaw into thrust limit TODO + stabDesired.Thrust = boundf(speedCommand, groundSettings->ThrustLimit.Min, groundSettings->ThrustLimit.Max); + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + StabilizationDesiredSet(&stabDesired); + + statusGround.NECommand.North = northCommand; + statusGround.NECommand.East = eastCommand; + statusGround.State.Thrust = stabDesired.Thrust; + statusGround.BodyCommand.Forward = forwardCommand; + statusGround.BodyCommand.Right = lateralCommand; + statusGround.ControlCommand.Course = courseCommand; + statusGround.ControlCommand.Speed = speedCommand; + StatusGroundDriveSet(&statusGround); + + return 1; +} diff --git a/flight/modules/PathFollower/inc/fixedwingflycontroller.h b/flight/modules/PathFollower/inc/fixedwingflycontroller.h new file mode 100644 index 000000000..19dcf4003 --- /dev/null +++ b/flight/modules/PathFollower/inc/fixedwingflycontroller.h @@ -0,0 +1,80 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup FixedWing CONTROL interface class + * @brief CONTROL interface class for pathfollower fixed wing fly controller + * @{ + * + * @file FixedWingCONTROL.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes CONTROL for fixed wing fly objectives + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef FIXEDWINGFLYCONTROLLER_H +#define FIXEDWINGFLYCONTROLLER_H +#include "pathfollowercontrol.h" + +class FixedWingFlyController : public PathFollowerControl { +private: + static FixedWingFlyController *p_inst; + FixedWingFlyController(); + + +public: + static FixedWingFlyController *instance() + { + if (!p_inst) { + p_inst = new FixedWingFlyController(); + } + return p_inst; + } + + int32_t Initialize(FixedWingPathFollowerSettingsData *fixedWingSettings); + void Activate(void); + void Deactivate(void); + void SettingsUpdated(void); + void UpdateAutoPilot(void); + void ObjectiveUpdated(void); + uint8_t IsActive(void); + uint8_t Mode(void); + void AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent * ev); + +private: + void resetGlobals(); + uint8_t updateAutoPilotFixedWing(); + void updatePathVelocity(float kFF, bool limited); + uint8_t updateFixedDesiredAttitude(); + bool correctCourse(float *C, float *V, float *F, float s); + + FixedWingPathFollowerSettingsData *fixedWingSettings; + uint8_t mActive; + uint8_t mMode; + + struct pid PIDposH[2]; + struct pid PIDposV; + struct pid PIDcourse; + struct pid PIDspeed; + struct pid PIDpower; + // correct speed by measured airspeed + float indicatedAirspeedStateBias; +}; + +#endif // FIXEDWINGFLYCONTROLLER_H diff --git a/flight/modules/PathFollower/inc/grounddrivecontroller.h b/flight/modules/PathFollower/inc/grounddrivecontroller.h new file mode 100644 index 000000000..106504769 --- /dev/null +++ b/flight/modules/PathFollower/inc/grounddrivecontroller.h @@ -0,0 +1,71 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Ground CONTROL interface class + * @brief CONTROL interface class for ground drive controller + * @{ + * + * @file grounddrivecontroller.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Class definition for ground drive controller implementation + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef GROUNDDRIVECONTROLLER_H +#define GROUNDDRIVECONTROLLER_H +#include "pathfollowercontrol.h" +#include "pidcontrolne.h" + +class GroundDriveController : public PathFollowerControl { +private: + static GroundDriveController *p_inst; + GroundDriveController(); + + +public: + static GroundDriveController *instance() + { + if (!p_inst) { + p_inst = new GroundDriveController(); + } + return p_inst; + } + + int32_t Initialize(GroundPathFollowerSettingsData *groundSettings); + void Activate(void); + void Deactivate(void); + void SettingsUpdated(void); + void UpdateAutoPilot(void); + void ObjectiveUpdated(void); + uint8_t IsActive(void); + uint8_t Mode(void); + +private: + uint8_t updateAutoPilotGround(); + void updatePathVelocity(float kFF); + uint8_t updateGroundDesiredAttitude(); + + GroundPathFollowerSettingsData *groundSettings; + uint8_t mActive; + uint8_t mMode; + PIDControlNE controlNE; +}; + +#endif // GROUNDDRIVECONTROLLER_H diff --git a/flight/modules/PathFollower/inc/pathfollowercontrol.h b/flight/modules/PathFollower/inc/pathfollowercontrol.h new file mode 100644 index 000000000..056db050c --- /dev/null +++ b/flight/modules/PathFollower/inc/pathfollowercontrol.h @@ -0,0 +1,50 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower CONTROL interface class + * @brief CONTROL interface class for pathfollower goal implementations + * @{ + * + * @file pathfollowercontrol.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Interface class for controllers + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PATHFOLLOWERCONTROL_H +#define PATHFOLLOWERCONTROL_H +class PathFollowerControl { +public: + virtual void Activate(void) = 0; + virtual void Deactivate(void) = 0; + virtual void SettingsUpdated(void) = 0; + virtual void UpdateAutoPilot(void) = 0; + virtual void ObjectiveUpdated(void) = 0; + virtual uint8_t Mode(void) = 0; + static int32_t Initialize(PathDesiredData *ptr_pathDesired, + FlightStatusData *ptr_flightStatus, + PathStatusData *ptr_pathStatus); +protected: + static PathDesiredData *pathDesired; + static FlightStatusData *flightStatus; + static PathStatusData *pathStatus; +}; + +#endif // PATHFOLLOWERCONTROL_H diff --git a/flight/modules/PathFollower/inc/pathfollowerfsm.h b/flight/modules/PathFollower/inc/pathfollowerfsm.h new file mode 100644 index 000000000..9701bc7de --- /dev/null +++ b/flight/modules/PathFollower/inc/pathfollowerfsm.h @@ -0,0 +1,80 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower FSM interface class + * @brief FSM interface class for pathfollower goal fsm implementations + * @{ + * + * @file pathfollowerfsm.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Interface class for PathFollower FSMs + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PATHFOLLOWERFSM_H +#define PATHFOLLOWERFSM_H + +extern "C" { +#include +} + +typedef enum { + PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup + PFFSM_STATE_ACTIVE, + PFFSM_STATE_DISARMED, + PFFSM_STATE_ABORT // Abort on error +} PathFollowerFSMState_T; + +class PathFollowerFSM { +public: + // PathFollowerFSM() {}; + virtual void Inactive(void) {} + virtual void Activate(void) {} + virtual void Update(void) {} + virtual void SettingsUpdated(void) {} + virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) {} + virtual PathFollowerFSMState_T GetCurrentState(void) + { + return PFFSM_STATE_INACTIVE; + } + virtual void ConstrainStabiDesired(__attribute__((unused)) StabilizationDesiredData *stabDesired) {} + virtual float BoundVelocityDown(float velocity) + { + return velocity; + } + virtual void CheckPidScaler(__attribute__((unused)) pid_scaler *scaler) {} + virtual void GetYaw(bool &yaw_attitude, float &yaw_direction) + { + yaw_attitude = false; yaw_direction = 0.0f; + } + virtual void Abort(void) {} + virtual uint8_t ManualThrust(void) + { + return false; + } + virtual uint8_t PositionHoldState(void) + { + return false; + } + + // virtual ~PathFollowerFSM() = 0; +}; + +#endif // PATHFOLLOWERFSM_H diff --git a/flight/modules/PathFollower/inc/pidcontroldown.h b/flight/modules/PathFollower/inc/pidcontroldown.h new file mode 100644 index 000000000..9546e2e75 --- /dev/null +++ b/flight/modules/PathFollower/inc/pidcontroldown.h @@ -0,0 +1,95 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower PID Control implementation + * @brief PID Controller for down direction + * @{ + * + * @file PIDControlDown.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes control loop for down direction + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PIDCONTROLDOWN_H +#define PIDCONTROLDOWN_H +extern "C" { +#include +#include +} +#include "pathfollowerfsm.h" + +class PIDControlDown { +public: + PIDControlDown(); + ~PIDControlDown(); + void Initialize(PathFollowerFSM *fsm); + void SetThrustLimits(float min_thrust, float max_thrust); + void Deactivate(); + void Activate(); + void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax); + void UpdateNeutralThrust(float neutral); + void UpdateVelocitySetpoint(float setpoint); + void RateLimit(float *spDesired, float *spCurrent, float rateLimit); + void UpdateVelocityState(float pv); + float GetVelocityDesired(void); + float GetDownCommand(void); + void UpdatePositionalParameters(float kp); + void UpdatePositionState(float pvDown); + void UpdatePositionSetpoint(float setpointDown); + void ControlPosition(); + void ControlPositionWithPath(struct path_status *progress); + void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity); + void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate); + +private: + void setup_neutralThrustCalc(); + void run_neutralThrustCalc(); + + struct pid2 PID; + float deltaTime; + float mVelocitySetpointTarget; + float mVelocitySetpointCurrent; + float mVelocityState; + float mDownCommand; + PathFollowerFSM *mFSM; + float mNeutral; + float mVelocityMax; + struct pid PIDpos; + float mPositionSetpointTarget; + float mPositionState; + float mMinThrust; + float mMaxThrust; + uint8_t mActive; + + struct NeutralThrustEstimation { + uint32_t count; + float sum; + float average; + float correction; + float min; + float max; + bool start_sampling; + bool have_correction; + }; + struct NeutralThrustEstimation neutralThrustEst; +}; + +#endif // PIDCONTROLDOWN_H diff --git a/flight/modules/PathFollower/inc/pidcontrolne.h b/flight/modules/PathFollower/inc/pidcontrolne.h new file mode 100644 index 000000000..8d2fa0d3b --- /dev/null +++ b/flight/modules/PathFollower/inc/pidcontrolne.h @@ -0,0 +1,80 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower CONTROL interface class + * @brief PID Controler for NE Class definition + * @{ + * + * @file pidcontrolne.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes pid control loop for NE + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PIDCONTROLNE_H +#define PIDCONTROLNE_H +extern "C" { +#include +} +#include "pathfollowerfsm.h" + +class PIDControlNE { +public: + PIDControlNE(); + ~PIDControlNE(); + void Initialize(); + void Deactivate(); + void Activate(); + void UpdateParameters(float kp, float ki, float kd, __attribute__((unused)) float ilimit, float dT, float velocityMax); + void UpdatePositionalParameters(float kp); + void UpdatePositionState(float pvNorth, float pvEast); + void UpdatePositionSetpoint(float setpointNorth, float setpointEast); + void UpdateVelocitySetpoint(float setpointNorth, float setpointEast); + // void RateLimit(float *spDesired, float *spCurrent, float rateLimit); + void UpdateVelocityState(float pvNorth, float pvEast); + void UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward); + void ControlPosition(); + void ControlPositionWithPath(struct path_status *progress); + void GetNECommand(float *northCommand, float *eastCommand); + void GetVelocityDesired(float *north, float *east); + void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity); + void UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate); + +private: + struct pid2 PIDvel[2]; // North, East + float mVelocitySetpointTarget[2]; + float mVelocityState[2]; + struct pid PIDposH[2]; + float mPositionSetpointTarget[2]; + float mPositionState[2]; + + + float deltaTime; + float mVelocitySetpointCurrent[2]; + float mNECommand; + float mNeutral; + float mVelocityMax; + float mMinCommand; + float mMaxCommand; + float mVelocityFeedforward; + uint8_t mActive; +}; + +#endif // PIDCONTROLNE_H diff --git a/flight/modules/PathFollower/inc/vtolbrakecontroller.h b/flight/modules/PathFollower/inc/vtolbrakecontroller.h new file mode 100644 index 000000000..102ffc947 --- /dev/null +++ b/flight/modules/PathFollower/inc/vtolbrakecontroller.h @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower CONTROL interface class + * @brief CONTROL interface class for brake controller + * @{ + * + * @file vtolbrakecontroller.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes brake controller for vtol + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef VTOLBRAKECONTROLLER_H +#define VTOLBRAKECONTROLLER_H +#include "pathfollowercontrol.h" +#include "pidcontroldown.h" +#include "pidcontrolne.h" +// forward decl +class PathFollowerFSM; +class VtolBrakeController : public PathFollowerControl { +private: + static VtolBrakeController *p_inst; + VtolBrakeController(); + + +public: + static VtolBrakeController *instance() + { + if (!p_inst) { + p_inst = new VtolBrakeController(); + } + return p_inst; + } + + int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings); + void Activate(void); + void Deactivate(void); + void SettingsUpdated(void); + void UpdateAutoPilot(void); + void ObjectiveUpdated(void); + uint8_t IsActive(void); + uint8_t Mode(void); + +private: + void UpdateVelocityDesired(void); + int8_t UpdateStabilizationDesired(void); + void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity); + + PathFollowerFSM *fsm; + VtolPathFollowerSettingsData *vtolPathFollowerSettings; + PIDControlDown controlDown; + PIDControlNE controlNE; + uint8_t mActive; + uint8_t mHoldActive; + uint8_t mManualThrust; +}; + +#endif // VTOLBRAKECONTROLLER_H diff --git a/flight/modules/PathFollower/inc/vtolbrakefsm.h b/flight/modules/PathFollower/inc/vtolbrakefsm.h new file mode 100644 index 000000000..6e5151977 --- /dev/null +++ b/flight/modules/PathFollower/inc/vtolbrakefsm.h @@ -0,0 +1,111 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower FSM Brake + * @brief Executes brake seqeuence + * @{ + * + * @file vtolbrakfsm.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes brake sequence fsm + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef VTOLBRAKEFSM_H +#define VTOLBRAKEFSM_H + +#include "pathfollowerfsm.h" + +// Brakeing state machine +typedef enum { + BRAKE_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup + BRAKE_STATE_BRAKE, // Initiate altitude hold before starting descent + BRAKE_STATE_HOLD, // Waiting for attainment of landing descent rate + BRAKE_STATE_SIZE +} PathFollowerFSM_BrakeState_T; + +typedef enum { + FSMBRAKESTATUS_STATEEXITREASON_NONE = 0 +} VtolBrakeFSMStatusStateExitReasonOptions; + +class VtolBrakeFSM : public PathFollowerFSM { +private: + static VtolBrakeFSM *p_inst; + VtolBrakeFSM(); + +public: + static VtolBrakeFSM *instance() + { + if (!p_inst) { + p_inst = new VtolBrakeFSM(); + } + return p_inst; + } + int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings, + PathDesiredData *pathDesired, + FlightStatusData *flightStatus, + PathStatusData *ptr_pathStatus); + void Inactive(void); + void Activate(void); + void Update(void); + PathFollowerFSMState_T GetCurrentState(void); + void Abort(void); + uint8_t PositionHoldState(void); + +protected: + + // FSM instance data type + typedef struct { + PathFollowerFSM_BrakeState_T currentState; + uint32_t stateRunCount; + uint32_t stateTimeoutCount; + float sum1; + float sum2; + uint8_t observationCount; + uint8_t observation2Count; + } VtolBrakeFSMData_T; + + // FSM state structure + typedef struct { + void(VtolBrakeFSM::*setup) (void); // Called to initialise the state + void(VtolBrakeFSM::*run) (uint8_t); // Run the event detection code for a state + } PathFollowerFSM_BrakeStateHandler_T; + + // Private variables + VtolBrakeFSMData_T *mBrakeData; + VtolPathFollowerSettingsData *vtolPathFollowerSettings; + PathDesiredData *pathDesired; + PathStatusData *pathStatus; + FlightStatusData *flightStatus; + + void setup_brake(void); + void run_brake(uint8_t); + void initFSM(void); + void setState(PathFollowerFSM_BrakeState_T newState, VtolBrakeFSMStatusStateExitReasonOptions reason); + int32_t runState(); + // void updateVtolBrakeFSMStatus(); + + void setStateTimeout(int32_t count); + void fallback_to_hold(void); + + static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE]; +}; + +#endif // VTOLBRAKEFSM_H diff --git a/flight/modules/PathFollower/inc/vtolflycontroller.h b/flight/modules/PathFollower/inc/vtolflycontroller.h new file mode 100644 index 000000000..af4ccab83 --- /dev/null +++ b/flight/modules/PathFollower/inc/vtolflycontroller.h @@ -0,0 +1,84 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower CONTROL interface class + * @brief vtol fly controller class definition + * @{ + * + * @file vtolflycontroller.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes fly control for vtols + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef VTOLFLYCONTROLLER_H +#define VTOLFLYCONTROLLER_H +#include "pathfollowercontrol.h" +#include "pidcontrolne.h" +#include "pidcontroldown.h" + +class VtolFlyController : public PathFollowerControl { +private: + static VtolFlyController *p_inst; + VtolFlyController(); + + +public: + static VtolFlyController *instance() + { + if (!p_inst) { + p_inst = new VtolFlyController(); + } + return p_inst; + } + + int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings); + + + void Activate(void); + void Deactivate(void); + void SettingsUpdated(void); + void UpdateAutoPilot(void); + void ObjectiveUpdated(void); + uint8_t IsActive(void); + uint8_t Mode(void); + +private: + void UpdateVelocityDesired(void); + int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction); + void UpdateDesiredAttitudeEmergencyFallback(); + void fallback_to_hold(void); + float updateTailInBearing(); + float updateCourseBearing(); + float updatePathBearing(); + float updatePOIBearing(); + uint8_t RunAutoPilot(); + + VtolPathFollowerSettingsData *vtolPathFollowerSettings; + PIDControlNE controlNE; + PIDControlDown controlDown; + uint8_t mActive; + uint8_t mManualThrust; + uint8_t mMode; + float vtolEmergencyFallback; + bool vtolEmergencyFallbackSwitch; +}; + +#endif // VTOLFLYCONTROLLER_H diff --git a/flight/modules/PathFollower/inc/vtollandcontroller.h b/flight/modules/PathFollower/inc/vtollandcontroller.h new file mode 100644 index 000000000..23fd2c346 --- /dev/null +++ b/flight/modules/PathFollower/inc/vtollandcontroller.h @@ -0,0 +1,76 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower CONTROL interface class + * @brief vtol land controller class + * @{ + * + * @file vtollandcontroller.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes CONTROL for landing sequence + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef VTOLLANDCONTROLLER_H +#define VTOLLANDCONTROLLER_H +#include "pathfollowercontrol.h" +#include "pidcontroldown.h" +#include "pidcontrolne.h" +// forward decl +class PathFollowerFSM; +class VtolLandController : public PathFollowerControl { +private: + static VtolLandController *p_inst; + VtolLandController(); + + +public: + static VtolLandController *instance() + { + if (!p_inst) { + p_inst = new VtolLandController(); + } + return p_inst; + } + + int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings); + + + void Activate(void); + void Deactivate(void); + void SettingsUpdated(void); + void UpdateAutoPilot(void); + void ObjectiveUpdated(void); + uint8_t IsActive(void); + uint8_t Mode(void); + +private: + void UpdateVelocityDesired(void); + int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction); + void setArmedIfChanged(uint8_t val); + + PathFollowerFSM *fsm; + VtolPathFollowerSettingsData *vtolPathFollowerSettings; + PIDControlDown controlDown; + PIDControlNE controlNE; + uint8_t mActive; +}; + +#endif // VTOLLANDCONTROLLER_H diff --git a/flight/modules/PathFollower/inc/vtollandfsm.h b/flight/modules/PathFollower/inc/vtollandfsm.h new file mode 100644 index 000000000..fb806a5da --- /dev/null +++ b/flight/modules/PathFollower/inc/vtollandfsm.h @@ -0,0 +1,148 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower FSM + * @brief Executes landing sequence via an FSM + * @{ + * + * @file vtollandfsm.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes FSM for landing sequence + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef VTOLLANDFSM_H +#define VTOLLANDFSM_H + +extern "C" { +#include "statusvtolland.h" +} +#include "pathfollowerfsm.h" + +// Landing state machine +typedef enum { + LAND_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup + LAND_STATE_INIT_ALTHOLD, // Initiate altitude hold before starting descent + LAND_STATE_WTG_FOR_DESCENTRATE, // Waiting for attainment of landing descent rate + LAND_STATE_AT_DESCENTRATE, // Landing descent rate achieved + LAND_STATE_WTG_FOR_GROUNDEFFECT, // Waiting for group effect to be detected + LAND_STATE_GROUNDEFFECT, // Ground effect detected + LAND_STATE_THRUSTDOWN, // Thrust down sequence + LAND_STATE_THRUSTOFF, // Thrust is now off + LAND_STATE_DISARMED, // Disarmed + LAND_STATE_ABORT, // Abort on error triggerig fallback to hold + LAND_STATE_SIZE +} PathFollowerFSM_LandState_T; + +class VtolLandFSM : public PathFollowerFSM { +private: + static VtolLandFSM *p_inst; + VtolLandFSM(); + +public: + static VtolLandFSM *instance() + { + if (!p_inst) { + p_inst = new VtolLandFSM(); + } + return p_inst; + } + int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings, + PathDesiredData *pathDesired, + FlightStatusData *flightStatus); + void Inactive(void); + void Activate(void); + void Update(void); + void BoundThrust(float &ulow, float &uhigh); + PathFollowerFSMState_T GetCurrentState(void); + void ConstrainStabiDesired(StabilizationDesiredData *stabDesired); + float BoundVelocityDown(float); + void CheckPidScaler(pid_scaler *scaler); + void Abort(void); + +protected: + + // FSM instance data type + typedef struct { + StatusVtolLandData fsmLandStatus; + PathFollowerFSM_LandState_T currentState; + TakeOffLocationData takeOffLocation; + uint32_t stateRunCount; + uint32_t stateTimeoutCount; + float sum1; + float sum2; + float expectedLandPositionNorth; + float expectedLandPositionEast; + float thrustLimit; + float boundThrustMin; + float boundThrustMax; + uint8_t observationCount; + uint8_t observation2Count; + uint8_t flZeroStabiHorizontal; + uint8_t flConstrainThrust; + uint8_t flLowAltitude; + uint8_t flAltitudeHold; + } VtolLandFSMData_T; + + // FSM state structure + typedef struct { + void(VtolLandFSM::*setup) (void); // Called to initialise the state + void(VtolLandFSM::*run) (uint8_t); // Run the event detection code for a state + } PathFollowerFSM_LandStateHandler_T; + + // Private variables + VtolLandFSMData_T *mLandData; + VtolPathFollowerSettingsData *vtolPathFollowerSettings; + PathDesiredData *pathDesired; + FlightStatusData *flightStatus; + + void setup_inactive(void); + void setup_init_althold(void); + void setup_wtg_for_descentrate(void); + void setup_at_descentrate(void); + void setup_wtg_for_groundeffect(void); + void run_init_althold(uint8_t); + void run_wtg_for_descentrate(uint8_t); + void run_at_descentrate(uint8_t); + void run_wtg_for_groundeffect(uint8_t); + void setup_groundeffect(void); + void run_groundeffect(uint8_t); + void setup_thrustdown(void); + void run_thrustdown(uint8_t); + void setup_thrustoff(void); + void run_thrustoff(uint8_t); + void setup_disarmed(void); + void run_disarmed(uint8_t); + void setup_abort(void); + void run_abort(uint8_t); + void initFSM(void); + void setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason); + int32_t runState(); + int32_t runAlways(); + void updateVtolLandFSMStatus(); + void assessAltitude(void); + + void setStateTimeout(int32_t count); + void fallback_to_hold(void); + + static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE]; +}; + +#endif // VTOLLANDFSM_H diff --git a/flight/modules/PathFollower/inc/vtolvelocitycontroller.h b/flight/modules/PathFollower/inc/vtolvelocitycontroller.h new file mode 100644 index 000000000..c5209a3d1 --- /dev/null +++ b/flight/modules/PathFollower/inc/vtolvelocitycontroller.h @@ -0,0 +1,72 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower Controller + * @brief Controller for Vtol velocity roam + * @{ + * + * @file vtolvelocitycontroller.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes velocity roam control + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PATHFOLLOWERCONTROLVELOCITYROAM_H +#define PATHFOLLOWERCONTROLVELOCITYROAM_H +#include "pathfollowercontrol.h" +#include "pidcontrolne.h" + +class VtolVelocityController : public PathFollowerControl { +private: + static VtolVelocityController *p_inst; + VtolVelocityController(); + + +public: + static VtolVelocityController *instance() + { + if (!p_inst) { + p_inst = new VtolVelocityController(); + } + return p_inst; + } + + int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings); + + + void Activate(void); + void Deactivate(void); + void SettingsUpdated(void); + void UpdateAutoPilot(void); + void ObjectiveUpdated(void); + uint8_t IsActive(void); + uint8_t Mode(void); + +private: + void UpdateVelocityDesired(void); + int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction); + void fallback_to_hold(void); + + VtolPathFollowerSettingsData *vtolPathFollowerSettings; + PIDControlNE controlNE; + uint8_t mActive; +}; + +#endif // ifndef PATHFOLLOWERCONTROLVELOCITYROAM_H diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c deleted file mode 100644 index a9389db5b..000000000 --- a/flight/modules/PathFollower/pathfollower.c +++ /dev/null @@ -1,1544 +0,0 @@ -/** - ****************************************************************************** - * - * @file pathfollower.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint - * and sets @ref AttitudeDesired. It only does this when the FlightMode field - * of @ref ManualControlCommand is Auto. - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -/** - * Input object: PathDesired - * Input object: PositionState - * Input object: ManualControlCommand - * Output object: StabilizationDesired - * - * This module acts as "autopilot" - it controls the setpoints of stabilization - * based on current flight situation and desired flight path (PathDesired) as - * directed by flightmode selection or pathplanner - * This is a periodic delayed callback module - * - * Modules have no API, all communication to other modules is done through UAVObjects. - * However modules may use the API exposed by shared libraries. - * See the OpenPilot wiki for more details. - * http://www.openpilot.org/OpenPilot_Application_Architecture - * - */ - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include "plans.h" -#include - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -// Private constants - -#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL - -#define PF_IDLE_UPDATE_RATE_MS 100 - -#define STACK_SIZE_BYTES 2048 - -#define DEADBAND_HIGH 0.10f -#define DEADBAND_LOW -0.10f - -#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f -#define BRAKE_EXIT_VELOCITY_LIMIT 0.2f - -#define BRAKE_RATE_MINIMUM 0.2f - -#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f -#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT 0.2f -#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT 0.2f -#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT 0.1f - -#define NEUTRALTHRUST_START_DELAY (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate) -#define NEUTRALTHRUST_END_COUNT (NEUTRALTHRUST_START_DELAY + (4 * 20)) // 4 second sample - -// Private types - -struct Globals { - struct pid PIDposH[2]; - struct pid PIDposV; - struct pid PIDvel[3]; // North, East, Down - struct pid BrakePIDvel[2]; // North, East - struct pid PIDcourse; - struct pid PIDspeed; - struct pid PIDpower; - float poiRadius; - float vtolEmergencyFallback; - bool vtolEmergencyFallbackSwitch; -}; - -struct NeutralThrustEstimation { - uint32_t count; - float sum; - float average; - float correction; - float algo_erro_check; - float min; - float max; - bool start_sampling; - bool have_correction; -}; -static struct NeutralThrustEstimation neutralThrustEst; - - -// Private variables -static DelayedCallbackInfo *pathFollowerCBInfo; -static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; -static struct Globals global; -static PathStatusData pathStatus; -static PathDesiredData pathDesired; -static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings; -static VtolPathFollowerSettingsData vtolPathFollowerSettings; -static FlightStatusData flightStatus; -static PathSummaryData pathSummary; - -// correct speed by measured airspeed -static float indicatedAirspeedStateBias = 0.0f; - - -// Private functions -static void pathFollowerTask(void); -static void resetGlobals(); -static void SettingsUpdatedCb(UAVObjEvent *ev); -static uint8_t updateAutoPilotByFrameType(); -static uint8_t updateAutoPilotFixedWing(); -static uint8_t updateAutoPilotVtol(); -static float updateTailInBearing(); -static float updateCourseBearing(); -static float updatePathBearing(); -static float updatePOIBearing(); -static void processPOI(); -static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity); -static void updatePathVelocity(float kFF, bool limited); -static uint8_t updateFixedDesiredAttitude(); -static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction); -static void updateFixedAttitude(); -static void updateVtolDesiredAttitudeEmergencyFallback(); -static void airspeedStateUpdatedCb(UAVObjEvent *ev); -static bool correctCourse(float *C, float *V, float *F, float s); - -/** - * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed - */ -int32_t PathFollowerStart() -{ - // Start main task - PathStatusGet(&pathStatus); - SettingsUpdatedCb(NULL); - PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo); - - return 0; -} - -/** - * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed - */ -int32_t PathFollowerInitialize() -{ - // initialize objects - FixedWingPathFollowerSettingsInitialize(); - FixedWingPathFollowerStatusInitialize(); - VtolPathFollowerSettingsInitialize(); - FlightStatusInitialize(); - FlightModeSettingsInitialize(); - PathStatusInitialize(); - PathSummaryInitialize(); - PathDesiredInitialize(); - PositionStateInitialize(); - VelocityStateInitialize(); - VelocityDesiredInitialize(); - StabilizationDesiredInitialize(); - AirspeedStateInitialize(); - AttitudeStateInitialize(); - TakeOffLocationInitialize(); - PoiLocationInitialize(); - ManualControlCommandInitialize(); - SystemSettingsInitialize(); - StabilizationBankInitialize(); - VtolSelfTuningStatsInitialize(); - - - // reset integrals - resetGlobals(); - - // Create object queue - pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES); - FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); - VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); - PathDesiredConnectCallback(SettingsUpdatedCb); - AirspeedStateConnectCallback(airspeedStateUpdatedCb); - - return 0; -} -MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart); - - -/** - * Module thread, should not return. - */ -static void pathFollowerTask(void) -{ - FlightStatusGet(&flightStatus); - - if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - resetGlobals(); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); - PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); - return; - } - - if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move into manualcontrol! - processPOI(); - } - - int16_t old_uid = pathStatus.UID; - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - if (pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) { - if (old_uid != pathStatus.UID) { - pathStatus.path_time = 0.0f; - } else { - pathStatus.path_time += updatePeriod / 1000.0f; - } - } - - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_BRAKE: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - { - uint8_t result = updateAutoPilotByFrameType(); - if (result) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - } else { - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); - } - } - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - break; - } - PathStatusSet(&pathStatus); - - - PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); -} - - -static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings); - - pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit); - pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit); - pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit); - - - VtolPathFollowerSettingsGet(&vtolPathFollowerSettings); - - pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); - pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); - pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit); - - pid_configure(&global.BrakePIDvel[0], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit); - pid_configure(&global.BrakePIDvel[1], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit); - - PathDesiredGet(&pathDesired); -} - - -static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - AirspeedStateData airspeedState; - VelocityStateData velocityState; - - AirspeedStateGet(&airspeedState); - VelocityStateGet(&velocityState); - float airspeedVector[2]; - float yaw; - AttitudeStateYawGet(&yaw); - airspeedVector[0] = cos_lookup_deg(yaw); - airspeedVector[1] = sin_lookup_deg(yaw); - // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement - float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; - - indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection; - // note - we do fly by Indicated Airspeed (== calibrated airspeed) however - // since airspeed is updated less often than groundspeed, we use sudden - // changes to groundspeed to offset the airspeed by the same measurement. - // This has a side effect that in the absence of any airspeed updates, the - // pathfollower will fly using groundspeed. -} - - -/** - * reset integrals - */ -static void resetGlobals() -{ - pid_zero(&global.PIDposH[0]); - pid_zero(&global.PIDposH[1]); - pid_zero(&global.PIDposV); - pid_zero(&global.PIDvel[0]); - pid_zero(&global.PIDvel[1]); - pid_zero(&global.PIDvel[2]); - pid_zero(&global.BrakePIDvel[0]); - pid_zero(&global.BrakePIDvel[1]); - pid_zero(&global.PIDcourse); - pid_zero(&global.PIDspeed); - pid_zero(&global.PIDpower); - global.poiRadius = 0.0f; - global.vtolEmergencyFallback = 0; - global.vtolEmergencyFallbackSwitch = false; - - // reset neutral thrust assessment. We restart this process - // and do once for each position hold engagement - neutralThrustEst.start_sampling = false; - neutralThrustEst.count = 0; - neutralThrustEst.sum = 0.0f; - neutralThrustEst.have_correction = false; - neutralThrustEst.average = 0.0f; - neutralThrustEst.correction = 0.0f; - neutralThrustEst.min = 0.0f; - neutralThrustEst.max = 0.0f; - - pathStatus.path_time = 0.0f; -} - -static uint8_t updateAutoPilotByFrameType() -{ - FrameType_t frameType = GetCurrentFrameType(); - - if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) { - switch (vtolPathFollowerSettings.TreatCustomCraftAs) { - case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: - frameType = FRAME_TYPE_FIXED_WING; - break; - case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: - frameType = FRAME_TYPE_MULTIROTOR; - break; - } - } - switch (frameType) { - case FRAME_TYPE_MULTIROTOR: - case FRAME_TYPE_HELI: - updatePeriod = vtolPathFollowerSettings.UpdatePeriod; - return updateAutoPilotVtol(); - - break; - case FRAME_TYPE_FIXED_WING: - default: - updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod; - return updateAutoPilotFixedWing(); - - break; - } -} - -/** - * fixed wing autopilot: - * straight forward: - * 1. update path velocity for limited motion crafts - * 2. update attitude according to default fixed wing pathfollower algorithm - */ -static uint8_t updateAutoPilotFixedWing() -{ - pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); - pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); - pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f); - updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true); - return updateFixedDesiredAttitude(); -} - -/** - * vtol autopilot - * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure) - * fall back to emergency fallback autopilot to keep minimum amount of flight control - */ -static uint8_t updateAutoPilotVtol() -{ - enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode; - enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode; - uint8_t result = 0; - - // decide on behaviour based on settings and system state - if (global.vtolEmergencyFallbackSwitch) { - returnmode = RETURN_0; - followermode = FOLLOWER_FALLBACK; - } else { - if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { - returnmode = RETURN_1; - followermode = FOLLOWER_FALLBACK; - } else { - returnmode = RETURN_RESULT; - followermode = FOLLOWER_REGULAR; - } - } - - // vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings - pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f); - - switch (followermode) { - case FOLLOWER_REGULAR: - { - // horizontal position control PID loop works according to settings in regular mode, allowing integral terms - pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); - pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false); - - // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm - bool yaw_attitude = true; - float yaw = 0.0f; - - if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { - yaw_attitude = false; - } else { - switch (vtolPathFollowerSettings.YawControl) { - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL: - yaw_attitude = false; - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: - yaw = updateTailInBearing(); - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION: - yaw = updateCourseBearing(); - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION: - yaw = updatePathBearing(); - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: - yaw = updatePOIBearing(); - break; - } - } - result = updateVtolDesiredAttitude(yaw_attitude, yaw); - - if (!result) { - if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { - plan_setup_assistedcontrol(true); // revert braking to position hold, user can always stick override - } else if (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) { - // switch to emergency follower if follower indicates problems - global.vtolEmergencyFallbackSwitch = true; - } - } - } - break; - case FOLLOWER_FALLBACK: - { - // fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly - pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f); - pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f); - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true); - - // emergency follower has no return value - updateVtolDesiredAttitudeEmergencyFallback(); - } - break; - } - - - // Brake mode end condition checks - if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { - bool exit_brake = false; - VelocityStateData velocityState; - if (pathStatus.path_time > pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]) { // enter hold on timeout - pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT; - exit_brake = true; - } else if (pathStatus.fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) { - VelocityStateGet(&velocityState); - if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) { - pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED; - exit_brake = true; - } - } - - if (exit_brake) { - // Calculate the distance error between the originally desired - // stopping point and the actual brake-exit point. - - PositionStateData p; - PositionStateGet(&p); - float north_offset = pathDesired.End.North - p.North; - float east_offset = pathDesired.End.East - p.East; - float down_offset = pathDesired.End.Down - p.Down; - pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset); - pathSummary.time_remaining = pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus.path_time; - pathSummary.fractional_progress = pathStatus.fractional_progress; - float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down; - cur_velocity = sqrtf(cur_velocity); - pathSummary.decelrate = (pathDesired.StartingVelocity - cur_velocity) / pathStatus.path_time; - pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings.BrakeRate; - pathSummary.velocityIntoHold = cur_velocity; - pathSummary.UID = pathStatus.UID; - PathSummarySet(&pathSummary); - - plan_setup_assistedcontrol(true); // braking timeout true - // having re-entered hold allow reassessment of neutral thrust - neutralThrustEst.have_correction = false; - } - } - - switch (returnmode) { - case RETURN_RESULT: - return result; - - default: - // returns either 0 or 1 according to enum definition above - return returnmode; - } -} - - -/** - * Compute bearing of current takeoff location - */ -static float updateTailInBearing() -{ - PositionStateData p; - - PositionStateGet(&p); - TakeOffLocationData t; - TakeOffLocationGet(&t); - // atan2f always returns in between + and - 180 degrees - return RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); -} - -/** - * Compute bearing of current movement direction - */ -static float updateCourseBearing() -{ - VelocityStateData v; - - VelocityStateGet(&v); - // atan2f always returns in between + and - 180 degrees - return RAD2DEG(atan2f(v.East, v.North)); -} - -/** - * Compute bearing of current path direction - */ -static float updatePathBearing() -{ - PositionStateData positionState; - - PositionStateGet(&positionState); - - float cur[3] = { positionState.North, - positionState.East, - positionState.Down }; - struct path_status progress; - - path_progress(&pathDesired, cur, &progress); - - // atan2f always returns in between + and - 180 degrees - return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0])); -} - -/** - * Compute bearing between current position and POI - */ -static float updatePOIBearing() -{ - PoiLocationData poi; - - PoiLocationGet(&poi); - PositionStateData positionState; - PositionStateGet(&positionState); - - const float dT = updatePeriod / 1000.0f; - float dLoc[3]; - float yaw = 0; - /*float elevation = 0;*/ - - dLoc[0] = positionState.North - poi.North; - dLoc[1] = positionState.East - poi.East; - dLoc[2] = positionState.Down - poi.Down; - - if (dLoc[1] < 0) { - yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; - } else { - yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; - } - ManualControlCommandData manualControlData; - ManualControlCommandGet(&manualControlData); - - float pathAngle = 0; - if (manualControlData.Roll > DEADBAND_HIGH) { - pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; - } else if (manualControlData.Roll < DEADBAND_LOW) { - pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; - } - - return yaw + (pathAngle / 2.0f); -} - -/** - * process POI control logic TODO: this should most likely go into manualcontrol!!!! - * TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently - **/ -static void processPOI() -{ - const float dT = updatePeriod / 1000.0f; - - - PositionStateData positionState; - - PositionStateGet(&positionState); - // TODO put commented out camera feature code back in place either - // permanently or optionally or remove it - // CameraDesiredData cameraDesired; - // CameraDesiredGet(&cameraDesired); - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - PoiLocationData poi; - PoiLocationGet(&poi); - - float dLoc[3]; - float yaw = 0; - // TODO camera feature - /*float elevation = 0;*/ - - dLoc[0] = positionState.North - poi.North; - dLoc[1] = positionState.East - poi.East; - dLoc[2] = positionState.Down - poi.Down; - - if (dLoc[1] < 0) { - yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; - } else { - yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; - } - - // distance - float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f)); - - ManualControlCommandData manualControlData; - ManualControlCommandGet(&manualControlData); - - float changeRadius = 0; - // Move closer or further, radially - if (manualControlData.Pitch > DEADBAND_HIGH) { - changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f; - } else if (manualControlData.Pitch < DEADBAND_LOW) { - changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f; - } - - // move along circular path - float pathAngle = 0; - if (manualControlData.Roll > DEADBAND_HIGH) { - pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; - } else if (manualControlData.Roll < DEADBAND_LOW) { - pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; - } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) { - // change radius only when not circling - global.poiRadius = distance + changeRadius; - } - - // don't try to move any closer - if (global.poiRadius >= 3.0f || changeRadius > 0) { - if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { - pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.End.East = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.StartingVelocity = 1.0f; - pathDesired.EndingVelocity = 0.0f; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - } - } - // not above - if (distance >= 3.0f) { - // TODO camera feature - // You can feed this into camerastabilization - /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ - - // cameraDesired.Yaw=yaw; - // cameraDesired.PitchOrServo2=elevation; - - // CameraDesiredSet(&cameraDesired); - } -} - -static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity) -{ - if (startingVelocity >= 0.0f) { - *updatedVelocity = startingVelocity - dT * brakeRate; - if (*updatedVelocity > currentVelocity) { - *updatedVelocity = currentVelocity; - } - if (*updatedVelocity < 0.0f) { - *updatedVelocity = 0.0f; - } - } else { - *updatedVelocity = startingVelocity + dT * brakeRate; - if (*updatedVelocity < currentVelocity) { - *updatedVelocity = currentVelocity; - } - if (*updatedVelocity > 0.0f) { - *updatedVelocity = 0.0f; - } - } -} - - -/** - * Compute desired velocity from the current position and path - */ -static void updatePathVelocity(float kFF, bool limited) -{ - PositionStateData positionState; - - PositionStateGet(&positionState); - VelocityStateData velocityState; - VelocityStateGet(&velocityState); - VelocityDesiredData velocityDesired; - - const float dT = updatePeriod / 1000.0f; - - if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { - float brakeRate = vtolPathFollowerSettings.BrakeRate; - if (brakeRate < BRAKE_RATE_MINIMUM) { - brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below - } - updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH], pathStatus.path_time, brakeRate, velocityState.North, &velocityDesired.North); - updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST], pathStatus.path_time, brakeRate, velocityState.East, &velocityDesired.East); - updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN], pathStatus.path_time, brakeRate, velocityState.Down, &velocityDesired.Down); - - float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down; - cur_velocity = sqrtf(cur_velocity); - float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down; - desired_velocity = sqrtf(desired_velocity); - - // update pathstatus - pathStatus.error = cur_velocity - desired_velocity; - pathStatus.fractional_progress = 1.0f; - if (pathDesired.StartingVelocity > 0.0f) { - pathStatus.fractional_progress = (pathDesired.StartingVelocity - cur_velocity) / pathDesired.StartingVelocity; - - // sometimes current velocity can exceed starting velocity due to initial acceleration - if (pathStatus.fractional_progress < 0.0f) { - pathStatus.fractional_progress = 0.0f; - } - } - pathStatus.path_direction_north = velocityDesired.North; - pathStatus.path_direction_east = velocityDesired.East; - pathStatus.path_direction_down = velocityDesired.Down; - - pathStatus.correction_direction_north = velocityDesired.North - velocityState.North; - pathStatus.correction_direction_east = velocityDesired.East - velocityState.East; - pathStatus.correction_direction_down = velocityDesired.Down - velocityState.Down; - } else { - // look ahead kFF seconds - float cur[3] = { positionState.North + (velocityState.North * kFF), - positionState.East + (velocityState.East * kFF), - positionState.Down + (velocityState.Down * kFF) }; - struct path_status progress; - path_progress(&pathDesired, cur, &progress); - - // calculate velocity - can be zero if waypoints are too close - velocityDesired.North = progress.path_vector[0]; - velocityDesired.East = progress.path_vector[1]; - velocityDesired.Down = progress.path_vector[2]; - - if (limited && - // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) - // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector - // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) - // leading to an S-shape snake course the wrong way - // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't - // turn steep unless there is enough space complete the turn before crossing the flightpath - // in this case the plane effectively needs to be turned around - // indicators: - // difference between correction_direction and velocitystate >90 degrees and - // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything ) - // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) - // calculating angles < 90 degrees through dot products - (vector_lengthf(progress.path_vector, 2) > 1e-6f) && - ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) && - ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) { - ; - } else { - // calculate correction - velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT); - velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT); - } - velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT); - - // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; - pathStatus.path_direction_north = progress.path_vector[0]; - pathStatus.path_direction_east = progress.path_vector[1]; - pathStatus.path_direction_down = progress.path_vector[2]; - - pathStatus.correction_direction_north = progress.correction_vector[0]; - pathStatus.correction_direction_east = progress.correction_vector[1]; - pathStatus.correction_direction_down = progress.correction_vector[2]; - } - - - VelocityDesiredSet(&velocityDesired); -} - - -/** - * Compute desired attitude from the desired velocity for fixed wing craft - */ -static uint8_t updateFixedDesiredAttitude() -{ - uint8_t result = 1; - - const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s] - - VelocityDesiredData velocityDesired; - VelocityStateData velocityState; - StabilizationDesiredData stabDesired; - AttitudeStateData attitudeState; - FixedWingPathFollowerStatusData fixedWingPathFollowerStatus; - AirspeedStateData airspeedState; - SystemSettingsData systemSettings; - - float groundspeedProjection; - float indicatedAirspeedState; - float indicatedAirspeedDesired; - float airspeedError; - - float pitchCommand; - - float descentspeedDesired; - float descentspeedError; - float powerCommand; - - float airspeedVector[2]; - float fluidMovement[2]; - float courseComponent[2]; - float courseError; - float courseCommand; - - FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus); - - VelocityStateGet(&velocityState); - StabilizationDesiredGet(&stabDesired); - VelocityDesiredGet(&velocityDesired); - AttitudeStateGet(&attitudeState); - AirspeedStateGet(&airspeedState); - SystemSettingsGet(&systemSettings); - - /** - * Compute speed error and course - */ - // missing sensors for airspeed-direction we have to assume within - // reasonable error that measured airspeed is actually the airspeed - // component in forward pointing direction - // airspeedVector is normalized - airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw); - airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw); - - // current ground speed projected in forward direction - groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; - - // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement, - // but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction - // than airspeed and gps sensors alone - indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias; - - // fluidMovement is a vector describing the aproximate movement vector of - // the surrounding fluid in 2d space (aka wind vector) - fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]); - fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]); - - // calculate the movement vector we need to fly to reach velocityDesired - - // taking fluidMovement into account - courseComponent[0] = velocityDesired.North - fluidMovement[0]; - courseComponent[1] = velocityDesired.East - fluidMovement[1]; - - indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]), - fixedWingPathFollowerSettings.HorizontalVelMin, - fixedWingPathFollowerSettings.HorizontalVelMax); - - // if we could fly at arbitrary speeds, we'd just have to move towards the - // courseComponent vector as previously calculated and we'd be fine - // unfortunately however we are bound by min and max air speed limits, so - // we need to recalculate the correct course to meet at least the - // velocityDesired vector direction at our current speed - // this overwrites courseComponent - bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired); - - // Error condition: wind speed too high, we can't go where we want anymore - fixedWingPathFollowerStatus.Errors.Wind = 0; - if ((!valid) && - fixedWingPathFollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on - fixedWingPathFollowerStatus.Errors.Wind = 1; - result = 0; - } - - // Airspeed error - airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; - - // Vertical speed error - descentspeedDesired = boundf( - velocityDesired.Down, - -fixedWingPathFollowerSettings.VerticalVelMax, - fixedWingPathFollowerSettings.VerticalVelMax); - descentspeedError = descentspeedDesired - velocityState.Down; - - // Error condition: plane too slow or too fast - fixedWingPathFollowerStatus.Errors.Highspeed = 0; - fixedWingPathFollowerStatus.Errors.Lowspeed = 0; - if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) { - fixedWingPathFollowerStatus.Errors.Overspeed = 1; - result = 0; - } - if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) { - fixedWingPathFollowerStatus.Errors.Highspeed = 1; - result = 0; - } - if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) { - fixedWingPathFollowerStatus.Errors.Lowspeed = 1; - result = 0; - } - if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) { - fixedWingPathFollowerStatus.Errors.Stallspeed = 1; - result = 0; - } - - /** - * Compute desired thrust command - */ - - // Compute the cross feed from vertical speed to pitch, with saturation - float speedErrorToPowerCommandComponent = boundf( - (airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp, - -fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max, - fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max - ); - - // Compute final thrust response - powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) + - speedErrorToPowerCommandComponent; - - // Output internal state to telemetry - fixedWingPathFollowerStatus.Error.Power = descentspeedError; - fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator; - fixedWingPathFollowerStatus.Command.Power = powerCommand; - - // set thrust - stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand, - fixedWingPathFollowerSettings.ThrustLimit.Min, - fixedWingPathFollowerSettings.ThrustLimit.Max); - - // Error condition: plane cannot hold altitude at current speed. - fixedWingPathFollowerStatus.Errors.Lowpower = 0; - if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedWingPathFollowerSettings.ThrustLimit.Max && // thrust at maximum - velocityState.Down > 0.0f && // we ARE going down - descentspeedDesired < 0.0f && // we WANT to go up - airspeedError > 0.0f && // we are too slow already - fixedWingPathFollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on - fixedWingPathFollowerStatus.Errors.Lowpower = 1; - result = 0; - } - // Error condition: plane keeps climbing despite minimum thrust (opposite of above) - fixedWingPathFollowerStatus.Errors.Highpower = 0; - if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedWingPathFollowerSettings.ThrustLimit.Min && // thrust at minimum - velocityState.Down < 0.0f && // we ARE going up - descentspeedDesired > 0.0f && // we WANT to go down - airspeedError < 0.0f && // we are too fast already - fixedWingPathFollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on - fixedWingPathFollowerStatus.Errors.Highpower = 1; - result = 0; - } - - /** - * Compute desired pitch command - */ - // Compute the cross feed from vertical speed to pitch, with saturation - float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp, - -fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max, - fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max - ); - - // Compute the pitch command as err*Kp + errInt*Ki + X_feed. - pitchCommand = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent; - - fixedWingPathFollowerStatus.Error.Speed = airspeedError; - fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator; - fixedWingPathFollowerStatus.Command.Speed = pitchCommand; - - stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand, - fixedWingPathFollowerSettings.PitchLimit.Min, - fixedWingPathFollowerSettings.PitchLimit.Max); - - // Error condition: high speed dive - fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0; - if (fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedWingPathFollowerSettings.PitchLimit.Max && // pitch demand is full up - velocityState.Down > 0.0f && // we ARE going down - descentspeedDesired < 0.0f && // we WANT to go up - airspeedError < 0.0f && // we are too fast already - fixedWingPathFollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on - fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1; - result = 0; - } - - /** - * Compute desired roll command - */ - courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; - - if (courseError < -180.0f) { - courseError += 360.0f; - } - if (courseError > 180.0f) { - courseError -= 360.0f; - } - - // overlap calculation. Theres a dead zone behind the craft where the - // counter-yawing of some craft while rolling could render a desired right - // turn into a desired left turn. Making the turn direction based on - // current roll angle keeps the plane committed to a direction once chosen - if (courseError < -180.0f + (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f) - && attitudeState.Roll > 0.0f) { - courseError += 360.0f; - } - if (courseError > 180.0f - (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f) - && attitudeState.Roll < 0.0f) { - courseError -= 360.0f; - } - - courseCommand = pid_apply(&global.PIDcourse, courseError, dT); - - fixedWingPathFollowerStatus.Error.Course = courseError; - fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator; - fixedWingPathFollowerStatus.Command.Course = courseCommand; - - stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral + - courseCommand, - fixedWingPathFollowerSettings.RollLimit.Min, - fixedWingPathFollowerSettings.RollLimit.Max); - - // TODO: find a check to determine loss of directional control. Likely needs some check of derivative - - - /** - * Compute desired yaw command - */ - // TODO implement raw control mode for yaw and base on Accels.Y - stabDesired.Yaw = 0.0f; - - - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - - StabilizationDesiredSet(&stabDesired); - - FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus); - - return result; -} - - -/** - * Function to calculate course vector C based on airspeed s, fluid movement F - * and desired movement vector V - * parameters in: V,F,s - * parameters out: C - * returns true if a valid solution could be found for V,F,s, false if not - * C will be set to a best effort attempt either way - */ -static bool correctCourse(float *C, float *V, float *F, float s) -{ - // Approach: - // Let Sc be a circle around origin marking possible movement vectors - // of the craft with airspeed s (all possible options for C) - // Let Vl be a line through the origin along movement vector V where fr any - // point v on line Vl v = k * (V / |V|) = k' * V - // Let Wl be a line parallel to Vl where for any point v on line Vl exists - // a point w on WL with w = v - F - // Then any intersection between circle Sc and line Wl represents course - // vector which would result in a movement vector - // V' = k * ( V / |V|) = k' * V - // If there is no intersection point, S is insufficient to compensate - // for F and we can only try to fly in direction of V (thus having wind drift - // but at least making progress orthogonal to wind) - - s = fabsf(s); - float f = vector_lengthf(F, 2); - - // normalize Cn=V/|V|, |V| must be >0 - float v = vector_lengthf(V, 2); - if (v < 1e-6f) { - // if |V|=0, we aren't supposed to move, turn into the wind - // (this allows hovering) - C[0] = -F[0]; - C[1] = -F[1]; - // if desired airspeed matches fluidmovement a hover is actually - // intended so return true - return fabsf(f - s) < 1e-3f; - } - float Vn[2] = { V[0] / v, V[1] / v }; - - // project F on V - float fp = F[0] * Vn[0] + F[1] * Vn[1]; - - // find component Fo of F that is orthogonal to V - // (which is exactly the distance between Vl and Wl) - float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) }; - float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1]; - - // find k where k * Vn = C - Fo - // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo - // so k^2 + fo^2 = s^2 (since |Vn|=1) - float k2 = s * s - fo2; - if (k2 <= -1e-3f) { - // there is no solution, we will be drifted off either way - // fallback: fly stupidly in direction of V and hope for the best - C[0] = V[0]; - C[1] = V[1]; - return false; - } else if (k2 <= 1e-3f) { - // there is exactly one solution: -Fo - C[0] = -Fo[0]; - C[1] = -Fo[1]; - return true; - } - // we have two possible solutions k positive and k negative as there are - // two intersection points between Wl and Sc - // which one is better? two criteria: - // 1. we MUST move in the right direction, if any k leads to -v its invalid - // 2. we should minimize the speed error - float k = sqrt(k2); - float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] }; - float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] }; - // project C+F on Vn to find signed resulting movement vector length - float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1]; - float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1]; - if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) { - // in this case the angle between course and resulting movement vector - // is greater than 90 degrees - so we actually fly backwards - C[0] = C1[0]; - C[1] = C1[1]; - return true; - } - C[0] = C2[0]; - C[1] = C2[1]; - if (vp2 >= 0.0f) { - // in this case the angle between course and movement vector is less than - // 90 degrees, but we do move in the right direction - return true; - } else { - // in this case we actually get driven in the opposite direction of V - // with both solutions for C - // this might be reached in headwind stronger than maximum allowed - // airspeed. - return false; - } -} - -/** - * Compute desired attitude from the desired velocity - * - * Takes in @ref NedState which has the acceleration in the - * NED frame as the feedback term and then compares the - * @ref VelocityState against the @ref VelocityDesired - */ -static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) -{ - const float dT = updatePeriod / 1000.0f; - uint8_t result = 1; - bool manual_thrust = false; - - VelocityDesiredData velocityDesired; - VelocityStateData velocityState; - StabilizationDesiredData stabDesired; - AttitudeStateData attitudeState; - StabilizationBankData stabSettings; - SystemSettingsData systemSettings; - VtolSelfTuningStatsData vtolSelfTuningStats; - - float northError; - float northCommand; - - float eastError; - float eastCommand; - - float downError; - float downCommand; - - SystemSettingsGet(&systemSettings); - VelocityStateGet(&velocityState); - VelocityDesiredGet(&velocityDesired); - StabilizationDesiredGet(&stabDesired); - VelocityDesiredGet(&velocityDesired); - AttitudeStateGet(&attitudeState); - StabilizationBankGet(&stabSettings); - VtolSelfTuningStatsGet(&vtolSelfTuningStats); - - - if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) { - // scale velocity if it is above configured maximum - // for braking, we can not help it if initial velocity was greater - float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); - if (velH > vtolPathFollowerSettings.HorizontalVelMax) { - velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH; - velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH; - } - if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) { - velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down); - } - } - - // calculate the velocity errors between desired and actual - northError = velocityDesired.North - velocityState.North; - eastError = velocityDesired.East - velocityState.East; - downError = velocityDesired.Down - velocityState.Down; - - // Must flip this sign - downError = -downError; - - // Compute desired commands - if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) { - northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward; - eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward; - } else { - northCommand = pid_apply(&global.BrakePIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.BrakeVelocityFeedforward; - eastCommand = pid_apply(&global.BrakePIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.BrakeVelocityFeedforward; - } - downCommand = pid_apply(&global.PIDvel[2], downError, dT); - - - if ((vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL && - flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) || - (flightStatus.FlightModeAssist && flightStatus.AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) { - manual_thrust = true; - } - - // if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment - // note that arming into this flight mode is not allowed, so assumption here is that - // we have not landed. If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the - // altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL. - // In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min, - // avoiding auto-takeoffs. Therefore no need to check that here. - if (!manual_thrust && neutralThrustEst.have_correction != true) { - // Assess if position hold state running. This can be normal position hold or - // another mode with assist-hold active. - bool ph_active = - ((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD && - flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) || - (flightStatus.FlightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE && - flightStatus.AssistedControlState == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD)); - - - bool stable = (fabsf(pathStatus.correction_direction_down) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT && - fabsf(velocityDesired.Down) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT && - fabsf(velocityState.Down) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT && - fabsf(downError) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT); - - if (ph_active && stable) { - if (neutralThrustEst.start_sampling) { - neutralThrustEst.count++; - - - // delay count for 2 seconds into hold allowing for stablisation - if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) { - neutralThrustEst.sum += global.PIDvel[2].iAccumulator; - if (global.PIDvel[2].iAccumulator < neutralThrustEst.min) { - neutralThrustEst.min = global.PIDvel[2].iAccumulator; - } - if (global.PIDvel[2].iAccumulator > neutralThrustEst.max) { - neutralThrustEst.max = global.PIDvel[2].iAccumulator; - } - } - - if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) { - // 6 seconds have past - // lets take an average - neutralThrustEst.average = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY); - neutralThrustEst.correction = neutralThrustEst.average / 1000.0f; - - global.PIDvel[2].iAccumulator -= neutralThrustEst.average; - - neutralThrustEst.start_sampling = false; - neutralThrustEst.have_correction = true; - - // Write a new adjustment value - // vtolSelfTuningStats.NeutralThrustOffset was incremental adjusted above - VtolSelfTuningStatsData new_vtolSelfTuningStats; - // add the average remaining i value to the - new_vtolSelfTuningStats.NeutralThrustOffset = vtolSelfTuningStats.NeutralThrustOffset + neutralThrustEst.correction; - new_vtolSelfTuningStats.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied - new_vtolSelfTuningStats.NeutralThrustAccumulator = global.PIDvel[2].iAccumulator; // the actual iaccumulator value after correction - new_vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min; - VtolSelfTuningStatsSet(&new_vtolSelfTuningStats); - } - } else { - // start a tick count - neutralThrustEst.start_sampling = true; - neutralThrustEst.count = 0; - neutralThrustEst.sum = 0.0f; - neutralThrustEst.max = 0.0f; - neutralThrustEst.min = 0.0f; - } - } else { - // reset sampling as we did't get 6 continuous seconds - neutralThrustEst.start_sampling = false; - } - } // else we already have a correction for this PH run - - // Generally in braking the downError will be an increased altitude. We really will rely on cruisecontrol to backoff. - stabDesired.Thrust = boundf(vtolSelfTuningStats.NeutralThrustOffset + downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); - - - // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in - if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) { - attitudeState.Yaw += 120.0f; - if (attitudeState.Yaw > 180.0f) { - attitudeState.Yaw -= 360.0f; - } - } - - - if ( // emergency flyaway detection - ( // integral already at its limit - vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f || - vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f - ) && - // angle between desired and actual velocity >90 degrees (by dot product) - (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) && - // quad is moving at significant speed (during flyaway it would keep speeding up) - squaref(velocityState.North) + squaref(velocityState.East) > 1.0f - ) { - global.vtolEmergencyFallback += dT; - if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) { - // after emergency timeout, trigger alarm - everything else is handled by callers - // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) - result = 0; - } - } else { - global.vtolEmergencyFallback = 0.0f; - } - - // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the - // craft should move similarly for 5 deg roll versus 5 deg pitch - - float maxPitch = vtolPathFollowerSettings.MaxRollPitch; - if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { - maxPitch = vtolPathFollowerSettings.BrakeMaxPitch; - } - - stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + - -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), - -maxPitch, maxPitch); - stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + - eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), - -maxPitch, maxPitch); - - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - - - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - if (yaw_attitude) { - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.Yaw = yaw_direction; - } else { - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; - } - - // default thrust mode to cruise control - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; - - // when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode. - if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) { - FlightModeSettingsData settings; - FlightModeSettingsGet(&settings); - FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL; - - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - thrustMode = settings.Stabilization1Settings.Thrust; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - thrustMode = settings.Stabilization2Settings.Thrust; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - thrustMode = settings.Stabilization3Settings.Thrust; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: - thrustMode = settings.Stabilization4Settings.Thrust; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: - thrustMode = settings.Stabilization5Settings.Thrust; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: - thrustMode = settings.Stabilization6Settings.Thrust; - break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - // we hard code the "GPS Assisted" PostionHold to use alt-vario which - // is a more appropriate throttle mode. "GPSAssist" adds braking - // and a better throttle management to the standard Position Hold. - thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO; - break; - } - stabDesired.StabilizationMode.Thrust = thrustMode; - stabDesired.Thrust = manualControl.Thrust; - } else if (manual_thrust) { - stabDesired.Thrust = manualControl.Thrust; - } - // else thrust is set by the PID controller - - StabilizationDesiredSet(&stabDesired); - - return result; -} - -/** - * Compute desired attitude for vtols - emergency fallback - */ -static void updateVtolDesiredAttitudeEmergencyFallback() -{ - const float dT = updatePeriod / 1000.0f; - - VelocityDesiredData velocityDesired; - VelocityStateData velocityState; - StabilizationDesiredData stabDesired; - - float courseError; - float courseCommand; - - float downError; - float downCommand; - - VelocityStateGet(&velocityState); - VelocityDesiredGet(&velocityDesired); - - ManualControlCommandData manualControlData; - ManualControlCommandGet(&manualControlData); - - courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North)); - - if (courseError < -180.0f) { - courseError += 360.0f; - } - if (courseError > 180.0f) { - courseError -= 360.0f; - } - - - courseCommand = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP); - - stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max); - - // Compute desired down command - downError = velocityDesired.Down - velocityState.Down; - // Must flip this sign - downError = -downError; - downCommand = pid_apply(&global.PIDvel[2], downError, dT); - - stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); - - - stabDesired.Roll = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll; - stabDesired.Pitch = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch; - - if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { - // For now override thrust with manual control. Disable at your risk, quad goes to China. - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - stabDesired.Thrust = manualControl.Thrust; - } - - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; - StabilizationDesiredSet(&stabDesired); -} - - -/** - * Compute desired attitude from a fixed preset - * - */ -static void updateFixedAttitude(float *attitude) -{ - StabilizationDesiredData stabDesired; - - StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; - stabDesired.Thrust = attitude[3]; - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - StabilizationDesiredSet(&stabDesired); -} diff --git a/flight/modules/PathFollower/pathfollower.cpp b/flight/modules/PathFollower/pathfollower.cpp new file mode 100644 index 000000000..1456fca6c --- /dev/null +++ b/flight/modules/PathFollower/pathfollower.cpp @@ -0,0 +1,462 @@ +/** + ****************************************************************************** + * + * @file pathfollower.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint + * and sets @ref AttitudeDesired. It only does this when the FlightMode field + * of @ref ManualControlCommand is Auto. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * Input object: PathDesired + * Input object: PositionState + * Input object: ManualControlCommand + * Output object: StabilizationDesired + * + * This module acts as "autopilot" - it controls the setpoints of stabilization + * based on current flight situation and desired flight path (PathDesired) as + * directed by flightmode selection or pathplanner + * This is a periodic delayed callback module + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +#include "pathfollowercontrol.h" +#include "vtollandcontroller.h" +#include "vtolvelocitycontroller.h" +#include "vtolbrakecontroller.h" +#include "vtolflycontroller.h" +#include "fixedwingflycontroller.h" +#include "grounddrivecontroller.h" + +// Private constants + +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + +#define PF_IDLE_UPDATE_RATE_MS 100 + +#define STACK_SIZE_BYTES 2048 + + +// Private types + +// Private variables +static DelayedCallbackInfo *pathFollowerCBInfo; +static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; +static FrameType_t frameType = FRAME_TYPE_MULTIROTOR; +static PathStatusData pathStatus; +static PathDesiredData pathDesired; +static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings; +static GroundPathFollowerSettingsData groundPathFollowerSettings; +static VtolPathFollowerSettingsData vtolPathFollowerSettings; +static FlightStatusData flightStatus; +static PathFollowerControl *activeController = 0; + +// Private functions +static void pathFollowerTask(void); +static void SettingsUpdatedCb(UAVObjEvent *ev); +static void airspeedStateUpdatedCb(UAVObjEvent *ev); +static void pathFollowerObjectiveUpdatedCb(UAVObjEvent *ev); +static void flightStatusUpdatedCb(UAVObjEvent *ev); +static void updateFixedAttitude(float *attitude); +static void pathFollowerInitializeControllersForFrameType(); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +extern "C" int32_t PathFollowerStart() +{ + // Start main task + PathStatusGet(&pathStatus); + SettingsUpdatedCb(NULL); + PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo); + + return 0; +} + + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +extern "C" int32_t PathFollowerInitialize() +{ + // initialize objects + GroundPathFollowerSettingsInitialize(); + FixedWingPathFollowerSettingsInitialize(); + FixedWingPathFollowerStatusInitialize(); + VtolPathFollowerSettingsInitialize(); + FlightStatusInitialize(); + FlightModeSettingsInitialize(); + PathStatusInitialize(); + PathSummaryInitialize(); + PathDesiredInitialize(); + PositionStateInitialize(); + VelocityStateInitialize(); + VelocityDesiredInitialize(); + StabilizationDesiredInitialize(); + AirspeedStateInitialize(); + AttitudeStateInitialize(); + TakeOffLocationInitialize(); + PoiLocationInitialize(); + ManualControlCommandInitialize(); + SystemSettingsInitialize(); + StabilizationBankInitialize(); + VtolSelfTuningStatsInitialize(); + PIDStatusInitialize(); + StatusVtolLandInitialize(); + StatusGroundDriveInitialize(); + + // VtolLandFSM additional objects + HomeLocationInitialize(); + AccelStateInitialize(); + + // Init references to controllers + PathFollowerControl::Initialize(&pathDesired, &flightStatus, &pathStatus); + + // Create object queue + pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES); + FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + GroundPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + PathDesiredConnectCallback(&pathFollowerObjectiveUpdatedCb); + FlightStatusConnectCallback(&flightStatusUpdatedCb); + SystemSettingsConnectCallback(&SettingsUpdatedCb); + AirspeedStateConnectCallback(&airspeedStateUpdatedCb); + VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb); + + return 0; +} +MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart); + +void pathFollowerInitializeControllersForFrameType() +{ + static uint8_t multirotor_initialised = 0; + static uint8_t fixedwing_initialised = 0; + static uint8_t ground_initialised = 0; + + switch (frameType) { + case FRAME_TYPE_MULTIROTOR: + case FRAME_TYPE_HELI: + if (!multirotor_initialised) { + VtolLandController::instance()->Initialize(&vtolPathFollowerSettings); + VtolVelocityController::instance()->Initialize(&vtolPathFollowerSettings); + VtolFlyController::instance()->Initialize(&vtolPathFollowerSettings); + VtolBrakeController::instance()->Initialize(&vtolPathFollowerSettings); + multirotor_initialised = 1; + } + break; + + case FRAME_TYPE_FIXED_WING: + if (!fixedwing_initialised) { + FixedWingFlyController::instance()->Initialize(&fixedWingPathFollowerSettings); + fixedwing_initialised = 1; + } + break; + + case FRAME_TYPE_GROUND: + if (!ground_initialised) { + GroundDriveController::instance()->Initialize(&groundPathFollowerSettings); + ground_initialised = 1; + } + break; + + default: + break; + } +} + +static void pathFollowerSetActiveController(void) +{ + // Init controllers for the frame type + if (activeController == 0) { + // Initialise + pathFollowerInitializeControllersForFrameType(); + + switch (frameType) { + case FRAME_TYPE_MULTIROTOR: + case FRAME_TYPE_HELI: + + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_BRAKE: // brake then hold sequence controller + activeController = VtolBrakeController::instance(); + activeController->Activate(); + break; + case PATHDESIRED_MODE_VELOCITY: // velocity roam controller + activeController = VtolVelocityController::instance(); + activeController->Activate(); + break; + case PATHDESIRED_MODE_GOTOENDPOINT: + case PATHDESIRED_MODE_FOLLOWVECTOR: + case PATHDESIRED_MODE_CIRCLERIGHT: + case PATHDESIRED_MODE_CIRCLELEFT: + activeController = VtolFlyController::instance(); + activeController->Activate(); + break; + case PATHDESIRED_MODE_LAND: // land with optional velocity roam option + activeController = VtolLandController::instance(); + activeController->Activate(); + break; + default: + activeController = 0; + break; + } + break; + + case FRAME_TYPE_FIXED_WING: + + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_GOTOENDPOINT: + case PATHDESIRED_MODE_FOLLOWVECTOR: + case PATHDESIRED_MODE_CIRCLERIGHT: + case PATHDESIRED_MODE_CIRCLELEFT: + activeController = FixedWingFlyController::instance(); + activeController->Activate(); + break; + default: + activeController = 0; + break; + } + break; + + case FRAME_TYPE_GROUND: + + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_GOTOENDPOINT: + case PATHDESIRED_MODE_FOLLOWVECTOR: + case PATHDESIRED_MODE_CIRCLERIGHT: + case PATHDESIRED_MODE_CIRCLELEFT: + activeController = GroundDriveController::instance(); + activeController->Activate(); + break; + default: + activeController = 0; + break; + } + break; + + default: + activeController = 0; + break; + } + } +} + +/** + * Module thread, should not return. + */ +static void pathFollowerTask(void) +{ + FlightStatusGet(&flightStatus); + + if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; + } + + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + + pathFollowerSetActiveController(); + + if (activeController) { + activeController->UpdateAutoPilot(); + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); + return; + } + + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + break; + default: + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + break; + } + PathStatusSet(&pathStatus); + + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); +} + + +static void pathFollowerObjectiveUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + PathDesiredGet(&pathDesired); + + if (activeController && pathDesired.Mode != activeController->Mode()) { + activeController->Deactivate(); + activeController = 0; + } + + pathFollowerSetActiveController(); + + if (activeController) { + activeController->ObjectiveUpdated(); + } +} + +// we use this to deactivate active controllers as the pathdesired +// objective never gets updated with switching to a non-pathfollower flight mode +static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + if (!activeController) { + return; + } + + FlightStatusControlChainData controlChain; + FlightStatusControlChainGet(&controlChain); + + if (controlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + activeController->Deactivate(); + activeController = 0; + } +} + + +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + VtolPathFollowerSettingsGet(&vtolPathFollowerSettings); + + FrameType_t previous_frameType = frameType; + frameType = GetCurrentFrameType(); + + if (frameType == FRAME_TYPE_CUSTOM) { + switch (vtolPathFollowerSettings.TreatCustomCraftAs) { + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: + frameType = FRAME_TYPE_FIXED_WING; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: + frameType = FRAME_TYPE_MULTIROTOR; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND: + frameType = FRAME_TYPE_GROUND; + break; + } + } + + // If frame type changes, initialise the frame type controllers + if (frameType != previous_frameType) { + pathFollowerInitializeControllersForFrameType(); + } + + switch (frameType) { + case FRAME_TYPE_MULTIROTOR: + case FRAME_TYPE_HELI: + updatePeriod = vtolPathFollowerSettings.UpdatePeriod; + break; + case FRAME_TYPE_FIXED_WING: + FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings); + updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod; + break; + case FRAME_TYPE_GROUND: + GroundPathFollowerSettingsGet(&groundPathFollowerSettings); + updatePeriod = groundPathFollowerSettings.UpdatePeriod; + break; + default: + updatePeriod = vtolPathFollowerSettings.UpdatePeriod; + break; + } + + if (activeController) { + activeController->SettingsUpdated(); + } +} + + +static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev); +} + + +/** + * Compute desired attitude from a fixed preset + * + */ +static void updateFixedAttitude(float *attitude) +{ + StabilizationDesiredData stabDesired; + + StabilizationDesiredGet(&stabDesired); + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; + stabDesired.Thrust = attitude[3]; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + StabilizationDesiredSet(&stabDesired); +} diff --git a/flight/modules/PathFollower/pathfollowercontrol.cpp b/flight/modules/PathFollower/pathfollowercontrol.cpp new file mode 100644 index 000000000..c9fe28cef --- /dev/null +++ b/flight/modules/PathFollower/pathfollowercontrol.cpp @@ -0,0 +1,52 @@ +/* + ****************************************************************************** + * + * @file PathFollowerControl.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Controller interface class implementation + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include +#include +#include +#include +} + +// C++ includes +#include "pathfollowercontrol.h" + +PathDesiredData *PathFollowerControl::pathDesired = 0; +FlightStatusData *PathFollowerControl::flightStatus = 0; +PathStatusData *PathFollowerControl::pathStatus = 0; + +int32_t PathFollowerControl::Initialize(PathDesiredData *ptr_pathDesired, + FlightStatusData *ptr_flightStatus, + PathStatusData *ptr_pathStatus) +{ + PIOS_Assert(ptr_pathDesired); + PIOS_Assert(ptr_flightStatus); + PIOS_Assert(ptr_pathStatus); + + pathDesired = ptr_pathDesired; + flightStatus = ptr_flightStatus; + pathStatus = ptr_pathStatus; + return 0; +} diff --git a/flight/modules/PathFollower/pidcontroldown.cpp b/flight/modules/PathFollower/pidcontroldown.cpp new file mode 100644 index 000000000..568db3d72 --- /dev/null +++ b/flight/modules/PathFollower/pidcontroldown.cpp @@ -0,0 +1,375 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower PID interface class + * @brief PID loop for down control + * @{ + * + * @file pidcontroldown.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes PID control for down direction + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include +#include +#include +} + +#include "pathfollowerfsm.h" +#include "pidcontroldown.h" + +#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f +#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT 0.2f +#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT 0.2f +#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT 0.1f + +#define NEUTRALTHRUST_START_DELAY (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate) +#define NEUTRALTHRUST_END_COUNT (NEUTRALTHRUST_START_DELAY + (4 * 20)) // 4 second sample + + +PIDControlDown::PIDControlDown() + : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f), + mFSM(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f), + mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false) +{ + Deactivate(); +} + +PIDControlDown::~PIDControlDown() {} + +void PIDControlDown::Initialize(PathFollowerFSM *fsm) +{ + mFSM = fsm; +} + +void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust) +{ + mMinThrust = min_thrust; + mMaxThrust = max_thrust; +} + +void PIDControlDown::Deactivate() +{ + // pid_zero(&PID); + mActive = false; +} + +void PIDControlDown::Activate() +{ + float currentThrust; + + StabilizationDesiredThrustGet(¤tThrust); + float u0 = currentThrust - mNeutral; + pid2_transfer(&PID, u0); + mActive = true; +} + +void PIDControlDown::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax) +{ + // pid_configure(&PID, kp, ki, kd, ilimit); + float Ti = kp / ki; + float Td = kd / kp; + float Tt = (Ti + Td) / 2.0f; + float kt = 1.0f / Tt; + float N = 10.0f; + float Tf = Td / N; + + if (ki < 1e-6f) { + // Avoid Ti being infinite + Ti = 1e6f; + // Tt antiwindup time constant - we don't need antiwindup with no I term + Tt = 1e6f; + kt = 0.0f; + } + + if (kd < 1e-6f) { + // PI Controller or P Controller + Tf = Ti / N; + } + + if (beta > 1.0f) { + beta = 1.0f; + } else if (beta < 0.4f) { + beta = 0.4f; + } + + pid2_configure(&PID, kp, ki, kd, Tf, kt, dT, beta, mNeutral, mNeutral, -1.0f); + deltaTime = dT; + mVelocityMax = velocityMax; +} + + +void PIDControlDown::UpdatePositionalParameters(float kp) +{ + pid_configure(&PIDpos, kp, 0.0f, 0.0f, 0.0f); +} +void PIDControlDown::UpdatePositionSetpoint(float setpointDown) +{ + mPositionSetpointTarget = setpointDown; +} +void PIDControlDown::UpdatePositionState(float pvDown) +{ + mPositionState = pvDown; + setup_neutralThrustCalc(); +} +// This is a pure position hold position control +void PIDControlDown::ControlPosition() +{ + // Current progress location relative to end + float velDown = 0.0f; + + velDown = pid_apply(&PIDpos, mPositionSetpointTarget - mPositionState, deltaTime); + UpdateVelocitySetpoint(velDown); + + run_neutralThrustCalc(); +} + + +void PIDControlDown::ControlPositionWithPath(struct path_status *progress) +{ + // Current progress location relative to end + float velDown = progress->path_vector[2]; + + velDown += pid_apply(&PIDpos, progress->correction_vector[2], deltaTime); + UpdateVelocitySetpoint(velDown); +} + + +void PIDControlDown::run_neutralThrustCalc(void) +{ + // if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment + // note that arming into this flight mode is not allowed, so assumption here is that + // we have not landed. If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the + // altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL. + // In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min, + // avoiding auto-takeoffs. Therefore no need to check that here. + + if (neutralThrustEst.have_correction != true) { + // Make FSM specific + bool stable = (fabsf(mPositionSetpointTarget - mPositionState) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT && + fabsf(mVelocitySetpointCurrent) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT && + fabsf(mVelocityState) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT && + fabsf(mVelocitySetpointCurrent - mVelocityState) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT); + + if (stable) { + if (neutralThrustEst.start_sampling) { + neutralThrustEst.count++; + + + // delay count for 2 seconds into hold allowing for stablisation + if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) { + neutralThrustEst.sum += PID.I; + if (PID.I < neutralThrustEst.min) { + neutralThrustEst.min = PID.I; + } + if (PID.I > neutralThrustEst.max) { + neutralThrustEst.max = PID.I; + } + } + + if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) { + // 6 seconds have past + // lets take an average + neutralThrustEst.average = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY); + neutralThrustEst.correction = neutralThrustEst.average; + + PID.I -= neutralThrustEst.average; + + neutralThrustEst.start_sampling = false; + neutralThrustEst.have_correction = true; + + // Write a new adjustment value + // vtolSelfTuningStats.NeutralThrustOffset was incremental adjusted above + VtolSelfTuningStatsData vtolSelfTuningStats; + VtolSelfTuningStatsGet(&vtolSelfTuningStats); + // add the average remaining i value to the + vtolSelfTuningStats.NeutralThrustOffset += neutralThrustEst.correction; + vtolSelfTuningStats.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied + vtolSelfTuningStats.NeutralThrustAccumulator = PID.I; // the actual iaccumulator value after correction + vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min; + VtolSelfTuningStatsSet(&vtolSelfTuningStats); + } + } else { + // start a tick count + neutralThrustEst.start_sampling = true; + neutralThrustEst.count = 0; + neutralThrustEst.sum = 0.0f; + neutralThrustEst.max = 0.0f; + neutralThrustEst.min = 0.0f; + } + } else { + // reset sampling as we did't get 6 continuous seconds + neutralThrustEst.start_sampling = false; + } + } // else we already have a correction for this PH run +} + + +void PIDControlDown::setup_neutralThrustCalc(void) +{ + // reset neutral thrust assessment. + // and do once for each position hold engagement + neutralThrustEst.start_sampling = false; + neutralThrustEst.count = 0; + neutralThrustEst.sum = 0.0f; + neutralThrustEst.have_correction = false; + neutralThrustEst.average = 0.0f; + neutralThrustEst.correction = 0.0f; + neutralThrustEst.min = 0.0f; + neutralThrustEst.max = 0.0f; +} + + +void PIDControlDown::UpdateNeutralThrust(float neutral) +{ + if (mActive) { + // adjust neutral and achieve bumpless transfer + PID.va = neutral; + pid2_transfer(&PID, mDownCommand); + } + mNeutral = neutral; +} + +void PIDControlDown::UpdateVelocitySetpoint(float setpoint) +{ + mVelocitySetpointTarget = setpoint; + if (fabsf(mVelocitySetpointTarget) > mVelocityMax) { + // maintain sign but set to max + mVelocitySetpointTarget *= mVelocityMax / fabsf(mVelocitySetpointTarget); + } +} + +void PIDControlDown::RateLimit(float *spDesired, float *spCurrent, float rateLimit) +{ + float velocity_delta = *spDesired - *spCurrent; + + if (fabsf(velocity_delta) < 1e-6f) { + *spCurrent = *spDesired; + return; + } + + // Calculate the rate of change + float accelerationDesired = velocity_delta / deltaTime; + + if (fabsf(accelerationDesired) > rateLimit) { + accelerationDesired *= rateLimit / accelerationDesired; + } + + if (fabsf(accelerationDesired) < 0.1f) { + *spCurrent = *spDesired; + } else { + *spCurrent += accelerationDesired * deltaTime; + } +} + +void PIDControlDown::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity) +{ + if (startingVelocity >= 0.0f) { + *updatedVelocity = startingVelocity - dT * brakeRate; + if (*updatedVelocity > currentVelocity) { + *updatedVelocity = currentVelocity; + } + if (*updatedVelocity < 0.0f) { + *updatedVelocity = 0.0f; + } + } else { + *updatedVelocity = startingVelocity + dT * brakeRate; + if (*updatedVelocity < currentVelocity) { + *updatedVelocity = currentVelocity; + } + if (*updatedVelocity > 0.0f) { + *updatedVelocity = 0.0f; + } + } +} + +void PIDControlDown::UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate) +{ + mVelocityState = pvDown; + + float velocitySetpointDesired; + + UpdateBrakeVelocity(mVelocitySetpointTarget, path_time, brakeRate, pvDown, &velocitySetpointDesired); + + // Calculate the rate of change + // RateLimit(velocitySetpointDesired[iaxis], mVelocitySetpointCurrent[iaxis], 2.0f ); + + mVelocitySetpointCurrent = velocitySetpointDesired; +} + + +// Update velocity state called per dT. Also update current +// desired velocity +void PIDControlDown::UpdateVelocityState(float pv) +{ + mVelocityState = pv; + + if (mFSM) { + // The FSM controls the actual descent velocity and introduces step changes as required + float velocitySetpointDesired = mFSM->BoundVelocityDown(mVelocitySetpointTarget); + // RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f ); + mVelocitySetpointCurrent = velocitySetpointDesired; + } else { + mVelocitySetpointCurrent = mVelocitySetpointTarget; + } +} + +float PIDControlDown::GetVelocityDesired(void) +{ + return mVelocitySetpointCurrent; +} + +float PIDControlDown::GetDownCommand(void) +{ + PIDStatusData pidStatus; + float ulow = mMinThrust; + float uhigh = mMaxThrust; + + if (mFSM) { + mFSM->BoundThrust(ulow, uhigh); + } + float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh); + pidStatus.setpoint = mVelocitySetpointCurrent; + pidStatus.actual = mVelocityState; + pidStatus.error = mVelocitySetpointCurrent - mVelocityState; + pidStatus.setpoint = mVelocitySetpointCurrent; + pidStatus.ulow = ulow; + pidStatus.uhigh = uhigh; + pidStatus.command = downCommand; + pidStatus.P = PID.P; + pidStatus.I = PID.I; + pidStatus.D = PID.D; + PIDStatusSet(&pidStatus); + mDownCommand = downCommand; + return mDownCommand; +} diff --git a/flight/modules/PathFollower/pidcontrolne.cpp b/flight/modules/PathFollower/pidcontrolne.cpp new file mode 100644 index 000000000..6cb871bea --- /dev/null +++ b/flight/modules/PathFollower/pidcontrolne.cpp @@ -0,0 +1,249 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PathFollower CONTROL interface class + * @brief PID controller for NE + * @{ + * + * @file PIDControlNE.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Executes PID control loops for NE directions + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include +} +#include "pathfollowerfsm.h" +#include "pidcontrolne.h" + +PIDControlNE::PIDControlNE() + : deltaTime(0), mNECommand(0), mNeutral(0), mVelocityMax(0), mMinCommand(0), mMaxCommand(0), mVelocityFeedforward(0), mActive(false) +{} + +PIDControlNE::~PIDControlNE() {} + +void PIDControlNE::Initialize() +{} + +void PIDControlNE::Deactivate() +{ + mActive = false; +} + +void PIDControlNE::Activate() +{ + // Do we need to initialise any loops for smooth transition + // float currentNE; + // StabilizationDesiredNEGet(¤tNE); + // float u0 = currentNE - mNeutral; + // pid2_transfer(&PID, u0); + mActive = true; +} + +void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax) +{ + // pid_configure(&PID, kp, ki, kd, ilimit); + float Ti = kp / ki; + float Td = kd / kp; + float Tt = (Ti + Td) / 2.0f; + float kt = 1.0f / Tt; + float u0 = 0.0f; + float N = 10.0f; + float Tf = Td / N; + + if (ki < 1e-6f) { + // Avoid Ti being infinite + Ti = 1e6f; + // Tt antiwindup time constant - we don't need antiwindup with no I term + Tt = 1e6f; + kt = 0.0f; + } + + if (kd < 1e-6f) { + // PI Controller + Tf = Ti / N; + } + + if (beta > 1.0f) { + beta = 1.0f; + } else if (beta < 0.4f) { + beta = 0.4f; + } + + pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f); + pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f); + deltaTime = dT; + mVelocityMax = velocityMax; +} + +void PIDControlNE::UpdatePositionalParameters(float kp) +{ + pid_configure(&PIDposH[0], kp, 0.0f, 0.0f, 0.0f); + pid_configure(&PIDposH[1], kp, 0.0f, 0.0f, 0.0f); +} +void PIDControlNE::UpdatePositionSetpoint(float setpointNorth, float setpointEast) +{ + mPositionSetpointTarget[0] = setpointNorth; + mPositionSetpointTarget[1] = setpointEast; +} +void PIDControlNE::UpdatePositionState(float pvNorth, float pvEast) +{ + mPositionState[0] = pvNorth; + mPositionState[1] = pvEast; +} +// This is a pure position hold position control +void PIDControlNE::ControlPosition() +{ + // Current progress location relative to end + float velNorth = 0.0f; + float velEast = 0.0f; + + velNorth = pid_apply(&PIDposH[0], mPositionSetpointTarget[0] - mPositionState[0], deltaTime); + velEast = pid_apply(&PIDposH[1], mPositionSetpointTarget[1] - mPositionState[1], deltaTime); + UpdateVelocitySetpoint(velNorth, velEast); +} + +void PIDControlNE::ControlPositionWithPath(struct path_status *progress) +{ + // Current progress location relative to end + float velNorth = progress->path_vector[0]; + float velEast = progress->path_vector[1]; + + velNorth += pid_apply(&PIDposH[0], progress->correction_vector[0], deltaTime); + velEast += pid_apply(&PIDposH[1], progress->correction_vector[1], deltaTime); + UpdateVelocitySetpoint(velNorth, velEast); +} + +void PIDControlNE::UpdateVelocitySetpoint(float setpointNorth, float setpointEast) +{ + // scale velocity if it is above configured maximum + // for braking, we can not help it if initial velocity was greater + float velH = sqrtf(setpointNorth * setpointNorth + setpointEast * setpointEast); + + if (velH > mVelocityMax) { + setpointNorth *= mVelocityMax / velH; + setpointEast *= mVelocityMax / velH; + } + + mVelocitySetpointTarget[0] = setpointNorth; + mVelocitySetpointTarget[1] = setpointEast; +} + + +void PIDControlNE::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity) +{ + if (startingVelocity >= 0.0f) { + *updatedVelocity = startingVelocity - dT * brakeRate; + if (*updatedVelocity > currentVelocity) { + *updatedVelocity = currentVelocity; + } + if (*updatedVelocity < 0.0f) { + *updatedVelocity = 0.0f; + } + } else { + *updatedVelocity = startingVelocity + dT * brakeRate; + if (*updatedVelocity < currentVelocity) { + *updatedVelocity = currentVelocity; + } + if (*updatedVelocity > 0.0f) { + *updatedVelocity = 0.0f; + } + } +} + +void PIDControlNE::UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate) +{ + mVelocityState[0] = pvNorth; + mVelocityState[1] = pvEast; + + float velocitySetpointDesired[2]; + + UpdateBrakeVelocity(mVelocitySetpointTarget[0], path_time, brakeRate, pvNorth, &velocitySetpointDesired[0]); + UpdateBrakeVelocity(mVelocitySetpointTarget[1], path_time, brakeRate, pvEast, &velocitySetpointDesired[1]); + + // If rate of change limits required, add here + for (int iaxis = 0; iaxis < 2; iaxis++) { + mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis]; + } +} + +void PIDControlNE::UpdateVelocityState(float pvNorth, float pvEast) +{ + mVelocityState[0] = pvNorth; + mVelocityState[1] = pvEast; + + // The FSM controls the actual descent velocity and introduces step changes as required + float velocitySetpointDesired[2]; + velocitySetpointDesired[0] = mVelocitySetpointTarget[0]; + velocitySetpointDesired[1] = mVelocitySetpointTarget[1]; + + // If rate of change limits required, add here + for (int iaxis = 0; iaxis < 2; iaxis++) { + mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis]; + } +} + + +void PIDControlNE::UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward) +{ + mMinCommand = minCommand; + mMaxCommand = maxCommand; + mVelocityFeedforward = velocityFeedforward; +} + + +void PIDControlNE::GetNECommand(float *northCommand, float *eastCommand) +{ + PIDvel[0].va = mVelocitySetpointCurrent[0] * mVelocityFeedforward; + *northCommand = pid2_apply(&(PIDvel[0]), mVelocitySetpointCurrent[0], mVelocityState[0], mMinCommand, mMaxCommand); + PIDvel[1].va = mVelocitySetpointCurrent[1] * mVelocityFeedforward; + *eastCommand = pid2_apply(&(PIDvel[1]), mVelocitySetpointCurrent[1], mVelocityState[1], mMinCommand, mMaxCommand); + + PIDStatusData pidStatus; + pidStatus.setpoint = mVelocitySetpointCurrent[0]; + pidStatus.actual = mVelocityState[0]; + pidStatus.error = mVelocitySetpointCurrent[0] - mVelocityState[0]; + pidStatus.setpoint = mVelocitySetpointCurrent[0]; + pidStatus.ulow = mMinCommand; + pidStatus.uhigh = mMaxCommand; + pidStatus.command = *northCommand; + pidStatus.P = PIDvel[0].P; + pidStatus.I = PIDvel[0].I; + pidStatus.D = PIDvel[0].D; + PIDStatusSet(&pidStatus); +} + +void PIDControlNE::GetVelocityDesired(float *north, float *east) +{ + *north = mVelocitySetpointCurrent[0]; + *east = mVelocitySetpointCurrent[1]; +} diff --git a/flight/modules/PathFollower/vtolbrakecontroller.cpp b/flight/modules/PathFollower/vtolbrakecontroller.cpp new file mode 100644 index 000000000..f985edc6f --- /dev/null +++ b/flight/modules/PathFollower/vtolbrakecontroller.cpp @@ -0,0 +1,355 @@ +/* + ****************************************************************************** + * + * @file vtolbrakecontroller.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief This landing state machine is a helper state machine to the + * pathfollower task/thread to implement detailed landing controls. + * This is to be called only from the pathfollower task. + * Note that initiation of the land occurs in the manual control + * command thread calling plans.c plan_setup_land which writes + * the required PathDesired LAND mode. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +// C++ includes +#include "vtolbrakecontroller.h" +#include "pathfollowerfsm.h" +#include "vtolbrakefsm.h" +#include "pidcontroldown.h" + +// Private constants +#define BRAKE_RATE_MINIMUM 0.2f + +// pointer to a singleton instance +VtolBrakeController *VtolBrakeController::p_inst = 0; + +VtolBrakeController::VtolBrakeController() + : fsm(0), vtolPathFollowerSettings(0), mActive(false), mHoldActive(false), mManualThrust(false) +{} + +// Called when mode first engaged +void VtolBrakeController::Activate(void) +{ + if (!mActive) { + mActive = true; + mHoldActive = false; + mManualThrust = false; + SettingsUpdated(); + fsm->Activate(); + controlDown.Activate(); + controlNE.Activate(); + } +} + +uint8_t VtolBrakeController::IsActive(void) +{ + return mActive; +} + +uint8_t VtolBrakeController::Mode(void) +{ + return PATHDESIRED_MODE_BRAKE; +} + +// Objective updated in pathdesired +void VtolBrakeController::ObjectiveUpdated(void) +{ + // Set the objective's target velocity + controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN]); + controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH], + pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST]); + if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) { + pathStatus->path_time = 0.0f; + } +} +void VtolBrakeController::Deactivate(void) +{ + if (mActive) { + mActive = false; + mHoldActive = false; + mManualThrust = false; + fsm->Inactive(); + controlDown.Deactivate(); + controlNE.Deactivate(); + } +} + + +void VtolBrakeController::SettingsUpdated(void) +{ + const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f; + + controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp, + vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki, + vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd, + vtolPathFollowerSettings->BrakeHorizontalVelPID.ILimit, + dT, + 10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake + controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); + controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward); + + controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp, + vtolPathFollowerSettings->LandVerticalVelPID.Ki, + vtolPathFollowerSettings->LandVerticalVelPID.Kd, + vtolPathFollowerSettings->LandVerticalVelPID.Beta, + dT, + 10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake + controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP); + controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max); + + VtolSelfTuningStatsData vtolSelfTuningStats; + VtolSelfTuningStatsGet(&vtolSelfTuningStats); + controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral); + fsm->SettingsUpdated(); +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t VtolBrakeController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings) +{ + PIOS_Assert(ptr_vtolPathFollowerSettings); + vtolPathFollowerSettings = ptr_vtolPathFollowerSettings; + if (fsm == 0) { + VtolBrakeFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus, pathStatus); + fsm = (PathFollowerFSM *)VtolBrakeFSM::instance(); + controlDown.Initialize(fsm); + } + return 0; +} + + +void VtolBrakeController::UpdateVelocityDesired() +{ + VelocityStateData velocityState; + + VelocityStateGet(&velocityState); + VelocityDesiredData velocityDesired; + + float brakeRate = vtolPathFollowerSettings->BrakeRate; + if (brakeRate < BRAKE_RATE_MINIMUM) { + brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below + } + + if (fsm->PositionHoldState()) { + PositionStateData positionState; + PositionStateGet(&positionState); + + // On first engagement set the position setpoint + if (!mHoldActive) { + mHoldActive = true; + controlNE.UpdatePositionSetpoint(positionState.North, positionState.East); + if (!mManualThrust) { + controlDown.UpdatePositionSetpoint(positionState.Down); + } + } + + // Update position state and control position to create inputs to velocity control + controlNE.UpdatePositionState(positionState.North, positionState.East); + controlNE.ControlPosition(); + if (!mManualThrust) { + controlDown.UpdatePositionState(positionState.Down); + controlDown.ControlPosition(); + } + + controlNE.UpdateVelocityState(velocityState.North, velocityState.East); + if (!mManualThrust) { + controlDown.UpdateVelocityState(velocityState.Down); + } + } else { + controlNE.UpdateVelocityStateWithBrake(velocityState.North, velocityState.East, pathStatus->path_time, brakeRate); + if (!mManualThrust) { + controlDown.UpdateVelocityStateWithBrake(velocityState.Down, pathStatus->path_time, brakeRate); + } + } + + if (!mManualThrust) { + velocityDesired.Down = controlDown.GetVelocityDesired(); + } else { velocityDesired.Down = 0.0f; } + + float north, east; + controlNE.GetVelocityDesired(&north, &east); + velocityDesired.North = north; + velocityDesired.East = east; + + VelocityDesiredSet(&velocityDesired); + + // update pathstatus + float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down; + cur_velocity = sqrtf(cur_velocity); + float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down; + desired_velocity = sqrtf(desired_velocity); + pathStatus->error = cur_velocity - desired_velocity; + pathStatus->fractional_progress = 1.0f; + if (pathDesired->StartingVelocity > 0.0f) { + pathStatus->fractional_progress = (pathDesired->StartingVelocity - cur_velocity) / pathDesired->StartingVelocity; + + // sometimes current velocity can exceed starting velocity due to initial acceleration + if (pathStatus->fractional_progress < 0.0f) { + pathStatus->fractional_progress = 0.0f; + } + } + pathStatus->path_direction_north = velocityDesired.North; + pathStatus->path_direction_east = velocityDesired.East; + pathStatus->path_direction_down = velocityDesired.Down; + + pathStatus->correction_direction_north = velocityDesired.North - velocityState.North; + pathStatus->correction_direction_east = velocityDesired.East - velocityState.East; +} + +int8_t VtolBrakeController::UpdateStabilizationDesired(void) +{ + uint8_t result = 1; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + StabilizationBankData stabSettings; + float northCommand; + float eastCommand; + + StabilizationDesiredGet(&stabDesired); + AttitudeStateGet(&attitudeState); + StabilizationBankGet(&stabSettings); + + controlNE.GetNECommand(&northCommand, &eastCommand); + + float angle_radians = DEG2RAD(attitudeState.Yaw); + float cos_angle = cosf(angle_radians); + float sine_angle = sinf(angle_radians); + float maxPitch = vtolPathFollowerSettings->BrakeMaxPitch; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch); + + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; + + // default thrust mode to cruise control + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + + // when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode. + if (flightStatus->FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) { + FlightModeSettingsData settings; + FlightModeSettingsGet(&settings); + uint8_t thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL; + + switch (flightStatus->FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + thrustMode = settings.Stabilization1Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + thrustMode = settings.Stabilization2Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + thrustMode = settings.Stabilization3Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: + thrustMode = settings.Stabilization4Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: + thrustMode = settings.Stabilization5Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: + thrustMode = settings.Stabilization6Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which + // is a more appropriate throttle mode. "GPSAssist" adds braking + // and a better throttle management to the standard Position Hold. + thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO; + break; + } + stabDesired.StabilizationMode.Thrust = thrustMode; + } + + // set the thrust value + if (mManualThrust) { + stabDesired.Thrust = manualControl.Thrust; + } else { + stabDesired.Thrust = controlDown.GetDownCommand(); + } + + StabilizationDesiredSet(&stabDesired); + + return result; +} + + +void VtolBrakeController::UpdateAutoPilot() +{ + if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) { + pathStatus->path_time += vtolPathFollowerSettings->UpdatePeriod / 1000.0f; + } + + if (flightStatus->FlightModeAssist && flightStatus->AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) { + mManualThrust = true; + } else { + mManualThrust = false; + } + + fsm->Update(); // This will run above end condition checks + + UpdateVelocityDesired(); + + int8_t result = UpdateStabilizationDesired(); + + if (!result) { + fsm->Abort(); // enter PH + } + + PathStatusSet(pathStatus); +} diff --git a/flight/modules/PathFollower/vtolbrakefsm.cpp b/flight/modules/PathFollower/vtolbrakefsm.cpp new file mode 100644 index 000000000..e9e47f889 --- /dev/null +++ b/flight/modules/PathFollower/vtolbrakefsm.cpp @@ -0,0 +1,261 @@ +/* + ****************************************************************************** + * + * @file vtolbrakefsm.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Vtol brake finate state machine to regulate behaviour of the + * brake controller. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +// C++ includes +#include + + +// Private constants +#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod) +#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f +#define BRAKE_EXIT_VELOCITY_LIMIT 0.2f + +VtolBrakeFSM::PathFollowerFSM_BrakeStateHandler_T VtolBrakeFSM::sBrakeStateTable[BRAKE_STATE_SIZE] = { + [BRAKE_STATE_INACTIVE] = { .setup = 0, .run = 0 }, + [BRAKE_STATE_BRAKE] = { .setup = &VtolBrakeFSM::setup_brake, .run = &VtolBrakeFSM::run_brake }, + [BRAKE_STATE_HOLD] = { .setup = 0, .run = 0 } +}; + +// pointer to a singleton instance +VtolBrakeFSM *VtolBrakeFSM::p_inst = 0; + + +VtolBrakeFSM::VtolBrakeFSM() + : mBrakeData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0) +{} + + +// Private types + +// Private functions +// Public API methods +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t VtolBrakeFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings, + PathDesiredData *ptr_pathDesired, + FlightStatusData *ptr_flightStatus, + PathStatusData *ptr_pathStatus) +{ + PIOS_Assert(ptr_vtolPathFollowerSettings); + PIOS_Assert(ptr_pathDesired); + PIOS_Assert(ptr_flightStatus); + + // allow for Initialize being called more than once. + if (!mBrakeData) { + mBrakeData = (VtolBrakeFSMData_T *)pios_malloc(sizeof(VtolBrakeFSMData_T)); + PIOS_Assert(mBrakeData); + } + memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0); + vtolPathFollowerSettings = ptr_vtolPathFollowerSettings; + pathDesired = ptr_pathDesired; + flightStatus = ptr_flightStatus; + pathStatus = ptr_pathStatus; + initFSM(); + + return 0; +} + +void VtolBrakeFSM::Inactive(void) +{ + memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0); + initFSM(); +} + +// Initialise the FSM +void VtolBrakeFSM::initFSM(void) +{ + mBrakeData->currentState = BRAKE_STATE_INACTIVE; +} + +void VtolBrakeFSM::Activate() +{ + memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0); + mBrakeData->currentState = BRAKE_STATE_INACTIVE; + setState(BRAKE_STATE_BRAKE, FSMBRAKESTATUS_STATEEXITREASON_NONE); +} + +void VtolBrakeFSM::Abort(void) +{ + setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE); +} + +PathFollowerFSMState_T VtolBrakeFSM::GetCurrentState(void) +{ + switch (mBrakeData->currentState) { + case BRAKE_STATE_INACTIVE: + return PFFSM_STATE_INACTIVE; + + break; + default: + return PFFSM_STATE_ACTIVE; + + break; + } +} + +void VtolBrakeFSM::Update() +{ + runState(); +} + +int32_t VtolBrakeFSM::runState(void) +{ + uint8_t flTimeout = false; + + mBrakeData->stateRunCount++; + + if (mBrakeData->stateTimeoutCount > 0 && mBrakeData->stateRunCount > mBrakeData->stateTimeoutCount) { + flTimeout = true; + } + + // If the current state has a static function, call it + if (sBrakeStateTable[mBrakeData->currentState].run) { + (this->*sBrakeStateTable[mBrakeData->currentState].run)(flTimeout); + } + return 0; +} + +// Set the new state and perform setup for subsequent state run calls +// This is called by state run functions on event detection that drive +// state transitions. +void VtolBrakeFSM::setState(PathFollowerFSM_BrakeState_T newState, __attribute__((unused)) VtolBrakeFSMStatusStateExitReasonOptions reason) +{ + // mBrakeData->fsmBrakeStatus.StateExitReason[mBrakeData->currentState] = reason; + + if (mBrakeData->currentState == newState) { + return; + } + mBrakeData->currentState = newState; + + // Restart state timer counter + mBrakeData->stateRunCount = 0; + + // Reset state timeout to disabled/zero + mBrakeData->stateTimeoutCount = 0; + + if (sBrakeStateTable[mBrakeData->currentState].setup) { + (this->*sBrakeStateTable[mBrakeData->currentState].setup)(); + } +} + + +// Timeout utility function for use by state init implementations +void VtolBrakeFSM::setStateTimeout(int32_t count) +{ + mBrakeData->stateTimeoutCount = count; +} + +// FSM Setup and Run method implementation + +// State: WAITING FOR DESCENT RATE +void VtolBrakeFSM::setup_brake(void) +{ + setStateTimeout(TIMER_COUNT_PER_SECOND * pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]); + mBrakeData->observationCount = 0; + mBrakeData->observation2Count = 0; +} + + +void VtolBrakeFSM::run_brake(uint8_t flTimeout) +{ + // Brake mode end condition checks + bool exit_brake = false; + VelocityStateData velocityState; + PathSummaryData pathSummary; + + if (flTimeout) { + pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT; + exit_brake = true; + } else if (pathStatus->fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) { + VelocityStateGet(&velocityState); + if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) { + pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED; + exit_brake = true; + } + } + + if (exit_brake) { + // Calculate the distance error between the originally desired + // stopping point and the actual brake-exit point. + + PositionStateData p; + PositionStateGet(&p); + float north_offset = pathDesired->End.North - p.North; + float east_offset = pathDesired->End.East - p.East; + float down_offset = pathDesired->End.Down - p.Down; + pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset); + pathSummary.time_remaining = pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus->path_time; + pathSummary.fractional_progress = pathStatus->fractional_progress; + float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down; + cur_velocity = sqrtf(cur_velocity); + pathSummary.decelrate = (pathDesired->StartingVelocity - cur_velocity) / pathStatus->path_time; + pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings->BrakeRate; + pathSummary.velocityIntoHold = cur_velocity; + pathSummary.Mode = PATHSUMMARY_MODE_BRAKE; + pathSummary.UID = pathStatus->UID; + PathSummarySet(&pathSummary); + + setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE); + } +} + +uint8_t VtolBrakeFSM::PositionHoldState(void) +{ + return mBrakeData->currentState == BRAKE_STATE_HOLD; +} diff --git a/flight/modules/PathFollower/vtolflycontroller.cpp b/flight/modules/PathFollower/vtolflycontroller.cpp new file mode 100644 index 000000000..e76c09d54 --- /dev/null +++ b/flight/modules/PathFollower/vtolflycontroller.cpp @@ -0,0 +1,547 @@ +/* + ****************************************************************************** + * + * @file vtolflycontroller.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Class implements the fly controller for vtols + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +// C++ includes +#include "vtolflycontroller.h" +#include "pathfollowerfsm.h" +#include "pidcontroldown.h" +#include "pidcontrolne.h" + +// Private constants +#define DEADBAND_HIGH 0.10f +#define DEADBAND_LOW -0.10f +#define RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS 0.95f +#define RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE 2.0f + +// pointer to a singleton instance +VtolFlyController *VtolFlyController::p_inst = 0; + +VtolFlyController::VtolFlyController() + : vtolPathFollowerSettings(NULL), mActive(false), mManualThrust(false), mMode(0), vtolEmergencyFallback(0.0f), vtolEmergencyFallbackSwitch(false) +{} + +// Called when mode first engaged +void VtolFlyController::Activate(void) +{ + if (!mActive) { + mActive = true; + mManualThrust = false; + SettingsUpdated(); + controlDown.Activate(); + controlNE.Activate(); + mMode = pathDesired->Mode; + + vtolEmergencyFallback = 0.0f; + vtolEmergencyFallbackSwitch = false; + } +} + +uint8_t VtolFlyController::IsActive(void) +{ + return mActive; +} + +uint8_t VtolFlyController::Mode(void) +{ + return mMode; +} + +// Objective updated in pathdesired +void VtolFlyController::ObjectiveUpdated(void) +{} + + +void VtolFlyController::Deactivate(void) +{ + if (mActive) { + mActive = false; + mManualThrust = false; + controlDown.Deactivate(); + controlNE.Deactivate(); + vtolEmergencyFallback = 0.0f; + vtolEmergencyFallbackSwitch = false; + } +} + + +void VtolFlyController::SettingsUpdated(void) +{ + const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f; + + controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, + vtolPathFollowerSettings->HorizontalVelPID.Ki, + vtolPathFollowerSettings->HorizontalVelPID.Kd, + vtolPathFollowerSettings->HorizontalVelPID.ILimit, + dT, + vtolPathFollowerSettings->HorizontalVelMax); + controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); + controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward); + + controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp, + vtolPathFollowerSettings->VerticalVelPID.Ki, + vtolPathFollowerSettings->VerticalVelPID.Kd, + vtolPathFollowerSettings->VerticalVelPID.ILimit, // TODO Change to BETA + dT, + vtolPathFollowerSettings->VerticalVelMax); + controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP); + + VtolSelfTuningStatsData vtolSelfTuningStats; + VtolSelfTuningStatsGet(&vtolSelfTuningStats); + controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral); + controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max); +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t VtolFlyController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings) +{ + PIOS_Assert(ptr_vtolPathFollowerSettings); + + vtolPathFollowerSettings = ptr_vtolPathFollowerSettings; + + return 0; +} + + +/** + * Compute desired velocity from the current position and path + */ +void VtolFlyController::UpdateVelocityDesired() +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + controlNE.UpdateVelocityState(velocityState.North, velocityState.East); + controlDown.UpdateVelocityState(velocityState.Down); + + VelocityDesiredData velocityDesired; + + // look ahead kFF seconds + float cur[3] = { positionState.North + (velocityState.North * vtolPathFollowerSettings->CourseFeedForward), + positionState.East + (velocityState.East * vtolPathFollowerSettings->CourseFeedForward), + positionState.Down + (velocityState.Down * vtolPathFollowerSettings->CourseFeedForward) }; + struct path_status progress; + path_progress(pathDesired, cur, &progress, true); + + controlNE.ControlPositionWithPath(&progress); + if (!mManualThrust) { + controlDown.ControlPositionWithPath(&progress); + } + + float north, east; + controlNE.GetVelocityDesired(&north, &east); + velocityDesired.North = north; + velocityDesired.East = east; + if (!mManualThrust) { + velocityDesired.Down = controlDown.GetVelocityDesired(); + } else { velocityDesired.Down = 0.0f; } + + // update pathstatus + pathStatus->error = progress.error; + pathStatus->fractional_progress = progress.fractional_progress; + pathStatus->path_direction_north = progress.path_vector[0]; + pathStatus->path_direction_east = progress.path_vector[1]; + pathStatus->path_direction_down = progress.path_vector[2]; + + pathStatus->correction_direction_north = progress.correction_vector[0]; + pathStatus->correction_direction_east = progress.correction_vector[1]; + pathStatus->correction_direction_down = progress.correction_vector[2]; + + VelocityDesiredSet(&velocityDesired); +} + + +int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction) +{ + uint8_t result = 1; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + StabilizationBankData stabSettings; + float northCommand; + float eastCommand; + + StabilizationDesiredGet(&stabDesired); + AttitudeStateGet(&attitudeState); + StabilizationBankGet(&stabSettings); + + controlNE.GetNECommand(&northCommand, &eastCommand); + + float angle_radians = DEG2RAD(attitudeState.Yaw); + float cos_angle = cosf(angle_radians); + float sine_angle = sinf(angle_radians); + float maxPitch = vtolPathFollowerSettings->MaxRollPitch; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch); + + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + + // TODO The below need to be rewritten because the PID implementation has changed. +#if 0 + // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in + if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) { + attitudeState.Yaw += 120.0f; + if (attitudeState.Yaw > 180.0f) { + attitudeState.Yaw -= 360.0f; + } + } + + + if ( // emergency flyaway detection + ( // integral already at its limit + vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f || + vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f + ) && + // angle between desired and actual velocity >90 degrees (by dot product) + (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) && + // quad is moving at significant speed (during flyaway it would keep speeding up) + squaref(velocityState.North) + squaref(velocityState.East) > 1.0f + ) { + vtolEmergencyFallback += dT; + if (vtolEmergencyFallback >= vtolPathFollowerSettings->FlyawayEmergencyFallbackTriggerTime) { + // after emergency timeout, trigger alarm - everything else is handled by callers + // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) + result = 0; + } + } else { + vtolEmergencyFallback = 0.0f; + } +#endif // if 0 + + if (yaw_attitude) { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Yaw = yaw_direction; + } else { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; + } + + // default thrust mode to cruise control + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + + if (mManualThrust) { + stabDesired.Thrust = manualControl.Thrust; + } else { + stabDesired.Thrust = controlDown.GetDownCommand(); + } + + StabilizationDesiredSet(&stabDesired); + + return result; +} + +/** + * Compute desired attitude for vtols - emergency fallback + */ +void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback() +{ + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + + float courseError; + float courseCommand; + + VelocityStateGet(&velocityState); + VelocityDesiredGet(&velocityDesired); + + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North)); + + if (courseError < -180.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f) { + courseError -= 360.0f; + } + + + courseCommand = (courseError * vtolPathFollowerSettings->EmergencyFallbackYawRate.kP); + stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings->EmergencyFallbackYawRate.Max, vtolPathFollowerSettings->EmergencyFallbackYawRate.Max); + + controlDown.UpdateVelocitySetpoint(velocityDesired.Down); + controlDown.UpdateVelocityState(velocityState.Down); + stabDesired.Thrust = controlDown.GetDownCommand(); + + + stabDesired.Roll = vtolPathFollowerSettings->EmergencyFallbackAttitude.Roll; + stabDesired.Pitch = vtolPathFollowerSettings->EmergencyFallbackAttitude.Pitch; + + if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { + stabDesired.Thrust = manualControlData.Thrust; + } + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + StabilizationDesiredSet(&stabDesired); +} + + +void VtolFlyController::UpdateAutoPilot() +{ + if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { + mManualThrust = true; + } + + uint8_t result = RunAutoPilot(); + + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + + PathStatusSet(pathStatus); + + // If rtbl, detect arrival at the endpoint and then triggers a change + // to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c + // can't manage this. And pathplanner whilst similar does not manage this as it is not a + // waypoint traversal and is not aware of flight modes other than path plan. + if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) { + if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) { + if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) { + plan_setup_land(); + } + } + } +} +/** + * vtol autopilot + * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure) + * fall back to emergency fallback autopilot to keep minimum amount of flight control + */ +uint8_t VtolFlyController::RunAutoPilot() +{ + enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode; + enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode; + uint8_t result = 0; + + // decide on behaviour based on settings and system state + if (vtolEmergencyFallbackSwitch) { + returnmode = RETURN_0; + followermode = FOLLOWER_FALLBACK; + } else { + if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { + returnmode = RETURN_1; + followermode = FOLLOWER_FALLBACK; + } else { + returnmode = RETURN_RESULT; + followermode = FOLLOWER_REGULAR; + } + } + + switch (followermode) { + case FOLLOWER_REGULAR: + { + // horizontal position control PID loop works according to settings in regular mode, allowing integral terms + UpdateVelocityDesired(); + + // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm + bool yaw_attitude = true; + float yaw = 0.0f; + + switch (vtolPathFollowerSettings->YawControl) { + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL: + yaw_attitude = false; + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: + yaw = updateTailInBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION: + yaw = updateCourseBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION: + yaw = updatePathBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: + yaw = updatePOIBearing(); + break; + } + + result = UpdateStabilizationDesired(yaw_attitude, yaw); + + if (!result) { + if (vtolPathFollowerSettings->FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) { + // switch to emergency follower if follower indicates problems + vtolEmergencyFallbackSwitch = true; + } + } + } + break; + case FOLLOWER_FALLBACK: + { + // fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly + controlNE.UpdatePositionalParameters(1.0f); + UpdateVelocityDesired(); + + // emergency follower has no return value + UpdateDesiredAttitudeEmergencyFallback(); + } + break; + } + + switch (returnmode) { + case RETURN_RESULT: + return result; + + default: + // returns either 0 or 1 according to enum definition above + return returnmode; + } +} + + +/** + * Compute bearing of current takeoff location + */ +float VtolFlyController::updateTailInBearing() +{ + PositionStateData p; + + PositionStateGet(&p); + TakeOffLocationData t; + TakeOffLocationGet(&t); + // atan2f always returns in between + and - 180 degrees + return RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); +} + + +/** + * Compute bearing of current movement direction + */ +float VtolFlyController::updateCourseBearing() +{ + VelocityStateData v; + + VelocityStateGet(&v); + // atan2f always returns in between + and - 180 degrees + return RAD2DEG(atan2f(v.East, v.North)); +} + + +/** + * Compute bearing of current path direction + */ +float VtolFlyController::updatePathBearing() +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + + float cur[3] = { positionState.North, + positionState.East, + positionState.Down }; + struct path_status progress; + + path_progress(pathDesired, cur, &progress, true); + + // atan2f always returns in between + and - 180 degrees + return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0])); +} + + +/** + * Compute bearing between current position and POI + */ +float VtolFlyController::updatePOIBearing() +{ + PoiLocationData poi; + + PoiLocationGet(&poi); + PositionStateData positionState; + PositionStateGet(&positionState); + + const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f; + float dLoc[3]; + float yaw = 0; + /*float elevation = 0;*/ + + dLoc[0] = positionState.North - poi.North; + dLoc[1] = positionState.East - poi.East; + dLoc[2] = positionState.Down - poi.Down; + + if (dLoc[1] < 0) { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; + } else { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; + } + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + float pathAngle = 0; + if (manualControlData.Roll > DEADBAND_HIGH) { + pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; + } else if (manualControlData.Roll < DEADBAND_LOW) { + pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; + } + + return yaw + (pathAngle / 2.0f); +} diff --git a/flight/modules/PathFollower/vtollandcontroller.cpp b/flight/modules/PathFollower/vtollandcontroller.cpp new file mode 100644 index 000000000..8e2fce732 --- /dev/null +++ b/flight/modules/PathFollower/vtollandcontroller.cpp @@ -0,0 +1,283 @@ +/* + ****************************************************************************** + * + * @file vtollandcontroller.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Vtol landing controller loop + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +// C++ includes +#include "vtollandcontroller.h" +#include "pathfollowerfsm.h" +#include "vtollandfsm.h" +#include "pidcontroldown.h" + +// Private constants + +// pointer to a singleton instance +VtolLandController *VtolLandController::p_inst = 0; + +VtolLandController::VtolLandController() + : fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false) +{} + +// Called when mode first engaged +void VtolLandController::Activate(void) +{ + if (!mActive) { + mActive = true; + SettingsUpdated(); + fsm->Activate(); + controlDown.Activate(); + controlNE.Activate(); + } +} + +uint8_t VtolLandController::IsActive(void) +{ + return mActive; +} + +uint8_t VtolLandController::Mode(void) +{ + return PATHDESIRED_MODE_LAND; +} + +// Objective updated in pathdesired, e.g. same flight mode but new target velocity +void VtolLandController::ObjectiveUpdated(void) +{ + // Set the objective's target velocity + controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]); + controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH], + pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]); + controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East); +} +void VtolLandController::Deactivate(void) +{ + if (mActive) { + mActive = false; + fsm->Inactive(); + controlDown.Deactivate(); + controlNE.Deactivate(); + } +} + + +void VtolLandController::SettingsUpdated(void) +{ + const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f; + + controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, + vtolPathFollowerSettings->HorizontalVelPID.Ki, + vtolPathFollowerSettings->HorizontalVelPID.Kd, + vtolPathFollowerSettings->HorizontalVelPID.ILimit, + dT, + vtolPathFollowerSettings->HorizontalVelMax); + + + controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); + controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward); + + controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp, + vtolPathFollowerSettings->LandVerticalVelPID.Ki, + vtolPathFollowerSettings->LandVerticalVelPID.Kd, + vtolPathFollowerSettings->LandVerticalVelPID.Beta, + dT, + vtolPathFollowerSettings->VerticalVelMax); + + // The following is not currently used in the landing control. + controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP); + + VtolSelfTuningStatsData vtolSelfTuningStats; + VtolSelfTuningStatsGet(&vtolSelfTuningStats); + controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral); + // initialise limits on thrust but note the FSM can override. + controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max); + fsm->SettingsUpdated(); +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings) +{ + PIOS_Assert(ptr_vtolPathFollowerSettings); + if (fsm == 0) { + fsm = (PathFollowerFSM *)VtolLandFSM::instance(); + VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus); + vtolPathFollowerSettings = ptr_vtolPathFollowerSettings; + controlDown.Initialize(fsm); + } + + return 0; +} + + +void VtolLandController::UpdateVelocityDesired() +{ + VelocityStateData velocityState; + + VelocityStateGet(&velocityState); + VelocityDesiredData velocityDesired; + + controlDown.UpdateVelocityState(velocityState.Down); + controlNE.UpdateVelocityState(velocityState.North, velocityState.East); + + // Implement optional horizontal position hold. + if (((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) { + // landing flight mode has stored original horizontal position in pathdesired + PositionStateData positionState; + PositionStateGet(&positionState); + controlNE.UpdatePositionState(positionState.North, positionState.East); + controlNE.ControlPosition(); + } + + velocityDesired.Down = controlDown.GetVelocityDesired(); + float north, east; + controlNE.GetVelocityDesired(&north, &east); + velocityDesired.North = north; + velocityDesired.East = east; + + // update pathstatus + pathStatus->error = 0.0f; + pathStatus->fractional_progress = 0.0f; + pathStatus->path_direction_north = velocityDesired.North; + pathStatus->path_direction_east = velocityDesired.East; + pathStatus->path_direction_down = velocityDesired.Down; + + pathStatus->correction_direction_north = velocityDesired.North - velocityState.North; + pathStatus->correction_direction_east = velocityDesired.East - velocityState.East; + pathStatus->correction_direction_down = velocityDesired.Down - velocityState.Down; + + + VelocityDesiredSet(&velocityDesired); +} + +int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction) +{ + uint8_t result = 1; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + StabilizationBankData stabSettings; + float northCommand; + float eastCommand; + + StabilizationDesiredGet(&stabDesired); + AttitudeStateGet(&attitudeState); + StabilizationBankGet(&stabSettings); + + controlNE.GetNECommand(&northCommand, &eastCommand); + stabDesired.Thrust = controlDown.GetDownCommand(); + + float angle_radians = DEG2RAD(attitudeState.Yaw); + float cos_angle = cosf(angle_radians); + float sine_angle = sinf(angle_radians); + float maxPitch = vtolPathFollowerSettings->MaxRollPitch; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch); + + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + + if (yaw_attitude) { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Yaw = yaw_direction; + } else { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; + } + + // default thrust mode to cruise control + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + + fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust + StabilizationDesiredSet(&stabDesired); + + return result; +} + +void VtolLandController::UpdateAutoPilot() +{ + fsm->Update(); + + UpdateVelocityDesired(); + + // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm + bool yaw_attitude = false; + float yaw = 0.0f; + + fsm->GetYaw(yaw_attitude, yaw); + + int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw); + + if (!result) { + fsm->Abort(); + } + + if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) { + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + } + + PathStatusSet(pathStatus); +} + +void VtolLandController::setArmedIfChanged(uint8_t val) +{ + if (flightStatus->Armed != val) { + flightStatus->Armed = val; + FlightStatusSet(flightStatus); + } +} diff --git a/flight/modules/PathFollower/vtollandfsm.cpp b/flight/modules/PathFollower/vtollandfsm.cpp new file mode 100644 index 000000000..c8f02e0be --- /dev/null +++ b/flight/modules/PathFollower/vtollandfsm.cpp @@ -0,0 +1,703 @@ +/* + ****************************************************************************** + * + * @file vtollandfsm.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief This landing state machine is a helper state machine to the + * VtolLandController. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +// C++ includes +#include + + +// Private constants +#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod) +#define MIN_LANDRATE 0.1f +#define MAX_LANDRATE 0.6f +#define LOW_ALT_DESCENT_REDUCTION_FACTOR 0.7f // TODO Need to make the transition smooth +#define LANDRATE_LOWLIMIT_FACTOR 0.5f +#define LANDRATE_HILIMIT_FACTOR 1.5f +#define TIMEOUT_INIT_ALTHOLD (3 * TIMER_COUNT_PER_SECOND) +#define TIMEOUT_WTG_FOR_DESCENTRATE (10 * TIMER_COUNT_PER_SECOND) +#define WTG_FOR_DESCENTRATE_COUNT_LIMIT 10 +#define TIMEOUT_AT_DESCENTRATE 10 +#define TIMEOUT_GROUNDEFFECT (1 * TIMER_COUNT_PER_SECOND) +#define TIMEOUT_THRUSTDOWN (2 * TIMER_COUNT_PER_SECOND) +#define LANDING_PID_SCALAR_P 2.0f +#define LANDING_PID_SCALAR_I 10.0f +#define LANDING_SLOWDOWN_HEIGHT -5.0f +#define BOUNCE_VELOCITY_TRIGGER_LIMIT -0.3f +#define BOUNCE_ACCELERATION_TRIGGER_LIMIT -6.0f +#define BOUNCE_TRIGGER_COUNT 4 +#define GROUNDEFFECT_SLOWDOWN_FACTOR 0.3f +#define GROUNDEFFECT_SLOWDOWN_COUNT 4 + +VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = { + [LAND_STATE_INACTIVE] = { .setup = &VtolLandFSM::setup_inactive, .run = 0 }, + [LAND_STATE_INIT_ALTHOLD] = { .setup = &VtolLandFSM::setup_init_althold, .run = &VtolLandFSM::run_init_althold }, + [LAND_STATE_WTG_FOR_DESCENTRATE] = { .setup = &VtolLandFSM::setup_wtg_for_descentrate, .run = &VtolLandFSM::run_wtg_for_descentrate }, + [LAND_STATE_AT_DESCENTRATE] = { .setup = &VtolLandFSM::setup_at_descentrate, .run = &VtolLandFSM::run_at_descentrate }, + [LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect }, + [LAND_STATE_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_groundeffect, .run = &VtolLandFSM::run_groundeffect }, + [LAND_STATE_THRUSTDOWN] = { .setup = &VtolLandFSM::setup_thrustdown, .run = &VtolLandFSM::run_thrustdown }, + [LAND_STATE_THRUSTOFF] = { .setup = &VtolLandFSM::setup_thrustoff, .run = &VtolLandFSM::run_thrustoff }, + [LAND_STATE_DISARMED] = { .setup = &VtolLandFSM::setup_disarmed, .run = &VtolLandFSM::run_disarmed }, + [LAND_STATE_ABORT] = { .setup = &VtolLandFSM::setup_abort, .run = &VtolLandFSM::run_abort } +}; + +// pointer to a singleton instance +VtolLandFSM *VtolLandFSM::p_inst = 0; + + +VtolLandFSM::VtolLandFSM() + : mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0) +{} + +// Private types + +// Private functions +// Public API methods +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings, + PathDesiredData *ptr_pathDesired, + FlightStatusData *ptr_flightStatus) +{ + PIOS_Assert(ptr_vtolPathFollowerSettings); + PIOS_Assert(ptr_pathDesired); + PIOS_Assert(ptr_flightStatus); + + if (mLandData == 0) { + mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T)); + PIOS_Assert(mLandData); + } + memset(mLandData, sizeof(VtolLandFSMData_T), 0); + vtolPathFollowerSettings = ptr_vtolPathFollowerSettings; + pathDesired = ptr_pathDesired; + flightStatus = ptr_flightStatus; + initFSM(); + + return 0; +} + +void VtolLandFSM::Inactive(void) +{ + memset(mLandData, sizeof(VtolLandFSMData_T), 0); + initFSM(); +} + +// Initialise the FSM +void VtolLandFSM::initFSM(void) +{ + if (vtolPathFollowerSettings != 0) { + setState(LAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE); + } else { + mLandData->currentState = LAND_STATE_INACTIVE; + } +} + +void VtolLandFSM::Activate() +{ + memset(mLandData, sizeof(VtolLandFSMData_T), 0); + mLandData->currentState = LAND_STATE_INACTIVE; + mLandData->flLowAltitude = false; + mLandData->flAltitudeHold = false; + mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE; + mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral; + mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral; + mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; + mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; + TakeOffLocationGet(&(mLandData->takeOffLocation)); + mLandData->fsmLandStatus.AltitudeAtState[LAND_STATE_INACTIVE] = 0.0f; + assessAltitude(); + + if (pathDesired->Mode == PATHDESIRED_MODE_LAND) { +#ifndef DEBUG_GROUNDIMPACT + setState(LAND_STATE_INIT_ALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE); +#else + setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE); +#endif + } else { + // move to error state and callback to position hold + setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE); + } +} + +void VtolLandFSM::Abort(void) +{ + setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE); +} + +PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void) +{ + switch (mLandData->currentState) { + case LAND_STATE_INACTIVE: + return PFFSM_STATE_INACTIVE; + + break; + case LAND_STATE_ABORT: + return PFFSM_STATE_ABORT; + + break; + case LAND_STATE_DISARMED: + return PFFSM_STATE_DISARMED; + + break; + default: + return PFFSM_STATE_ACTIVE; + + break; + } +} + +void VtolLandFSM::Update() +{ + runState(); + if (GetCurrentState() != PFFSM_STATE_INACTIVE) { + runAlways(); + } +} + +int32_t VtolLandFSM::runState(void) +{ + uint8_t flTimeout = false; + + mLandData->stateRunCount++; + + if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) { + flTimeout = true; + } + + // If the current state has a static function, call it + if (sLandStateTable[mLandData->currentState].run) { + (this->*sLandStateTable[mLandData->currentState].run)(flTimeout); + } + return 0; +} + +int32_t VtolLandFSM::runAlways(void) +{ + void assessAltitude(void); + + return 0; +} + +// PathFollower implements the PID scheme and has a objective +// set by a PathDesired object. Based on the mode, pathfollower +// uses FSM's as helper functions that manage state and event detection. +// PathFollower calls into FSM methods to alter its commands. + +void VtolLandFSM::BoundThrust(float &ulow, float &uhigh) +{ + ulow = mLandData->boundThrustMin; + uhigh = mLandData->boundThrustMax; + + + if (mLandData->flConstrainThrust) { + uhigh = mLandData->thrustLimit; + } +} + +void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired) +{ + if (mLandData->flZeroStabiHorizontal && stabDesired) { + stabDesired->Pitch = 0.0f; + stabDesired->Roll = 0.0f; + stabDesired->Yaw = 0.0f; + } +} + +void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler) +{ + if (mLandData->flLowAltitude) { + local_scaler->p = LANDING_PID_SCALAR_P; + local_scaler->i = LANDING_PID_SCALAR_I; + } +} + + +// Set the new state and perform setup for subsequent state run calls +// This is called by state run functions on event detection that drive +// state transitions. +void VtolLandFSM::setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason) +{ + mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason; + + if (mLandData->currentState == newState) { + return; + } + mLandData->currentState = newState; + + if (newState != LAND_STATE_INACTIVE) { + PositionStateData positionState; + PositionStateGet(&positionState); + float takeOffDown = 0.0f; + if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) { + takeOffDown = mLandData->takeOffLocation.Down; + } + mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown; + assessAltitude(); + } + + // Restart state timer counter + mLandData->stateRunCount = 0; + + // Reset state timeout to disabled/zero + mLandData->stateTimeoutCount = 0; + + if (sLandStateTable[mLandData->currentState].setup) { + (this->*sLandStateTable[mLandData->currentState].setup)(); + } + + updateVtolLandFSMStatus(); +} + + +// Timeout utility function for use by state init implementations +void VtolLandFSM::setStateTimeout(int32_t count) +{ + mLandData->stateTimeoutCount = count; +} + +void VtolLandFSM::updateVtolLandFSMStatus() +{ + mLandData->fsmLandStatus.State = mLandData->currentState; + if (mLandData->flLowAltitude) { + mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW; + } else { + mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH; + } + StatusVtolLandSet(&mLandData->fsmLandStatus); +} + + +float VtolLandFSM::BoundVelocityDown(float velocity_down) +{ + velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE); + if (mLandData->flLowAltitude) { + velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR; + } + mLandData->fsmLandStatus.targetDescentRate = velocity_down; + + if (mLandData->flAltitudeHold) { + return 0.0f; + } else { + return velocity_down; + } +} + +void VtolLandFSM::assessAltitude(void) +{ + float positionDown; + + PositionStateDownGet(&positionDown); + float takeOffDown = 0.0f; + if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) { + takeOffDown = mLandData->takeOffLocation.Down; + } + float positionDownRelativeToTakeoff = positionDown - takeOffDown; + if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) { + mLandData->flLowAltitude = false; + } else { + mLandData->flLowAltitude = true; + } +} + + +// FSM Setup and Run method implementation + +// State: INACTIVE +void VtolLandFSM::setup_inactive(void) +{ + // Re-initialise local variables + mLandData->flZeroStabiHorizontal = false; + mLandData->flConstrainThrust = false; +} + +// State: INIT ALTHOLD +void VtolLandFSM::setup_init_althold(void) +{ + setStateTimeout(TIMEOUT_INIT_ALTHOLD); + // get target descent velocity + mLandData->flZeroStabiHorizontal = false; + mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]); + mLandData->flConstrainThrust = false; + mLandData->flAltitudeHold = true; + mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; + mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; +} + +void VtolLandFSM::run_init_althold(uint8_t flTimeout) +{ + if (flTimeout) { + mLandData->flAltitudeHold = false; + setState(LAND_STATE_WTG_FOR_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT); + } +} + + +// State: WAITING FOR DESCENT RATE +void VtolLandFSM::setup_wtg_for_descentrate(void) +{ + setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE); + // get target descent velocity + mLandData->flZeroStabiHorizontal = false; + mLandData->observationCount = 0; + mLandData->observation2Count = 0; + mLandData->flConstrainThrust = false; + mLandData->flAltitudeHold = false; + mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; + mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; +} + +void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout) +{ + // Look at current actual thrust...are we already shutdown?? + VelocityStateData velocityState; + + VelocityStateGet(&velocityState); + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + + // We don't expect PID to get exactly the target descent rate, so have a lower + // water mark but need to see 5 observations to be confident that we have semi-stable + // descent achieved + + // we need to see velocity down within a range of control before we proceed, without which we + // really don't have confidence to allow later states to run. + if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) && + velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) { + if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) { + setState(LAND_STATE_AT_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK); + return; + } + } + + if (flTimeout) { + setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT); + } +} + + +// State: AT DESCENT RATE +void VtolLandFSM::setup_at_descentrate(void) +{ + setStateTimeout(TIMEOUT_AT_DESCENTRATE); + mLandData->flZeroStabiHorizontal = false; + mLandData->observationCount = 0; + mLandData->sum1 = 0.0f; + mLandData->sum2 = 0.0f; + mLandData->flConstrainThrust = false; + mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE; + mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral; + mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; + mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; +} + +void VtolLandFSM::run_at_descentrate(uint8_t flTimeout) +{ + VelocityStateData velocityState; + + VelocityStateGet(&velocityState); + + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + + mLandData->sum1 += velocityState.Down; + mLandData->sum2 += stabDesired.Thrust; + mLandData->observationCount++; + if (flTimeout) { + mLandData->fsmLandStatus.averageDescentRate = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE); + mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max); + + // We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground + // As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can + // detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value + mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust; + mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max); + + + setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK); + } +} + + +// State: WAITING FOR GROUND EFFECT +void VtolLandFSM::setup_wtg_for_groundeffect(void) +{ + // No timeout + mLandData->flZeroStabiHorizontal = false; + mLandData->observationCount = 0; + mLandData->observation2Count = 0; + mLandData->sum1 = 0.0f; + mLandData->sum2 = 0.0f; + mLandData->flConstrainThrust = false; + mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f; + mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f; + mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; + mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; +} + +void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout) +{ + // detect material downrating in thrust for 1 second. + VelocityStateData velocityState; + + VelocityStateGet(&velocityState); + AccelStateData accelState; + AccelStateGet(&accelState); + + // +ve 9.8 expected + float g_e; + HomeLocationg_eGet(&g_e); + + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + + // detect bounce + uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT); + if (flBounce) { + mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down; + } else { + mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f; + } + + // invert sign of accel to the standard convention of down is +ve and subtract the gravity to get + // a relative acceleration term. + float bounceAccel = -accelState.z - g_e; + uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT); + if (flBounceAccel) { + mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel; + } else { + mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f; + } + + if (flBounce || flBounceAccel) { + mLandData->observation2Count++; + if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) { + setState(LAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL)); + return; + } + } else { + mLandData->observation2Count = 0; + } + + // detect low descent rate + uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate)); + if (flDescentRateLow) { + mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust; + mLandData->observationCount++; + if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) { +#ifndef DEBUG_GROUNDIMPACT + setState(LAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE); +#endif + return; + } + } else { + mLandData->observationCount = 0; + } + + updateVtolLandFSMStatus(); +} + +// STATE: GROUNDEFFET +void VtolLandFSM::setup_groundeffect(void) +{ + setStateTimeout(TIMEOUT_GROUNDEFFECT); + mLandData->flZeroStabiHorizontal = true; + PositionStateData positionState; + PositionStateGet(&positionState); + mLandData->expectedLandPositionNorth = positionState.North; + mLandData->expectedLandPositionEast = positionState.East; + mLandData->flConstrainThrust = false; + + // now that we have ground effect limit max thrust to neutral + mLandData->boundThrustMin = -0.1f; + mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust; +} +void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout) +{ + StabilizationDesiredData stabDesired; + + StabilizationDesiredGet(&stabDesired); + if (stabDesired.Thrust < 0.0f) { + setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST); + return; + } + + // Stay in this state until we get a low altitude flag. + if (mLandData->flLowAltitude == false) { + // worst case scenario is that we land and the pid brings thrust down to zero. + return; + } + + // detect broad sideways drift. If for some reason we have a hard landing that the bounce detection misses, this will kick in + PositionStateData positionState; + PositionStateGet(&positionState); + float north_error = mLandData->expectedLandPositionNorth - positionState.North; + float east_error = mLandData->expectedLandPositionEast - positionState.East; + float positionError = sqrtf(north_error * north_error + east_error * east_error); + if (positionError > 0.3f) { + setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR); + return; + } + + if (flTimeout) { + setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT); + } +} + +// STATE: THRUSTDOWN +void VtolLandFSM::setup_thrustdown(void) +{ + setStateTimeout(TIMEOUT_THRUSTDOWN); + mLandData->flZeroStabiHorizontal = true; + mLandData->flConstrainThrust = true; + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + mLandData->thrustLimit = stabDesired.Thrust; + mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN; + mLandData->boundThrustMin = -0.1f; + mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust; +} + +void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout) +{ + // reduce thrust setpoint step by step + mLandData->thrustLimit -= mLandData->sum1; + + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) { + setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST); + } + + if (flTimeout) { + setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT); + } +} + +// STATE: THRUSTOFF +void VtolLandFSM::setup_thrustoff(void) +{ + mLandData->thrustLimit = -1.0f; + mLandData->flConstrainThrust = true; + mLandData->boundThrustMin = -0.1f; + mLandData->boundThrustMax = 0.0f; +} + +void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout) +{ + setState(LAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE); +} + +// STATE: DISARMED +void VtolLandFSM::setup_disarmed(void) +{ + // nothing to do + mLandData->flConstrainThrust = false; + mLandData->flZeroStabiHorizontal = false; + mLandData->observationCount = 0; + mLandData->boundThrustMin = -0.1f; + mLandData->boundThrustMax = 0.0f; +} + +void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout) +{ +#ifdef DEBUG_GROUNDIMPACT + if (mLandData->observationCount++ > 100) { + setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE); + } +#endif +} + +void VtolLandFSM::fallback_to_hold(void) +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + pathDesired->End.North = positionState.North; + pathDesired->End.East = positionState.East; + pathDesired->End.Down = positionState.Down; + pathDesired->Start.North = positionState.North; + pathDesired->Start.East = positionState.East; + pathDesired->Start.Down = positionState.Down; + pathDesired->StartingVelocity = 0.0f; + pathDesired->EndingVelocity = 0.0f; + pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT; + + PathDesiredSet(pathDesired); +} + +// abort repeatedly overwrites pathfollower's objective on a landing abort and +// continues to do so until a flight mode change. +void VtolLandFSM::setup_abort(void) +{ + mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; + mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; + mLandData->flConstrainThrust = false; + mLandData->flZeroStabiHorizontal = false; + fallback_to_hold(); +} + +void VtolLandFSM::run_abort(__attribute__((unused)) uint8_t flTimeout) +{} diff --git a/flight/modules/PathFollower/vtolvelocitycontroller.cpp b/flight/modules/PathFollower/vtolvelocitycontroller.cpp new file mode 100644 index 000000000..84ba99daa --- /dev/null +++ b/flight/modules/PathFollower/vtolvelocitycontroller.cpp @@ -0,0 +1,227 @@ +/* + ****************************************************************************** + * + * @file VtolVelocityController.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Velocity roam controller for vtols + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include + +#include + +#include +#include +#include +#include +#include +#include +#include "plans.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +// C++ includes +#include "vtolvelocitycontroller.h" +#include "pidcontrolne.h" + +// Private constants + +// pointer to a singleton instance +VtolVelocityController *VtolVelocityController::p_inst = 0; + +VtolVelocityController::VtolVelocityController() + : vtolPathFollowerSettings(0), mActive(false) +{} + +// Called when mode first engaged +void VtolVelocityController::Activate(void) +{ + if (!mActive) { + mActive = true; + SettingsUpdated(); + controlNE.Activate(); + } +} + +uint8_t VtolVelocityController::IsActive(void) +{ + return mActive; +} + +uint8_t VtolVelocityController::Mode(void) +{ + return PATHDESIRED_MODE_VELOCITY; +} + +void VtolVelocityController::ObjectiveUpdated(void) +{ + controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH], + pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]); +} + +void VtolVelocityController::Deactivate(void) +{ + if (mActive) { + mActive = false; + controlNE.Deactivate(); + } +} + +void VtolVelocityController::SettingsUpdated(void) +{ + const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f; + + controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, + vtolPathFollowerSettings->HorizontalVelPID.Ki, + vtolPathFollowerSettings->HorizontalVelPID.Kd, + vtolPathFollowerSettings->HorizontalVelPID.ILimit, + dT, + vtolPathFollowerSettings->HorizontalVelMax); + + + controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); + controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward); +} + +int32_t VtolVelocityController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings) +{ + PIOS_Assert(ptr_vtolPathFollowerSettings); + + vtolPathFollowerSettings = ptr_vtolPathFollowerSettings; + return 0; +} + + +void VtolVelocityController::UpdateVelocityDesired() +{ + VelocityStateData velocityState; + + VelocityStateGet(&velocityState); + VelocityDesiredData velocityDesired; + + controlNE.UpdateVelocityState(velocityState.North, velocityState.East); + + velocityDesired.Down = 0.0f; + float north, east; + controlNE.GetVelocityDesired(&north, &east); + velocityDesired.North = north; + velocityDesired.East = east; + + // update pathstatus + pathStatus->error = 0.0f; + pathStatus->fractional_progress = 0.0f; + pathStatus->path_direction_north = velocityDesired.North; + pathStatus->path_direction_east = velocityDesired.East; + pathStatus->path_direction_down = velocityDesired.Down; + + pathStatus->correction_direction_north = velocityDesired.North - velocityState.North; + pathStatus->correction_direction_east = velocityDesired.East - velocityState.East; + pathStatus->correction_direction_down = 0.0f; + + VelocityDesiredSet(&velocityDesired); +} + +int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)) bool yaw_attitude, __attribute__((unused)) float yaw_direction) +{ + uint8_t result = 1; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + StabilizationBankData stabSettings; + float northCommand; + float eastCommand; + + StabilizationDesiredGet(&stabDesired); + AttitudeStateGet(&attitudeState); + StabilizationBankGet(&stabSettings); + + controlNE.GetNECommand(&northCommand, &eastCommand); + + float angle_radians = DEG2RAD(attitudeState.Yaw); + float cos_angle = cosf(angle_radians); + float sine_angle = sinf(angle_radians); + float maxPitch = vtolPathFollowerSettings->MaxRollPitch; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch); + + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; + + // default thrust mode to altvario + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO; + + StabilizationDesiredSet(&stabDesired); + + return result; +} + +void VtolVelocityController::UpdateAutoPilot() +{ + UpdateVelocityDesired(); + + bool yaw_attitude = false; + float yaw = 0.0f; + + int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw); + + if (!result) { + fallback_to_hold(); + } + + PathStatusSet(pathStatus); +} + +void VtolVelocityController::fallback_to_hold(void) +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + pathDesired->End.North = positionState.North; + pathDesired->End.East = positionState.East; + pathDesired->End.Down = positionState.Down; + pathDesired->Start.North = positionState.North; + pathDesired->Start.East = positionState.East; + pathDesired->Start.Down = positionState.Down; + pathDesired->StartingVelocity = 0.0f; + pathDesired->EndingVelocity = 0.0f; + pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT; + + PathDesiredSet(pathDesired); +} diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 2ab9f1a61..8277d4da8 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -43,8 +43,11 @@ #include "waypoint.h" #include "waypointactive.h" #include "flightmodesettings.h" +#include #include "paths.h" #include "plans.h" +#include +#include // Private constants #define STACK_SIZE_BYTES 1024 @@ -73,6 +76,7 @@ static uint8_t conditionAboveSpeed(); static uint8_t conditionPointingTowardsNext(); static uint8_t conditionPythonScript(); static uint8_t conditionImmediate(); +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); // Private variables @@ -82,7 +86,10 @@ static WaypointActiveData waypointActive; static WaypointData waypoint; static PathActionData pathAction; static bool pathplanner_active = false; +static FrameType_t frameType; +static bool mode3D; +extern FrameType_t GetCurrentFrameType(); /** * Module initialization @@ -95,6 +102,9 @@ int32_t PathPlannerStart() WaypointActiveConnectCallback(commandUpdated); PathActionConnectCallback(commandUpdated); PathStatusConnectCallback(statusUpdated); + SettingsUpdatedCb(NULL); + SystemSettingsConnectCallback(&SettingsUpdatedCb); + VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); // Start main task callback PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle); @@ -125,6 +135,34 @@ int32_t PathPlannerInitialize() MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart); + +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + uint8_t TreatCustomCraftAs; + + VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs); + + frameType = GetCurrentFrameType(); + + if (frameType == FRAME_TYPE_CUSTOM) { + switch (TreatCustomCraftAs) { + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: + frameType = FRAME_TYPE_FIXED_WING; + mode3D = true; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: + frameType = FRAME_TYPE_MULTIROTOR; + mode3D = true; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND: + frameType = FRAME_TYPE_GROUND; + mode3D = false; + break; + } + } +} + + /** * Module task */ @@ -160,22 +198,17 @@ static void pathPlannerTask() static uint8_t failsafeRTHset = 0; if (!validPathPlan) { - // At this point the craft is in PathPlanner mode, so pilot has no manual control capability. - // Failsafe: behave as if in return to base mode - // what to do in this case is debatable, it might be better to just - // make a forced disarming but rth has a higher chance of survival when - // in flight. pathplanner_active = false; if (!failsafeRTHset) { failsafeRTHset = 1; - // copy pasta: same calculation as in manualcontrol, set return to home coordinates plan_setup_positionHold(); } AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL); return; } + failsafeRTHset = 0; AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN); @@ -525,7 +558,7 @@ static uint8_t conditionLegRemaining() struct path_status progress; path_progress(&pathDesired, - cur, &progress); + cur, &progress, mode3D); if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) { return true; } @@ -549,7 +582,7 @@ static uint8_t conditionBelowError() struct path_status progress; path_progress(&pathDesired, - cur, &progress); + cur, &progress, mode3D); if (progress.error <= pathAction.ConditionParameters[0]) { return true; } diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index 798a4ed6e..a1146f75e 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -108,8 +108,8 @@ static void radioRxTask(void *parameters); static void PPMInputTask(void *parameters); static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length); static int32_t RadioSendHandler(uint8_t *buf, int32_t length); -static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); -static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); +static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count); +static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count); static void objectPersistenceUpdatedCb(UAVObjEvent *objEv); static void registerObject(UAVObjHandle obj); @@ -403,14 +403,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX); #endif if (PIOS_COM_RADIO) { - uint8_t serial_data[1]; + uint8_t serial_data[16]; uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { if (data->parseUAVTalk) { // Pass the data through the UAVTalk parser. - for (uint8_t i = 0; i < bytes_to_process; i++) { - ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]); - } + ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data, bytes_to_process); } else if (PIOS_COM_TELEMETRY) { // Send the data straight to the telemetry port. // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex) @@ -448,12 +446,10 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) } #endif /* PIOS_INCLUDE_USB */ if (inputPort) { - uint8_t serial_data[1]; + uint8_t serial_data[16]; uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { - for (uint8_t i = 0; i < bytes_to_process; i++) { - ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data[i]); - } + ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data, bytes_to_process); } } else { vTaskDelay(5); @@ -597,42 +593,45 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) * @param[in] outConnectionHandle The UAVTalk connection handle on the radio port. * @param[in] rxbyte The received byte. */ -static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte) +static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length) { - // Keep reading until we receive a completed packet. - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte); + uint8_t position = 0; - if (state == UAVTALK_STATE_COMPLETE) { - // We only want to unpack certain telemetry objects - uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); - switch (objId) { - case OPLINKSTATUS_OBJID: - case OPLINKSETTINGS_OBJID: - case OPLINKRECEIVER_OBJID: - case MetaObjectId(OPLINKSTATUS_OBJID): - case MetaObjectId(OPLINKSETTINGS_OBJID): - case MetaObjectId(OPLINKRECEIVER_OBJID): - UAVTalkReceiveObject(inConnectionHandle); - break; - case OBJECTPERSISTENCE_OBJID: - case MetaObjectId(OBJECTPERSISTENCE_OBJID): - // receive object locally - // some objects will send back a response to telemetry - // FIXME: - // OPLM will ack or nack all objects requests and acked object sends - // Receiver will probably also ack / nack the same messages - // This has some consequences like : - // Second ack/nack will not match an open transaction or will apply to wrong transaction - // Question : how does GCS handle receiving the same object twice - // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks... - UAVTalkReceiveObject(inConnectionHandle); - // relay packet to remote modem - UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); - break; - default: - // all other packets are relayed to the remote modem - UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); - break; + // Keep reading until we receive a completed packet. + while (position < length) { + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position); + if (state == UAVTALK_STATE_COMPLETE) { + // We only want to unpack certain telemetry objects + uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); + switch (objId) { + case OPLINKSTATUS_OBJID: + case OPLINKSETTINGS_OBJID: + case OPLINKRECEIVER_OBJID: + case MetaObjectId(OPLINKSTATUS_OBJID): + case MetaObjectId(OPLINKSETTINGS_OBJID): + case MetaObjectId(OPLINKRECEIVER_OBJID): + UAVTalkReceiveObject(inConnectionHandle); + break; + case OBJECTPERSISTENCE_OBJID: + case MetaObjectId(OBJECTPERSISTENCE_OBJID): + // receive object locally + // some objects will send back a response to telemetry + // FIXME: + // OPLM will ack or nack all objects requests and acked object sends + // Receiver will probably also ack / nack the same messages + // This has some consequences like : + // Second ack/nack will not match an open transaction or will apply to wrong transaction + // Question : how does GCS handle receiving the same object twice + // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks... + UAVTalkReceiveObject(inConnectionHandle); + // relay packet to remote modem + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + break; + default: + // all other packets are relayed to the remote modem + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + break; + } } } } @@ -642,39 +641,43 @@ static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalk * * @param[in] inConnectionHandle The UAVTalk connection handle on the radio port. * @param[in] outConnectionHandle The UAVTalk connection handle on the telemetry port. - * @param[in] rxbyte The received byte. + * @param[in] rxbuffer The received buffer. + * @param[in] length buffer length */ -static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte) +static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length) { - // Keep reading until we receive a completed packet. - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte); + uint8_t position = 0; - if (state == UAVTALK_STATE_COMPLETE) { - // We only want to unpack certain objects from the remote modem - // Similarly we only want to relay certain objects to the telemetry port - uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); - switch (objId) { - case OPLINKSTATUS_OBJID: - case OPLINKSETTINGS_OBJID: - case MetaObjectId(OPLINKSTATUS_OBJID): - case MetaObjectId(OPLINKSETTINGS_OBJID): - // Ignore object... - // These objects are shadowed by the modem and are not transmitted to the telemetry port - // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead - // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead - break; - case OPLINKRECEIVER_OBJID: - case MetaObjectId(OPLINKRECEIVER_OBJID): - // Receive object locally - // These objects are received by the modem and are not transmitted to the telemetry port - // - OPLINKRECEIVER_OBJID : not sure why - // some objects will send back a response to the remote modem - UAVTalkReceiveObject(inConnectionHandle); - break; - default: - // all other packets are relayed to the telemetry port - UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); - break; + // Keep reading until we receive a completed packet. + while (position < length) { + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position); + if (state == UAVTALK_STATE_COMPLETE) { + // We only want to unpack certain objects from the remote modem + // Similarly we only want to relay certain objects to the telemetry port + uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); + switch (objId) { + case OPLINKSTATUS_OBJID: + case OPLINKSETTINGS_OBJID: + case MetaObjectId(OPLINKSTATUS_OBJID): + case MetaObjectId(OPLINKSETTINGS_OBJID): + // Ignore object... + // These objects are shadowed by the modem and are not transmitted to the telemetry port + // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead + // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead + break; + case OPLINKRECEIVER_OBJID: + case MetaObjectId(OPLINKRECEIVER_OBJID): + // Receive object locally + // These objects are received by the modem and are not transmitted to the telemetry port + // - OPLINKRECEIVER_OBJID : not sure why + // some objects will send back a response to the remote modem + UAVTalkReceiveObject(inConnectionHandle); + break; + default: + // all other packets are relayed to the telemetry port + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + break; + } } } } diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 2f42b7eed..3ff31a704 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -41,10 +41,12 @@ #include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #include +#include #endif #include #include #include +#include #if defined(PIOS_INCLUDE_USB_RCTX) @@ -72,6 +74,7 @@ // Private variables static xTaskHandle taskHandle; static portTickType lastSysTime; +static FrameType_t frameType = FRAME_TYPE_MULTIROTOR; #ifdef USE_INPUT_LPF static portTickType lastSysTimeLPF; @@ -84,6 +87,7 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); static bool validInputRange(int16_t min, int16_t max, uint16_t value); static void applyDeadband(float *value, float deadband); +static void SettingsUpdatedCb(UAVObjEvent *ev); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES static uint8_t isAssistedFlightMode(uint8_t position); @@ -124,6 +128,7 @@ int32_t ReceiverStart() #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); #endif + SettingsUpdatedCb(NULL); return 0; } @@ -141,13 +146,43 @@ int32_t ReceiverInitialize() ManualControlSettingsInitialize(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES StabilizationSettingsInitialize(); + VtolPathFollowerSettingsInitialize(); + VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); #endif + SystemSettingsInitialize(); + SystemSettingsConnectCallback(&SettingsUpdatedCb); return 0; } MODULE_INITCALL(ReceiverInitialize, ReceiverStart); +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + frameType = GetCurrentFrameType(); + +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + uint8_t TreatCustomCraftAs; + VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs); + + + if (frameType == FRAME_TYPE_CUSTOM) { + switch (TreatCustomCraftAs) { + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: + frameType = FRAME_TYPE_FIXED_WING; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: + frameType = FRAME_TYPE_MULTIROTOR; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND: + frameType = FRAME_TYPE_GROUND; + break; + } + } +#endif +} + + /** * Module task */ @@ -243,22 +278,16 @@ static void receiverTask(__attribute__((unused)) void *parameters) } } - // Check settings, if error raise alarm - if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + // Sanity Check Throttle and Yaw + if (settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || // Check all channel mappings are valid - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID || // Check the driver exists - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER || // Check collective if required @@ -282,15 +311,39 @@ static void receiverTask(__attribute__((unused)) void *parameters) continue; } + + if (frameType != FRAME_TYPE_GROUND) { + // Sanity Check Pitch and Roll + if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || + // Check all channel mappings are valid + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID + || + // Check the driver exists + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER) { + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL); + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + ManualControlCommandSet(&cmd); + + continue; + } + } + // decide if we have valid manual input or not valid_input_detected &= validInputRange(settings.ChannelMin.Throttle, settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) - && validInputRange(settings.ChannelMin.Roll, - settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) && validInputRange(settings.ChannelMin.Yaw, - settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) - && validInputRange(settings.ChannelMin.Pitch, - settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]); + + if (frameType != FRAME_TYPE_GROUND) { + valid_input_detected &= validInputRange(settings.ChannelMin.Roll, + settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin.Pitch, + settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + } if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { valid_input_detected &= validInputRange(settings.ChannelMin.Collective, @@ -321,10 +374,14 @@ static void receiverTask(__attribute__((unused)) void *parameters) } if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - cmd.Throttle = settings.FailsafeChannel.Throttle; - cmd.Roll = settings.FailsafeChannel.Roll; - cmd.Pitch = settings.FailsafeChannel.Pitch; - cmd.Yaw = settings.FailsafeChannel.Yaw; + if (frameType != FRAME_TYPE_GROUND) { + cmd.Throttle = settings.FailsafeChannel.Throttle; + } else { + cmd.Throttle = 0.0f; + } + cmd.Roll = settings.FailsafeChannel.Roll; + cmd.Pitch = settings.FailsafeChannel.Pitch; + cmd.Yaw = settings.FailsafeChannel.Yaw; cmd.Collective = settings.FailsafeChannel.Collective; switch (thrustType) { case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: @@ -401,6 +458,9 @@ static void receiverTask(__attribute__((unused)) void *parameters) applyDeadband(&cmd.Roll, deadband_checked); applyDeadband(&cmd.Pitch, deadband_checked); applyDeadband(&cmd.Yaw, deadband_checked); + if (frameType == FRAME_TYPE_GROUND) { // assumes reversible motors + applyDeadband(&cmd.Throttle, deadband_checked); + } } #ifdef USE_INPUT_LPF // Apply Low Pass Filter to input channels, time delta between calls in ms diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index b1d8288d6..623480dd5 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -411,9 +411,9 @@ static void handleAccel(float *samples, float temperature) AccelSensorData accelSensorData; updateAccelTempBias(temperature); - float accels_out[3] = { samples[0] * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0], - samples[1] * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1], - samples[2] * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] }; + float accels_out[3] = { (samples[0] - agcal.accel_bias.X) * agcal.accel_scale.X - accel_temp_bias[0], + (samples[1] - agcal.accel_bias.Y) * agcal.accel_scale.Y - accel_temp_bias[1], + (samples[2] - agcal.accel_bias.Z) * agcal.accel_scale.Z - accel_temp_bias[2] }; rot_mult(R, accels_out, samples); accelSensorData.x = samples[0]; @@ -485,7 +485,10 @@ static void updateAccelTempBias(float temperature) if ((accel_temp_calibrated) && !accel_temp_calibration_count) { accel_temp_calibration_count = TEMP_CALIB_INTERVAL; if (accel_temp_calibrated) { - float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min); + float ctemp = boundf(accel_temperature, + agcal.temp_calibrated_extent.max, + agcal.temp_calibrated_extent.min); + accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp; accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp; accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp; @@ -588,8 +591,12 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent); - baro_temp_correction_enabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f || - (baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f)); + baro_temp_correction_enabled = + (baroCorrectionExtent.max - baroCorrectionExtent.min > 0.1f && + (fabsf(baroCorrection.a) > 1e-9f || + fabsf(baroCorrection.b) > 1e-9f || + fabsf(baroCorrection.c) > 1e-9f || + fabsf(baroCorrection.d) > 1e-9f)); } /** * @} diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 5c999f01e..3ed9dfe02 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -345,7 +345,6 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart); */ static void StateEstimationCb(void) { - static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD; static filterResult alarm = FILTERRESULT_OK; static filterResult lastAlarm = FILTERRESULT_UNINITIALISED; static uint16_t alarmcounter = 0; @@ -361,193 +360,171 @@ static void StateEstimationCb(void) return; } - switch (runState) { - case RUNSTATE_LOAD: + alarm = FILTERRESULT_OK; - alarm = FILTERRESULT_OK; - - // set alarm to warning if called through timeout - if (updatedSensors == 0) { - if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) { - alarm = FILTERRESULT_WARNING; - } - } else { - last_time = PIOS_DELAY_GetRaw(); + // set alarm to warning if called through timeout + if (updatedSensors == 0) { + if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) { + alarm = FILTERRESULT_WARNING; } + } else { + last_time = PIOS_DELAY_GetRaw(); + } - // check if a new filter chain should be initialized - if (fusionAlgorithm != revoSettings.FusionAlgorithm) { - FlightStatusData fs; - FlightStatusGet(&fs); - if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) { - const filterPipeline *newFilterChain; - switch (revoSettings.FusionAlgorithm) { - case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY: - newFilterChain = cfQueue; - break; - case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG: - newFilterChain = cfmiQueue; - break; - case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR: - newFilterChain = cfmQueue; - break; - case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: - newFilterChain = ekf13iQueue; - break; - case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: - newFilterChain = ekf13Queue; - break; - default: - newFilterChain = NULL; - } - // initialize filters in chain - current = newFilterChain; - bool error = 0; - while (current != NULL) { - int32_t result = current->filter->init((stateFilter *)current->filter); - if (result != 0) { - error = 1; - break; - } - current = current->next; - } - if (error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return; - } else { - // set new fusion algortithm - filterChain = newFilterChain; - fusionAlgorithm = revoSettings.FusionAlgorithm; - } - } - } - - // read updated sensor UAVObjects and set initial state - states.updated = updatedSensors; - updatedSensors = 0; - - // fetch sensors, check values, and load into state struct - FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z); - if (IS_SET(states.updated, SENSORUPDATES_gyro)) { - gyroRaw[0] = states.gyro[0]; - gyroRaw[1] = states.gyro[1]; - gyroRaw[2] = states.gyro[2]; - } - FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z); - FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z); - FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z); - FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); - FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true); - FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); - - // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself - - // at this point sensor state is stored in "states" with some rudimentary filtering applied - - // apply all filters in the current filter chain - current = filterChain; - - // we are not done, re-dispatch self execution - runState = RUNSTATE_FILTER; - PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback); - break; - - case RUNSTATE_FILTER: - - if (current != NULL) { - filterResult result = current->filter->filter((stateFilter *)current->filter, &states); - if (result > alarm) { - alarm = result; - } - current = current->next; - } - - // we are not done, re-dispatch self execution - if (!current) { - runState = RUNSTATE_SAVE; - } - PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback); - break; - - case RUNSTATE_SAVE: - - // the final output of filters is saved in state variables - // EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut - if (IS_SET(states.updated, SENSORUPDATES_gyro)) { - gyroDelta[0] = states.gyro[0] - gyroRaw[0]; - gyroDelta[1] = states.gyro[1] - gyroRaw[1]; - gyroDelta[2] = states.gyro[2] - gyroRaw[2]; - } - EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z); - if (IS_SET(states.updated, SENSORUPDATES_mag)) { - MagStateData s; - - MagStateGet(&s); - s.x = states.mag[0]; - s.y = states.mag[1]; - s.z = states.mag[2]; - switch (states.magStatus) { - case MAGSTATUS_OK: - s.Source = MAGSTATE_SOURCE_ONBOARD; + // check if a new filter chain should be initialized + if (fusionAlgorithm != revoSettings.FusionAlgorithm) { + FlightStatusData fs; + FlightStatusGet(&fs); + if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) { + const filterPipeline *newFilterChain; + switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) { + case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY: + newFilterChain = cfQueue; break; - case MAGSTATUS_AUX: - s.Source = MAGSTATE_SOURCE_AUX; + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG: + newFilterChain = cfmiQueue; + break; + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR: + newFilterChain = cfmQueue; + break; + case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: + newFilterChain = ekf13iQueue; + break; + case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: + newFilterChain = ekf13Queue; break; default: - s.Source = MAGSTATE_SOURCE_INVALID; + newFilterChain = NULL; + } + // initialize filters in chain + current = newFilterChain; + bool error = 0; + while (current != NULL) { + int32_t result = current->filter->init((stateFilter *)current->filter); + if (result != 0) { + error = 1; + break; + } + current = current->next; + } + if (error) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return; + } else { + // set new fusion algortithm + filterChain = newFilterChain; + fusionAlgorithm = revoSettings.FusionAlgorithm; } - MagStateSet(&s); } + } - EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down); - EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down); - EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed); - // attitude nees manual conversion from quaternion to euler - if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \ - AttitudeStateData s; - AttitudeStateGet(&s); - s.q1 = states.attitude[0]; - s.q2 = states.attitude[1]; - s.q3 = states.attitude[2]; - s.q4 = states.attitude[3]; - Quaternion2RPY(&s.q1, &s.Roll); - AttitudeStateSet(&s); + // read updated sensor UAVObjects and set initial state + states.updated = updatedSensors; + updatedSensors = 0; + + // fetch sensors, check values, and load into state struct + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z); + if (IS_SET(states.updated, SENSORUPDATES_gyro)) { + gyroRaw[0] = states.gyro[0]; + gyroRaw[1] = states.gyro[1]; + gyroRaw[2] = states.gyro[2]; + } + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true); + FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); + + // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself + + // at this point sensor state is stored in "states" with some rudimentary filtering applied + + // apply all filters in the current filter chain + current = filterChain; + + // we are not done, re-dispatch self execution + + while (current) { + filterResult result = current->filter->filter((stateFilter *)current->filter, &states); + if (result > alarm) { + alarm = result; } + current = current->next; + } - // throttle alarms, raise alarm flags immediately - // but require system to run for a while before decreasing - // to prevent alarm flapping - if (alarm >= lastAlarm) { + // the final output of filters is saved in state variables + // EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut + if (IS_SET(states.updated, SENSORUPDATES_gyro)) { + gyroDelta[0] = states.gyro[0] - gyroRaw[0]; + gyroDelta[1] = states.gyro[1] - gyroRaw[1]; + gyroDelta[2] = states.gyro[2] - gyroRaw[2]; + } + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z); + if (IS_SET(states.updated, SENSORUPDATES_mag)) { + MagStateData s; + + MagStateGet(&s); + s.x = states.mag[0]; + s.y = states.mag[1]; + s.z = states.mag[2]; + switch (states.magStatus) { + case MAGSTATUS_OK: + s.Source = MAGSTATE_SOURCE_ONBOARD; + break; + case MAGSTATUS_AUX: + s.Source = MAGSTATE_SOURCE_AUX; + break; + default: + s.Source = MAGSTATE_SOURCE_INVALID; + } + MagStateSet(&s); + } + + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down); + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down); + EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed); + // attitude nees manual conversion from quaternion to euler + if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \ + AttitudeStateData s; + AttitudeStateGet(&s); + s.q1 = states.attitude[0]; + s.q2 = states.attitude[1]; + s.q3 = states.attitude[2]; + s.q4 = states.attitude[3]; + Quaternion2RPY(&s.q1, &s.Roll); + AttitudeStateSet(&s); + } + // throttle alarms, raise alarm flags immediately + // but require system to run for a while before decreasing + // to prevent alarm flapping + if (alarm >= lastAlarm) { + lastAlarm = alarm; + alarmcounter = 0; + } else { + if (alarmcounter < 100) { + alarmcounter++; + } else { lastAlarm = alarm; alarmcounter = 0; - } else { - if (alarmcounter < 100) { - alarmcounter++; - } else { - lastAlarm = alarm; - alarmcounter = 0; - } } + } - // clear alarms if everything is alright, then schedule callback execution after timeout - if (lastAlarm == FILTERRESULT_WARNING) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - } else if (lastAlarm == FILTERRESULT_CRITICAL) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else if (lastAlarm >= FILTERRESULT_ERROR) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } + // clear alarms if everything is alright, then schedule callback execution after timeout + if (lastAlarm == FILTERRESULT_WARNING) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); + } else if (lastAlarm == FILTERRESULT_CRITICAL) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + } else if (lastAlarm >= FILTERRESULT_ERROR) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } - // we are done, re-schedule next self execution - runState = RUNSTATE_LOAD; - if (updatedSensors) { - PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback); - } else { - PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER); - } - break; + if (updatedSensors) { + PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback); + } else { + PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER); } } diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 142c0a2ad..86d565891 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -130,8 +130,10 @@ int32_t TelemetryStart(void) PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); #ifdef PIOS_INCLUDE_RFM22B - xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle); + if (radioPort) { + xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle); + } #endif return 0; @@ -432,14 +434,12 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) if (inputPort) { // Block until data are available - uint8_t serial_data[1]; + uint8_t serial_data[16]; uint16_t bytes_to_process; bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500); if (bytes_to_process > 0) { - for (uint8_t i = 0; i < bytes_to_process; i++) { - UAVTalkProcessInputStream(uavTalkCon, serial_data[i]); - } + UAVTalkProcessInputStream(uavTalkCon, serial_data, bytes_to_process); } } else { vTaskDelay(5); @@ -457,14 +457,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters) while (1) { if (radioPort) { // Block until data are available - uint8_t serial_data[1]; + uint8_t serial_data[16]; uint16_t bytes_to_process; bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500); if (bytes_to_process > 0) { - for (uint8_t i = 0; i < bytes_to_process; i++) { - UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]); - } + UAVTalkProcessInputStream(radioUavTalkCon, serial_data, bytes_to_process); } } else { vTaskDelay(5); diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index f10632415..6a9c70879 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -42,7 +42,9 @@ #ifndef PIOS_MS5611_SLOW_TEMP_RATE #define PIOS_MS5611_SLOW_TEMP_RATE 1 #endif - +// Running moving average smoothing factor +#define PIOS_MS5611_TEMP_SMOOTHING 10 +// /* Local Types */ typedef struct { uint16_t C[6]; @@ -72,6 +74,7 @@ static uint32_t RawTemperature; static uint32_t RawPressure; static int64_t Pressure; static int64_t Temperature; +static int64_t FilteredTemperature = INT32_MIN; static int32_t lastConversionStart; static uint32_t conversionDelayMs; @@ -247,6 +250,12 @@ int32_t PIOS_MS5611_ReadADC(void) // Actual temperature (-40…85°C with 0.01°C resolution) // TEMP = 20°C + dT * TEMPSENS = 2000 + dT * C6 / 2^23 Temperature = 2000l + ((deltaTemp * CalibData.C[5]) / POW2(23)); + if (FilteredTemperature != INT32_MIN) { + FilteredTemperature = (FilteredTemperature * (PIOS_MS5611_TEMP_SMOOTHING - 1) + + Temperature) / PIOS_MS5611_TEMP_SMOOTHING; + } else { + FilteredTemperature = Temperature; + } } else { int64_t Offset; int64_t Sens; @@ -259,21 +268,21 @@ int32_t PIOS_MS5611_ReadADC(void) return -2; } // check if temperature is less than 20°C - if (Temperature < 2000) { + if (FilteredTemperature < 2000) { // Apply compensation // T2 = dT^2 / 2^31 // OFF2 = 5 ⋅ (TEMP – 2000)^2/2 // SENS2 = 5 ⋅ (TEMP – 2000)^2/2^2 - int64_t tcorr = (Temperature - 2000) * (Temperature - 2000); + int64_t tcorr = (FilteredTemperature - 2000) * (FilteredTemperature - 2000); Offset2 = (5 * tcorr) / 2; Sens2 = (5 * tcorr) / 4; compensation_t2 = (deltaTemp * deltaTemp) >> 31; // Apply the "Very low temperature compensation" when temp is less than -15°C - if (Temperature < -1500) { + if (FilteredTemperature < -1500) { // OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2 // SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2 - int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500); + int64_t tcorr2 = (FilteredTemperature + 1500) * (FilteredTemperature + 1500); Offset2 += 7 * tcorr2; Sens2 += (11 * tcorr2) / 2; } @@ -302,7 +311,7 @@ int32_t PIOS_MS5611_ReadADC(void) static float PIOS_MS5611_GetTemperature(void) { // Apply the second order low and very low temperature compensation offset - return ((float)(Temperature - compensation_t2)) / 100.0f; + return ((float)(FilteredTemperature - compensation_t2)) / 100.0f; } /** diff --git a/flight/pios/inc/pios_posix.h b/flight/pios/inc/pios_posix.h index 74483b404..aaf3cdc95 100644 --- a/flight/pios/inc/pios_posix.h +++ b/flight/pios/inc/pios_posix.h @@ -28,7 +28,9 @@ #include +#ifndef __cplusplus typedef enum { FALSE = 0, TRUE = !FALSE } bool; +#endif #ifndef false #define false FALSE diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index 9f4007db8..b53518358 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -19,6 +19,7 @@ # These are the UAVObjects supposed to be build as part of the OpenPilot target # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusvtolland UAVOBJSRCFILENAMES += vtolselftuningstats UAVOBJSRCFILENAMES += accelgyrosettings UAVOBJSRCFILENAMES += accessorydesired @@ -61,6 +62,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats UAVOBJSRCFILENAMES += manualcontrolcommand diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 9f4007db8..6d3dee2f4 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -19,6 +19,9 @@ # These are the UAVObjects supposed to be build as part of the OpenPilot target # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland UAVOBJSRCFILENAMES += vtolselftuningstats UAVOBJSRCFILENAMES += accelgyrosettings UAVOBJSRCFILENAMES += accessorydesired @@ -61,6 +64,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats UAVOBJSRCFILENAMES += manualcontrolcommand diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 987266d1f..2196842e6 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -19,6 +19,9 @@ # These are the UAVObjects supposed to be build as part of the OpenPilot target # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland UAVOBJSRCFILENAMES += vtolselftuningstats UAVOBJSRCFILENAMES += accelgyrosettings UAVOBJSRCFILENAMES += accessorydesired @@ -61,6 +64,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats UAVOBJSRCFILENAMES += manualcontrolcommand diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 2417a5127..70673a563 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -20,6 +20,9 @@ # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA ##### +# REVO C++ support +USE_CXX = YES + override ARM_SDK_PREFIX := override THUMB := @@ -83,10 +86,11 @@ endif ## MODULES SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}} +CPPSRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.cpp}} SRC += ${OUTDIR}/InitMods.c ## OPENPILOT CORE: SRC += ${OPMODULEDIR}/System/systemmod.c -SRC += $(OPSYSTEM)/simposix.c +CPPSRC += $(OPSYSTEM)/simposix.cpp SRC += $(OPSYSTEM)/pios_board.c SRC += $(FLIGHTLIB)/alarms.c SRC += $(OPUAVTALK)/uavtalk.c @@ -124,6 +128,7 @@ SRC += $(PIOSCORECOMMON)/pios_mem.c ## PIOS Hardware include $(PIOS)/posix/library.mk + include ./UAVObjects.inc SRC += $(UAVOBJSRC) @@ -315,7 +320,7 @@ endif # Link: create ELF output file from object files. -$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ))) +$(eval $(call LINK_CXX_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ))) # Assemble: create object files from assembler source files. $(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src)))) @@ -330,7 +335,7 @@ $(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src)))) $(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src)))) # Compile: create object files from C++ source files. -$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src)))) +$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CXX_TEMPLATE, $(src)))) # Compile: create object files from C++ source files. ARM-only $(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src)))) diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index ab588c123..95641a61e 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -24,6 +24,9 @@ # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland UAVOBJSRCFILENAMES += vtolselftuningstats UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand @@ -62,6 +65,7 @@ UAVOBJSRCFILENAMES += gpstime UAVOBJSRCFILENAMES += gpsvelocitysensor UAVOBJSRCFILENAMES += gpssettings UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus UAVOBJSRCFILENAMES += homelocation diff --git a/flight/targets/boards/simposix/firmware/simposix.c b/flight/targets/boards/simposix/firmware/simposix.cpp similarity index 99% rename from flight/targets/boards/simposix/firmware/simposix.c rename to flight/targets/boards/simposix/firmware/simposix.cpp index 411d9a79a..69e53de79 100644 --- a/flight/targets/boards/simposix/firmware/simposix.c +++ b/flight/targets/boards/simposix/firmware/simposix.cpp @@ -31,6 +31,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +extern "C" { #include "inc/openpilot.h" #include #include @@ -74,6 +75,7 @@ static void initTask(void *parameters); /* Prototype of generated InitModules() function */ extern void InitModules(void); +} /** * OpenPilot Main function: diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index f66eb4a00..e485882ef 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -57,8 +57,8 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection); int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); -UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte); -UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte); +UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length); +UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length, uint8_t *position); int32_t UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle); int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle); void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats, bool reset); diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 5d618e046..aeb580b13 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -53,7 +53,15 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj); static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data); static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId); - +// UavTalk Process FSM functions +static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position); +static bool UAVTalkProcess_TYPE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position); +static bool UAVTalkProcess_OBJID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position); +static bool UAVTalkProcess_INSTID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position); +static bool UAVTalkProcess_SIZE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position); +static bool UAVTalkProcess_TIMESTAMP(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position); +static bool UAVTalkProcess_DATA(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position); +static bool UAVTalkProcess_CS(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position); /** * Initialize the UAVTalk library * \param[in] connection UAVTalkConnection to be used @@ -350,10 +358,12 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type /** * Process an byte from the telemetry stream. * \param[in] connectionHandle UAVTalkConnection to be used - * \param[in] rxbyte Received byte + * \param[in] rxbuffer Received buffer + * \param[in/out] Length in bytes of received buffer + * \param[in/out] position Next item to be read inside rxbuffer * \return UAVTalkRxState */ -UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte) +UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length, uint8_t *position) { UAVTalkConnectionData *connection; @@ -361,231 +371,83 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle UAVTalkInputProcessor *iproc = &connection->iproc; - ++connection->stats.rxBytes; - if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) { iproc->state = UAVTALK_STATE_SYNC; } - if (iproc->rxPacketLength < 0xffff) { - // update packet byte count - iproc->rxPacketLength++; - } + uint8_t processedBytes = (*position); + uint8_t count = 0; - // Receive state machine - switch (iproc->state) { - case UAVTALK_STATE_SYNC: - - if (rxbyte != UAVTALK_SYNC_VAL) { - connection->stats.rxSyncErrors++; + // stop processing as soon as a complete packet is received, error is encountered or buffer is processed entirely + while ((count = length - (*position)) > 0 + && iproc->state != UAVTALK_STATE_COMPLETE + && iproc->state != UAVTALK_STATE_ERROR) { + // Receive state machine + if (iproc->state == UAVTALK_STATE_SYNC && + !UAVTalkProcess_SYNC(connection, iproc, rxbuffer, length, position)) { break; } - // Initialize and update the CRC - iproc->cs = PIOS_CRC_updateByte(0, rxbyte); - - iproc->rxPacketLength = 1; - iproc->rxCount = 0; - - iproc->type = 0; - iproc->state = UAVTALK_STATE_TYPE; - break; - - case UAVTALK_STATE_TYPE: - - if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) { - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_SYNC; + if (iproc->state == UAVTALK_STATE_TYPE && + !UAVTalkProcess_TYPE(connection, iproc, rxbuffer, length, position)) { break; } - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - iproc->type = rxbyte; - - iproc->packet_size = 0; - iproc->state = UAVTALK_STATE_SIZE; - break; - - case UAVTALK_STATE_SIZE: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - if (iproc->rxCount == 0) { - iproc->packet_size += rxbyte; - iproc->rxCount++; - break; - } - iproc->packet_size += rxbyte << 8; - iproc->rxCount = 0; - - if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { - // incorrect packet size - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; + if (iproc->state == UAVTALK_STATE_SIZE && + !UAVTalkProcess_SIZE(connection, iproc, rxbuffer, length, position)) { break; } - iproc->objId = 0; - iproc->state = UAVTALK_STATE_OBJID; - break; - - case UAVTALK_STATE_OBJID: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - iproc->objId += rxbyte << (8 * (iproc->rxCount++)); - if (iproc->rxCount < 4) { - break; - } - iproc->rxCount = 0; - - iproc->instId = 0; - iproc->state = UAVTALK_STATE_INSTID; - break; - - case UAVTALK_STATE_INSTID: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - iproc->instId += rxbyte << (8 * (iproc->rxCount++)); - if (iproc->rxCount < 2) { - break; - } - iproc->rxCount = 0; - - UAVObjHandle obj = UAVObjGetByID(iproc->objId); - - // Determine data length - if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) { - iproc->length = 0; - iproc->timestampLength = 0; - } else { - iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; - if (obj) { - iproc->length = UAVObjGetNumBytes(obj); - } else { - iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength; - } - } - - // Check length - if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { - // packet error - exceeded payload max length - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; + if (iproc->state == UAVTALK_STATE_OBJID && + !UAVTalkProcess_OBJID(connection, iproc, rxbuffer, length, position)) { break; } - // Check the lengths match - if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { - // packet error - mismatched packet size - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; + if (iproc->state == UAVTALK_STATE_INSTID && + !UAVTalkProcess_INSTID(connection, iproc, rxbuffer, length, position)) { break; } - // Determine next state - if (iproc->type & UAVTALK_TIMESTAMPED) { - // If there is a timestamp get it - iproc->timestamp = 0; - iproc->state = UAVTALK_STATE_TIMESTAMP; - } else { - // If there is a payload get it, otherwise receive checksum - if (iproc->length > 0) { - iproc->state = UAVTALK_STATE_DATA; - } else { - iproc->state = UAVTALK_STATE_CS; - } - } - break; - - case UAVTALK_STATE_TIMESTAMP: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - iproc->timestamp += rxbyte << (8 * (iproc->rxCount++)); - if (iproc->rxCount < 2) { - break; - } - iproc->rxCount = 0; - - // If there is a payload get it, otherwise receive checksum - if (iproc->length > 0) { - iproc->state = UAVTALK_STATE_DATA; - } else { - iproc->state = UAVTALK_STATE_CS; - } - break; - - case UAVTALK_STATE_DATA: - - // update the CRC - iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - - connection->rxBuffer[iproc->rxCount++] = rxbyte; - if (iproc->rxCount < iproc->length) { - break; - } - iproc->rxCount = 0; - - iproc->state = UAVTALK_STATE_CS; - break; - - case UAVTALK_STATE_CS: - - // Check the CRC byte - if (rxbyte != iproc->cs) { - // packet error - faulty CRC - UAVT_DEBUGLOG_PRINTF("BAD CRC"); - connection->stats.rxCrcErrors++; - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; + if (iproc->state == UAVTALK_STATE_TIMESTAMP && + !UAVTalkProcess_TIMESTAMP(connection, iproc, rxbuffer, length, position)) { break; } - if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) { - // packet error - mismatched packet size - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_ERROR; + if (iproc->state == UAVTALK_STATE_DATA && + !UAVTalkProcess_DATA(connection, iproc, rxbuffer, length, position)) { break; } - connection->stats.rxObjects++; - connection->stats.rxObjectBytes += iproc->length; - - iproc->state = UAVTALK_STATE_COMPLETE; - break; - - default: - - iproc->state = UAVTALK_STATE_ERROR; - break; + if (iproc->state == UAVTALK_STATE_CS && + !UAVTalkProcess_CS(connection, iproc, rxbuffer, length, position)) { + break; + } } // Done + processedBytes = (*position) - processedBytes; + connection->stats.rxBytes += processedBytes; return iproc->state; } /** - * Process an byte from the telemetry stream. + * Process a buffer from the telemetry stream. * \param[in] connection UAVTalkConnection to be used - * \param[in] rxbyte Received byte + * \param[in] rxbuffer Received buffer + * \param[in] count bytes inside rxbuffer * \return UAVTalkRxState */ -UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) +UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length) { - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); + uint8_t position = 0; + UAVTalkRxState state = UAVTALK_STATE_ERROR; - if (state == UAVTALK_STATE_COMPLETE) { - UAVTalkReceiveObject(connectionHandle); + while (position < length) { + state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbuffer, length, &position); + if (state == UAVTALK_STATE_COMPLETE) { + UAVTalkReceiveObject(connectionHandle); + } } - return state; } @@ -1000,6 +862,236 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, return 0; } +/* + * Functions that implements the UAVTalk Process FSM. return false to break out of current cycle + */ + +static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position) +{ + uint8_t rxbyte = rxbuffer[(*position)++]; + + if (rxbyte != UAVTALK_SYNC_VAL) { + connection->stats.rxSyncErrors++; + return false; + } + + // Initialize and update the CRC + iproc->cs = PIOS_CRC_updateByte(0, rxbyte); + + iproc->rxPacketLength = 1; + iproc->rxCount = 0; + + iproc->type = 0; + iproc->state = UAVTALK_STATE_TYPE; + return true; +} + +static bool UAVTalkProcess_TYPE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position) +{ + uint8_t rxbyte = rxbuffer[(*position)++]; + + if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) { + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_SYNC; + return false; + } + + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + + iproc->type = rxbyte; + iproc->rxPacketLength++; + iproc->packet_size = 0; + iproc->state = UAVTALK_STATE_SIZE; + return true; +} + +static bool UAVTalkProcess_SIZE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position) +{ + while (iproc->rxCount < 2 && length > (*position)) { + uint8_t rxbyte = rxbuffer[(*position)++]; + // update the CRC + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + iproc->packet_size += rxbyte << 8 * iproc->rxCount; + iproc->rxCount++; + } + + if (iproc->rxCount < 2) { + return false;; + } + + iproc->rxCount = 0; + + if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { + // incorrect packet size + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + return false; + } + iproc->rxPacketLength += 2; + iproc->objId = 0; + iproc->state = UAVTALK_STATE_OBJID; + return true; +} + +static bool UAVTalkProcess_OBJID(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position) +{ + while (iproc->rxCount < 4 && length > (*position)) { + uint8_t rxbyte = rxbuffer[(*position)++]; + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + iproc->objId += rxbyte << (8 * (iproc->rxCount++)); + } + + if (iproc->rxCount < 4) { + return false; + } + iproc->rxCount = 0; + iproc->rxPacketLength += 4; + iproc->instId = 0; + iproc->state = UAVTALK_STATE_INSTID; + return true; +} + +static bool UAVTalkProcess_INSTID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position) +{ + while (iproc->rxCount < 2 && length > (*position)) { + uint8_t rxbyte = rxbuffer[(*position)++]; + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + iproc->instId += rxbyte << (8 * (iproc->rxCount++)); + } + + if (iproc->rxCount < 2) { + return false; + } + iproc->rxPacketLength += 2; + iproc->rxCount = 0; + + UAVObjHandle obj = UAVObjGetByID(iproc->objId); + + // Determine data length + if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) { + iproc->length = 0; + iproc->timestampLength = 0; + } else { + iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0; + if (obj) { + iproc->length = UAVObjGetNumBytes(obj); + } else { + iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength; + } + } + + // Check length + if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { + // packet error - exceeded payload max length + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + return false; + } + + // Check the lengths match + if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { + // packet error - mismatched packet size + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + return false; + } + + // Determine next state + if (iproc->type & UAVTALK_TIMESTAMPED) { + // If there is a timestamp get it + iproc->timestamp = 0; + iproc->state = UAVTALK_STATE_TIMESTAMP; + } else { + // If there is a payload get it, otherwise receive checksum + if (iproc->length > 0) { + iproc->state = UAVTALK_STATE_DATA; + } else { + iproc->state = UAVTALK_STATE_CS; + } + } + return true; +} + +static bool UAVTalkProcess_TIMESTAMP(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position) +{ + while (iproc->rxCount < 2 && length > (*position)) { + uint8_t rxbyte = rxbuffer[(*position)++]; + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); + iproc->timestamp += rxbyte << (8 * (iproc->rxCount++)); + } + + if (iproc->rxCount < 2) { + return false;; + } + + iproc->rxCount = 0; + iproc->rxPacketLength += 2; + // If there is a payload get it, otherwise receive checksum + if (iproc->length > 0) { + iproc->state = UAVTALK_STATE_DATA; + } else { + iproc->state = UAVTALK_STATE_CS; + } + return true; +} + +static bool UAVTalkProcess_DATA(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position) +{ + uint8_t toCopy = iproc->length - iproc->rxCount; + + if (toCopy > length - (*position)) { + toCopy = length - (*position); + } + + memcpy(&connection->rxBuffer[iproc->rxCount], &rxbuffer[(*position)], toCopy); + (*position) += toCopy; + + // update the CRC + iproc->cs = PIOS_CRC_updateCRC(iproc->cs, &connection->rxBuffer[iproc->rxCount], toCopy); + iproc->rxCount += toCopy; + + iproc->rxPacketLength += toCopy; + + if (iproc->rxCount < iproc->length) { + return false; + } + + iproc->rxCount = 0; + iproc->state = UAVTALK_STATE_CS; + return true; +} + +static bool UAVTalkProcess_CS(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position) +{ + // Check the CRC byte + uint8_t rxbyte = rxbuffer[(*position)++]; + + if (rxbyte != iproc->cs) { + // packet error - faulty CRC + UAVT_DEBUGLOG_PRINTF("BAD CRC"); + connection->stats.rxCrcErrors++; + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + return false;; + } + iproc->rxPacketLength++; + + if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) { + // packet error - mismatched packet size + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_ERROR; + return false;; + } + + connection->stats.rxObjects++; + connection->stats.rxObjectBytes += iproc->length; + + iproc->state = UAVTALK_STATE_COMPLETE; + return true; +} + + /** * @} * @} diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp index 338dcc874..b82d27e33 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp @@ -116,7 +116,6 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) m_aircraft->gvThrottleCurve1GroupBox->setEnabled(true); m_aircraft->gvThrottleCurve2GroupBox->setEnabled(true); - // Default Curve2 range -1 -> +1, allow forward/reverse (Car and Tank) m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_PITCH); @@ -145,8 +144,8 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) m_aircraft->differentialSteeringSlider1->setEnabled(true); m_aircraft->differentialSteeringSlider2->setEnabled(true); - m_aircraft->gvThrottleCurve1GroupBox->setTitle(""); - m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve"); + m_aircraft->gvThrottleCurve1GroupBox->setTitle("Throttle curve1"); + m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve2 "); m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_PITCH); m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); @@ -154,13 +153,11 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) initMixerCurves(frameType); // If new setup, set sliders to defaults and set curves values - // Allow forward/reverse 0.8 / -0.8 for Throttle, keep some room - // to allow rotate at full throttle and heading stabilization if (frameTypeSaved->getValue().toString() != "GroundVehicleDifferential") { m_aircraft->differentialSteeringSlider1->setValue(100); m_aircraft->differentialSteeringSlider2->setValue(100); - m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0); - m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 0.8, -0.8); + m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0, 0.0); + m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0, 0.0); } } else if (frameType == "GroundVehicleMotorcycle" || frameType == "Motorcycle") { // Motorcycle @@ -179,12 +176,11 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) m_aircraft->gvSteering2Label->setText("Balancing"); // Curve1 for Motorcyle - m_aircraft->gvThrottleCurve1GroupBox->setTitle("Rear throttle curve"); + m_aircraft->gvThrottleCurve1GroupBox->setTitle("Throttle curve1"); m_aircraft->gvThrottleCurve1GroupBox->setEnabled(true); - m_aircraft->gvThrottleCurve2GroupBox->setTitle(""); - m_aircraft->gvThrottleCurve2GroupBox->setEnabled(false); + m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve2"); + m_aircraft->gvThrottleCurve2GroupBox->setEnabled(true); - // Curve range 0 -> +1 (no reverse) m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); @@ -192,8 +188,8 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) // If new setup, set curves values if (frameTypeSaved->getValue().toString() != "GroundVehicleMotorCycle") { - m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0); - m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0); + m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0, 0.0); + m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0, 0.0); } } else { // Car @@ -212,11 +208,10 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) m_aircraft->gvSteering1Label->setText("Front steering"); m_aircraft->gvSteering2Label->setText("Rear steering"); - // Curve2 for Car - m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve"); + m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve2"); m_aircraft->gvThrottleCurve2GroupBox->setEnabled(true); - m_aircraft->gvThrottleCurve1GroupBox->setTitle(""); - m_aircraft->gvThrottleCurve1GroupBox->setEnabled(false); + m_aircraft->gvThrottleCurve1GroupBox->setTitle("Throttle curve1"); + m_aircraft->gvThrottleCurve1GroupBox->setEnabled(true); m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_PITCH); m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); @@ -225,11 +220,8 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType) // If new setup, set curves values if (frameTypeSaved->getValue().toString() != "GroundVehicleCar") { - m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0); - // Set curve2 range from -0.926 to 1 (forward / reverse) - // Take in account 4% offset in Throttle input after calibration - // 0.5 / 0.54 = 0.926 - m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0, -0.926); + m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0, 0.0); + m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0, 0.0); } } @@ -325,7 +317,13 @@ void ConfigGroundVehicleWidget::initMixerCurves(QString frameType) m_aircraft->groundVehicleThrottle1->initCurve(&curveValues); } else { // no, init a straight curve - m_aircraft->groundVehicleThrottle1->initLinearCurve(curveValues.count(), 1.0); + if (frameType == "GroundVehicleDifferential") { + m_aircraft->groundVehicleThrottle1->initLinearCurve(curveValues.count(), 0.8, 0.0); + } else if (frameType == "GroundVehicleCar") { + m_aircraft->groundVehicleThrottle1->initLinearCurve(curveValues.count(), 1.0, 0.0); + } else { + m_aircraft->groundVehicleThrottle1->initLinearCurve(curveValues.count(), 1.0, 0.0); + } } // Setup all Throttle2 curves for all types of airframes @@ -336,11 +334,11 @@ void ConfigGroundVehicleWidget::initMixerCurves(QString frameType) } else { // no, init a straight curve if (frameType == "GroundVehicleDifferential") { - m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 0.8, -0.8); + m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 0.8, 0.0); } else if (frameType == "GroundVehicleCar") { - m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 1.0, -1.0); + m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 1.0, 0.0); } else { - m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 1.0); + m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 1.0, 0.0); } } } @@ -513,11 +511,11 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType) channel = m_aircraft->gvMotor1ChannelBox->currentIndex() - 1; setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, 127); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); channel = m_aircraft->gvMotor2ChannelBox->currentIndex() - 1; setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, 127); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); // Output success message m_aircraft->gvStatusLabel->setText("Mixer generated"); diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp index 981aeb5b7..55949a9e4 100644 --- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp @@ -138,7 +138,7 @@ void MagicWaypointGadgetWidget::positionSelected(double north, double east) pathDesired.End[PathDesired::END_NORTH] = north * scale; pathDesired.End[PathDesired::END_EAST] = east * scale; - pathDesired.Mode = PathDesired::MODE_FLYENDPOINT; + pathDesired.Mode = PathDesired::MODE_GOTOENDPOINT; getPathDesired()->setData(pathDesired); } diff --git a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp index ee0444023..46a7f6b28 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp @@ -86,18 +86,17 @@ void modelMapProxy::selectedWPChanged(QList list) modelMapProxy::overlayType modelMapProxy::overlayTranslate(int type) { switch (type) { - case MapDataDelegate::MODE_FLYENDPOINT: - case MapDataDelegate::MODE_FLYVECTOR: - case MapDataDelegate::MODE_DRIVEENDPOINT: - case MapDataDelegate::MODE_DRIVEVECTOR: + case MapDataDelegate::MODE_GOTOENDPOINT: + case MapDataDelegate::MODE_FOLLOWVECTOR: + case MapDataDelegate::MODE_VELOCITY: + case MapDataDelegate::MODE_LAND: + case MapDataDelegate::MODE_BRAKE: return OVERLAY_LINE; - case MapDataDelegate::MODE_FLYCIRCLERIGHT: - case MapDataDelegate::MODE_DRIVECIRCLERIGHT: + case MapDataDelegate::MODE_CIRCLERIGHT: return OVERLAY_CIRCLE_RIGHT; - case MapDataDelegate::MODE_FLYCIRCLELEFT: - case MapDataDelegate::MODE_DRIVECIRCLELEFT: + case MapDataDelegate::MODE_CIRCLELEFT: default: return OVERLAY_CIRCLE_LEFT; } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp index 89024da92..606d73545 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp @@ -104,15 +104,13 @@ void opmap_edit_waypoint_dialog::setupModeWidgets() MapDataDelegate::ModeOptions mode = (MapDataDelegate::ModeOptions)ui->cbMode->itemData(ui->cbMode->currentIndex()).toInt(); switch (mode) { - case MapDataDelegate::MODE_FLYENDPOINT: - case MapDataDelegate::MODE_FLYVECTOR: - case MapDataDelegate::MODE_FLYCIRCLERIGHT: - case MapDataDelegate::MODE_FLYCIRCLELEFT: - case MapDataDelegate::MODE_DRIVEENDPOINT: - case MapDataDelegate::MODE_DRIVEVECTOR: - case MapDataDelegate::MODE_DRIVECIRCLELEFT: - case MapDataDelegate::MODE_DRIVECIRCLERIGHT: + case MapDataDelegate::MODE_GOTOENDPOINT: + case MapDataDelegate::MODE_FOLLOWVECTOR: + case MapDataDelegate::MODE_CIRCLERIGHT: + case MapDataDelegate::MODE_CIRCLELEFT: case MapDataDelegate::MODE_DISARMALARM: + case MapDataDelegate::MODE_LAND: + case MapDataDelegate::MODE_BRAKE: ui->modeParam1->setVisible(false); ui->modeParam2->setVisible(false); ui->modeParam3->setVisible(false); @@ -122,6 +120,16 @@ void opmap_edit_waypoint_dialog::setupModeWidgets() ui->dsb_modeParam3->setVisible(false); ui->dsb_modeParam4->setVisible(false); break; + case MapDataDelegate::MODE_VELOCITY: + ui->modeParam1->setVisible(true); + ui->modeParam2->setVisible(true); + ui->modeParam3->setVisible(true); + ui->modeParam4->setVisible(false); + ui->dsb_modeParam1->setVisible(true); + ui->dsb_modeParam2->setVisible(true); + ui->dsb_modeParam3->setVisible(true); + ui->dsb_modeParam4->setVisible(false); + break; case MapDataDelegate::MODE_FIXEDATTITUDE: ui->modeParam1->setText("pitch"); ui->modeParam2->setText("roll"); diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp index 49fbca8b8..ec95a944b 100644 --- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp @@ -106,19 +106,17 @@ void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDa { switch (type) { case flightDataModel::MODE: - combo->addItem("Fly Direct", MODE_FLYENDPOINT); - combo->addItem("Fly Vector", MODE_FLYVECTOR); - combo->addItem("Fly Circle Right", MODE_FLYCIRCLERIGHT); - combo->addItem("Fly Circle Left", MODE_FLYCIRCLELEFT); - - combo->addItem("Drive Direct", MODE_DRIVEENDPOINT); - combo->addItem("Drive Vector", MODE_DRIVEVECTOR); - combo->addItem("Drive Circle Right", MODE_DRIVECIRCLELEFT); - combo->addItem("Drive Circle Left", MODE_DRIVECIRCLERIGHT); - + combo->addItem("Goto Endpoint", MODE_GOTOENDPOINT); + combo->addItem("Follow Vector", MODE_FOLLOWVECTOR); + combo->addItem("Circle Right", MODE_CIRCLERIGHT); + combo->addItem("Circle Left", MODE_CIRCLELEFT); combo->addItem("Fixed Attitude", MODE_FIXEDATTITUDE); combo->addItem("Set Accessory", MODE_SETACCESSORY); combo->addItem("Disarm Alarm", MODE_DISARMALARM); + combo->addItem("Land", MODE_LAND); + combo->addItem("Brake", MODE_BRAKE); + combo->addItem("Velocity", MODE_VELOCITY); + break; case flightDataModel::CONDITION: combo->addItem("None", ENDCONDITION_NONE); diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h index 2b824c979..6b9af7c06 100644 --- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h +++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h @@ -36,9 +36,10 @@ class MapDataDelegate : public QItemDelegate { Q_OBJECT public: - typedef enum { MODE_FLYENDPOINT = 0, MODE_FLYVECTOR = 1, MODE_FLYCIRCLERIGHT = 2, MODE_FLYCIRCLELEFT = 3, - MODE_DRIVEENDPOINT = 4, MODE_DRIVEVECTOR = 5, MODE_DRIVECIRCLELEFT = 6, MODE_DRIVECIRCLERIGHT = 7, - MODE_FIXEDATTITUDE = 8, MODE_SETACCESSORY = 9, MODE_DISARMALARM = 10 } ModeOptions; + typedef enum { MODE_GOTOENDPOINT = 0, MODE_FOLLOWVECTOR = 1, MODE_CIRCLERIGHT = 2, MODE_CIRCLELEFT = 3, + MODE_FIXEDATTITUDE = 8, MODE_SETACCESSORY = 9, MODE_DISARMALARM = 10, MODE_LAND = 11, + MODE_BRAKE = 12, MODE_VELOCITY = 13 } ModeOptions; + typedef enum { ENDCONDITION_NONE = 0, ENDCONDITION_TIMEOUT = 1, ENDCONDITION_DISTANCETOTARGET = 2, ENDCONDITION_LEGREMAINING = 3, ENDCONDITION_BELOWERROR = 4, ENDCONDITION_ABOVEALTITUDE = 5, ENDCONDITION_ABOVESPEED = 6, ENDCONDITION_POINTINGTOWARDSNEXT = 7, ENDCONDITION_PYTHONSCRIPT = 8, diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index ec02a7d6c..f87689b32 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -779,17 +779,15 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch mSettings->setMixerValueRoll(100); mSettings->setMixerValuePitch(100); mSettings->setMixerValueYaw(100); - // Set curve2 range from -0.926 to 1 : take in account 4% offset in Throttle input - // 0.5 / 0.54 = 0.926 maxThrottle = 1; - minThrottle = -0.926; + minThrottle = 0; break; case VehicleConfigurationSource::GROUNDVEHICLE_DIFFERENTIAL: mSettings->setMixerValueRoll(100); mSettings->setMixerValuePitch(100); mSettings->setMixerValueYaw(100); maxThrottle = 0.8; - minThrottle = -0.8; + minThrottle = 0; break; default: break; @@ -2076,8 +2074,8 @@ void VehicleConfigurationHelper::setupCar() // Motor (Chan 2) channels[1].type = MIXER_TYPE_REVERSABLEMOTOR; - channels[1].throttle1 = 0; - channels[1].throttle2 = 100; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; channels[1].roll = 0; channels[1].pitch = 0; channels[1].yaw = 0; @@ -2101,16 +2099,16 @@ void VehicleConfigurationHelper::setupTank() // Left Motor (Chan 1) channels[0].type = MIXER_TYPE_REVERSABLEMOTOR; - channels[0].throttle1 = 0; - channels[0].throttle2 = 100; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; channels[0].roll = 0; channels[0].pitch = 0; channels[0].yaw = 100; // Right Motor (Chan 2) channels[1].type = MIXER_TYPE_REVERSABLEMOTOR; - channels[1].throttle1 = 0; - channels[1].throttle2 = 100; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; channels[1].roll = 0; channels[1].pitch = 0; channels[1].yaw = -100; @@ -2140,7 +2138,7 @@ void VehicleConfigurationHelper::setupMotorcycle() channels[0].pitch = 0; channels[0].yaw = 100; - // Motor (Chan 2) : Curve1, no reverse + // Motor (Chan 2) channels[1].type = MIXER_TYPE_MOTOR; channels[1].throttle1 = 100; channels[1].throttle2 = 0; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 94225fc81..c079b3282 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -27,6 +27,9 @@ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files HEADERS += \ + $$UAVOBJECT_SYNTHETICS/statusgrounddrive.h \ + $$UAVOBJECT_SYNTHETICS/pidstatus.h \ + $$UAVOBJECT_SYNTHETICS/statusvtolland.h \ $$UAVOBJECT_SYNTHETICS/vtolselftuningstats.h \ $$UAVOBJECT_SYNTHETICS/accelgyrosettings.h \ $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ @@ -96,6 +99,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.h \ $$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.h \ + $$UAVOBJECT_SYNTHETICS/groundpathfollowersettings.h \ $$UAVOBJECT_SYNTHETICS/ratedesired.h \ $$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \ $$UAVOBJECT_SYNTHETICS/i2cstats.h \ @@ -133,6 +137,9 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/perfcounter.h SOURCES += \ + $$UAVOBJECT_SYNTHETICS/statusgrounddrive.cpp \ + $$UAVOBJECT_SYNTHETICS/pidstatus.cpp \ + $$UAVOBJECT_SYNTHETICS/statusvtolland.cpp \ $$UAVOBJECT_SYNTHETICS/vtolselftuningstats.cpp \ $$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \ $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ @@ -202,6 +209,7 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.cpp \ $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.cpp \ $$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.cpp \ + $$UAVOBJECT_SYNTHETICS/groundpathfollowersettings.cpp \ $$UAVOBJECT_SYNTHETICS/ratedesired.cpp \ $$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \ $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \ diff --git a/hardware/Production/OpenPilot Atom/Assembly.OutJob b/hardware/Production/OpenPilot Atom/Assembly.OutJob new file mode 100644 index 000000000..11b7f13c3 --- /dev/null +++ b/hardware/Production/OpenPilot Atom/Assembly.OutJob @@ -0,0 +1,176 @@ +[OutputJobFile] +Version=1.0 + +[OutputGroup1] +Name=Assembly.OutJob +Description= +TargetOutputMedium=PDF +VariantName=[No Variations] +VariantScope=1 +CurrentConfigurationName= +TargetPrinter=Brother HL-2170W +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintWhat=1 +OutputMedium1=Print Job +OutputMedium1_Type=Printer +OutputMedium1_Printer= +OutputMedium1_PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintWhat=1 +OutputMedium2=PDF +OutputMedium2_Type=Publish +OutputMedium3=Folder Structure +OutputMedium3_Type=GeneratedFiles +OutputMedium4=Video +OutputMedium4_Type=Multimedia +OutputType1=Assembly +OutputName1=Assembly Drawings +OutputCategory1=Assembly +OutputDocumentPath1= +OutputVariantName1= +OutputEnabled1=1 +OutputEnabled1_OutputMedium1=0 +OutputEnabled1_OutputMedium2=1 +OutputEnabled1_OutputMedium3=0 +OutputEnabled1_OutputMedium4=0 +OutputDefault1=0 +PageOptions1=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=9.24|XCorrection=1.00|YCorrection=1.00|PrintKind=0|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PaperKind=A4|PrintScaleMode=1 +Configuration1_Name1=OutputConfigurationParameter1 +Configuration1_Item1=DesignatorDisplayMode=Physical|PrintArea=DesignExtent|PrintAreaLowerLeftCornerX=0|PrintAreaLowerLeftCornerY=0|PrintAreaUpperRightCornerX=0|PrintAreaUpperRightCornerY=0|Record=PcbPrintView +Configuration1_Name2=OutputConfigurationParameter2 +Configuration1_Item2=IncludeBottomLayerComponents=False|IncludeMultiLayerComponents=True|IncludeTopLayerComponents=True|Index=0|Mirror=False|Name=Top Assembly Drawing|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=True +Configuration1_Name3=OutputConfigurationParameter3 +Configuration1_Item3=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical6|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name4=OutputConfigurationParameter4 +Configuration1_Item4=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Hidden|FFill=Hidden|FPad=Hidden|FRegion=Hidden|FText=Hidden|FTrack=Hidden|FVia=Hidden|Layer=TopLayer|Polygon=Hidden|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name5=OutputConfigurationParameter5 +Configuration1_Item5=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=TopOverlay|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name6=OutputConfigurationParameter6 +Configuration1_Item6=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Hidden|Layer=MultiLayer|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name7=OutputConfigurationParameter7 +Configuration1_Item7=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical1|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name8=OutputConfigurationParameter8 +Configuration1_Item8=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical5|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name9=OutputConfigurationParameter9 +Configuration1_Item9=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical13|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name10=OutputConfigurationParameter10 +Configuration1_Item10=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical14|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name11=OutputConfigurationParameter11 +Configuration1_Item11=IncludeBottomLayerComponents=True|IncludeMultiLayerComponents=True|IncludeTopLayerComponents=False|Index=1|Mirror=True|Name=Bottom Assembly Drawing|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=True +Configuration1_Name12=OutputConfigurationParameter12 +Configuration1_Item12=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical7|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration1_Name13=OutputConfigurationParameter13 +Configuration1_Item13=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Hidden|FFill=Hidden|FPad=Hidden|FRegion=Hidden|FText=Hidden|FTrack=Hidden|FVia=Hidden|Layer=BottomLayer|Polygon=Hidden|PrintOutIndex=1|Record=PcbPrintLayer +Configuration1_Name14=OutputConfigurationParameter14 +Configuration1_Item14=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=BottomOverlay|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration1_Name15=OutputConfigurationParameter15 +Configuration1_Item15=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Hidden|Layer=MultiLayer|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration1_Name16=OutputConfigurationParameter16 +Configuration1_Item16=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical1|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration1_Name17=OutputConfigurationParameter17 +Configuration1_Item17=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical5|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration1_Name18=OutputConfigurationParameter18 +Configuration1_Item18=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical13|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration1_Name19=OutputConfigurationParameter19 +Configuration1_Item19=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical14|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration1_Name20=OutputConfigurationParameter20 +Configuration1_Item20=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical16|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +OutputType2=PCB 3D Print +OutputName2=PCB 3D Top +OutputCategory2=Documentation +OutputDocumentPath2= +OutputVariantName2= +OutputEnabled2=1 +OutputEnabled2_OutputMedium1=0 +OutputEnabled2_OutputMedium2=2 +OutputEnabled2_OutputMedium3=0 +OutputEnabled2_OutputMedium4=0 +OutputDefault2=0 +PageOptions2=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=9.57|XCorrection=1.00|YCorrection=1.00|PrintKind=0|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PaperKind=A4|PrintScaleMode=1 +Configuration2_Name1=OutputConfigurationParameter1 +Configuration2_Item1=Record=Pcb3DPrintView|ResX=300|ResY=300|ViewX=14173229|ViewY=14173230|LookAtX=31403926|LookAtY=26533924|LookAtZ=-1000|QuatX=0|QuatY=0|QuatZ=0|QuatW=1|Zoom=4.90361053902321E-5|UnitsPercent=True|UnitsDPI=True|LockResAspect=True|ViewConfigType=.config_3d|CustomCamera=False|ViewFromTop=True|ViewConfig=RECORD\3Board\2CFGALL.CONFIGURATIONKIND\33\2CFGALL.CONFIGURATIONDESC\3Altium%203D%20NB%20Black%20Configuration\2CFG3D.POSITIVETOPSOLDERMASK\3TRUE\2CFG3D.POSITIVEBOTTOMSOLDERMASK\3TRUE\2CFG3D.SHOWCOMPONENTBODIES\3SYSTEM\2CFG3D.SHOWCOMPONENTSTEPMODELS\3SYSTEM\2CFG3D.COMPONENTMODELPREFERENCE\30\2CFG3D.SHOWCOMPONENTSNAPMARKERS\3TRUE\2CFG3D.SHOWCOMPONENTAXES\3TRUE\2CFG3D.SHOWBOARDCORE\3TRUE\2CFG3D.SHOWBOARDPREPREG\3TRUE\2CFG3D.SHOWTOPSILKSCREEN\3TRUE\2CFG3D.SHOWBOTSILKSCREEN\3TRUE\2CFG3D.SHOWORIGINMARKER\3TRUE\2CFG3D.EYEDIST\32000\2CFG3D.SHOWCUTOUTS\3TRUE\2CFG3D.SHOWROUTETOOLPATH\3TRUE\2CFG3D.SHOWROOMS3D\3FALSE\2CFG3D.USESYSCOLORSFOR3D\3FALSE\2CFG3D.WORKSPACECOLOR\311840160\2CFG3D.BOARDCORECOLOR\313491161\2CFG3D.BOARDPREPREGCOLOR\30\2CFG3D.TOPSOLDERMASKCOLOR\30\2CFG3D.BOTSOLDERMASKCOLOR\30\2CFG3D.COPPERCOLOR\33323360\2CFG3D.TOPSILKSCREENCOLOR\315461355\2CFG3D.BOTSILKSCREENCOLOR\315461355\2CFG3D.WORKSPACELUMINANCEVARIATION\330\2CFG3D.WORKSPACECOLOROPACITY\31.000000\2CFG3D.BOARDCORECOLOROPACITY\30.820000\2CFG3D.BOARDPREPREGCOLOROPACITY\30.500000\2CFG3D.TOPSOLDERMASKCOLOROPACITY\30.900000\2CFG3D.BOTSOLDERMASKCOLOROPACITY\30.900000\2CFG3D.COPPERCOLOROPACITY\31.000000\2CFG3D.TOPSILKSCREENCOLOROPACITY\31.000000\2CFG3D.BOTSILKSCREENCOLOROPACITY\31.000000\2CFG3D.BOARDTHICKNESSSCALING\31.000000\2CFG3D.SHOWMECHANICALLAYERS\3FALSE\2CFG3D.MECHANICALLAYERSOPACITY\31.000000 +OutputType3=PCB 3D Print +OutputName3=PCB 3D Bottom +OutputCategory3=Documentation +OutputDocumentPath3= +OutputVariantName3= +OutputEnabled3=1 +OutputEnabled3_OutputMedium1=0 +OutputEnabled3_OutputMedium2=3 +OutputEnabled3_OutputMedium3=0 +OutputEnabled3_OutputMedium4=0 +OutputDefault3=0 +PageOptions3=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=9.57|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PaperKind=A4|PrintScaleMode=1 +Configuration3_Name1=OutputConfigurationParameter1 +Configuration3_Item1=Record=Pcb3DPrintView|ResX=300|ResY=300|ViewX=14173229|ViewY=14173230|LookAtX=31403926|LookAtY=26533924|LookAtZ=-1000|QuatX=0|QuatY=0|QuatZ=0|QuatW=1|Zoom=4.90361053902321E-5|UnitsPercent=True|UnitsDPI=True|LockResAspect=True|ViewConfigType=.config_3d|CustomCamera=False|ViewFromTop=False|ViewConfig=RECORD\3Board\2CFGALL.CONFIGURATIONKIND\33\2CFGALL.CONFIGURATIONDESC\3Altium%203D%20NB%20Black%20Configuration\2CFG3D.POSITIVETOPSOLDERMASK\3TRUE\2CFG3D.POSITIVEBOTTOMSOLDERMASK\3TRUE\2CFG3D.SHOWCOMPONENTBODIES\3SYSTEM\2CFG3D.SHOWCOMPONENTSTEPMODELS\3SYSTEM\2CFG3D.COMPONENTMODELPREFERENCE\30\2CFG3D.SHOWCOMPONENTSNAPMARKERS\3TRUE\2CFG3D.SHOWCOMPONENTAXES\3TRUE\2CFG3D.SHOWBOARDCORE\3TRUE\2CFG3D.SHOWBOARDPREPREG\3TRUE\2CFG3D.SHOWTOPSILKSCREEN\3TRUE\2CFG3D.SHOWBOTSILKSCREEN\3TRUE\2CFG3D.SHOWORIGINMARKER\3TRUE\2CFG3D.EYEDIST\32000\2CFG3D.SHOWCUTOUTS\3TRUE\2CFG3D.SHOWROUTETOOLPATH\3TRUE\2CFG3D.SHOWROOMS3D\3FALSE\2CFG3D.USESYSCOLORSFOR3D\3FALSE\2CFG3D.WORKSPACECOLOR\311840160\2CFG3D.BOARDCORECOLOR\313491161\2CFG3D.BOARDPREPREGCOLOR\30\2CFG3D.TOPSOLDERMASKCOLOR\30\2CFG3D.BOTSOLDERMASKCOLOR\30\2CFG3D.COPPERCOLOR\33323360\2CFG3D.TOPSILKSCREENCOLOR\315461355\2CFG3D.BOTSILKSCREENCOLOR\315461355\2CFG3D.WORKSPACELUMINANCEVARIATION\330\2CFG3D.WORKSPACECOLOROPACITY\31.000000\2CFG3D.BOARDCORECOLOROPACITY\30.820000\2CFG3D.BOARDPREPREGCOLOROPACITY\30.500000\2CFG3D.TOPSOLDERMASKCOLOROPACITY\30.900000\2CFG3D.BOTSOLDERMASKCOLOROPACITY\30.900000\2CFG3D.COPPERCOLOROPACITY\31.000000\2CFG3D.TOPSILKSCREENCOLOROPACITY\31.000000\2CFG3D.BOTSILKSCREENCOLOROPACITY\31.000000\2CFG3D.BOARDTHICKNESSSCALING\31.000000\2CFG3D.SHOWMECHANICALLAYERS\3FALSE\2CFG3D.MECHANICALLAYERSOPACITY\31.000000 + +[PublishSettings] +OutputFilePath2=C:\Users\David\Documents\OP-WIP\trunk\hardware\production\OpenPilot Atom\Project Outputs for OpenPilot Atom\..\Assembly\OpenPilot Atom Assembly.PDF +ReleaseManaged2=1 +OutputBasePath2=Project Outputs for OpenPilot Atom +OutputPathMedia2=..\Assembly +OutputPathOutputer2=[Output Type] +OutputFileName2=OpenPilot Atom Assembly.PDF +OpenOutput2=1 +PromptOverwrite2=1 +PublishMethod2=0 +ZoomLevel2=50 +FitSCHPrintSizeToDoc2=1 +FitPCBPrintSizeToDoc2=1 +GenerateNetsInfo2=1 +MarkPins2=1 +MarkNetLabels2=1 +MarkPortsId2=1 +GenerateTOC=1 +OutputFilePath3=C:\Users\David\Documents\OP-WIP\trunk\hardware\production\OpenPilot Atom\Project Outputs for OpenPilot Atom\ +ReleaseManaged3=1 +OutputBasePath3=Project Outputs for OpenPilot Atom +OutputPathMedia3= +OutputPathOutputer3=[Output Type] +OutputFileName3= +OpenOutput3=1 +OutputFilePath4=C:\Users\David\Documents\OP-WIP\trunk\hardware\production\OpenPilot Atom\Project Outputs for OpenPilot Atom\ +ReleaseManaged4=1 +OutputBasePath4=Project Outputs for OpenPilot Atom +OutputPathMedia4= +OutputPathOutputer4=[Output Type] +OutputFileName4= +OpenOutput4=1 +PromptOverwrite4=1 +PublishMethod4=5 +ZoomLevel4=50 +FitSCHPrintSizeToDoc4=1 +FitPCBPrintSizeToDoc4=1 +GenerateNetsInfo4=1 +MarkPins4=1 +MarkNetLabels4=1 +MarkPortsId4=1 +MediaFormat4=Windows Media file (*.wmv,*.wma,*.asf) +FixedDimensions4=1 +Width4=352 +Height4=288 +MultiFile4=0 +FramesPerSecond4=25 +FramesPerSecondDenom4=1 +AviPixelFormat4=7 +AviCompression4=MP42 MS-MPEG4 V2 +AviQuality4=100 +FFmpegVideoCodecId4=13 +FFmpegPixelFormat4=0 +FFmpegQuality4=80 +WmvVideoCodecName4=Windows Media Video V7 +WmvQuality4=80 + +[GeneratedFilesSettings] +RelativeOutputPath2=C:\Users\David\Documents\OP-WIP\trunk\hardware\production\OpenPilot Atom\Project Outputs for OpenPilot Atom\..\Assembly\OpenPilot Atom Assembly.PDF +OpenOutputs2=1 +RelativeOutputPath3=C:\Users\David\Documents\OP-WIP\trunk\hardware\production\OpenPilot Atom\Project Outputs for OpenPilot Atom\ +OpenOutputs3=1 +AddToProject3=1 +TimestampFolder3=0 +UseOutputName3=0 +OpenODBOutput3=0 +OpenGerberOutput3=0 +OpenNCDrillOutput3=0 +OpenIPCOutput3=0 +EnableReload3=0 +RelativeOutputPath4=C:\Users\David\Documents\OP-WIP\trunk\hardware\production\OpenPilot Atom\Project Outputs for OpenPilot Atom\ +OpenOutputs4=1 + diff --git a/hardware/Production/OpenPilot Atom/Assembly/OpenPilot Atom Assembly.PDF b/hardware/Production/OpenPilot Atom/Assembly/OpenPilot Atom Assembly.PDF new file mode 100644 index 000000000..ed281f084 Binary files /dev/null and b/hardware/Production/OpenPilot Atom/Assembly/OpenPilot Atom Assembly.PDF differ diff --git a/hardware/Production/OpenPilot Atom/Assembly/OpenPilot Atom Step.zip b/hardware/Production/OpenPilot Atom/Assembly/OpenPilot Atom Step.zip new file mode 100644 index 000000000..ebcfbc2e0 Binary files /dev/null and b/hardware/Production/OpenPilot Atom/Assembly/OpenPilot Atom Step.zip differ diff --git a/hardware/Production/OpenPilot Atom/Assembly/OpenPilot Atom Texture.PDF b/hardware/Production/OpenPilot Atom/Assembly/OpenPilot Atom Texture.PDF new file mode 100644 index 000000000..30f12df43 Binary files /dev/null and b/hardware/Production/OpenPilot Atom/Assembly/OpenPilot Atom Texture.PDF differ diff --git a/hardware/Production/OpenPilot Atom/BOM/OpenPilot Atom.xls b/hardware/Production/OpenPilot Atom/BOM/OpenPilot Atom.xls new file mode 100644 index 000000000..2e8e8a8cd Binary files /dev/null and b/hardware/Production/OpenPilot Atom/BOM/OpenPilot Atom.xls differ diff --git a/hardware/Production/OpenPilot Atom/Gerbers/OpenPilot Atom Gerbers.zip b/hardware/Production/OpenPilot Atom/Gerbers/OpenPilot Atom Gerbers.zip new file mode 100644 index 000000000..f14679d59 Binary files /dev/null and b/hardware/Production/OpenPilot Atom/Gerbers/OpenPilot Atom Gerbers.zip differ diff --git a/hardware/Production/OpenPilot Atom/OpenPilot Atom.PcbDoc b/hardware/Production/OpenPilot Atom/OpenPilot Atom.PcbDoc new file mode 100644 index 000000000..6e82b11ed Binary files /dev/null and b/hardware/Production/OpenPilot Atom/OpenPilot Atom.PcbDoc differ diff --git a/hardware/Production/OpenPilot Atom/OpenPilot Atom.PrjPCB b/hardware/Production/OpenPilot Atom/OpenPilot Atom.PrjPCB new file mode 100644 index 000000000..6b5920089 --- /dev/null +++ b/hardware/Production/OpenPilot Atom/OpenPilot Atom.PrjPCB @@ -0,0 +1,1160 @@ +[Design] +Version=1.0 +HierarchyMode=0 +ChannelRoomNamingStyle=0 +ReleasesFolder= +ReleaseVaultGUID= +ReleaseVaultName= +ChannelDesignatorFormatString=$Component_$RoomName +ChannelRoomLevelSeperator=_ +OpenOutputs=1 +ArchiveProject=0 +TimestampOutput=0 +SeparateFolders=0 +TemplateLocationPath= +PinSwapBy_Netlabel=1 +PinSwapBy_Pin=1 +AllowPortNetNames=0 +AllowSheetEntryNetNames=1 +AppendSheetNumberToLocalNets=0 +NetlistSinglePinNets=0 +DefaultConfiguration=Default Configuration +UserID=0xFFFFFFFF +DefaultPcbProtel=1 +DefaultPcbPcad=0 +ReorderDocumentsOnCompile=1 +NameNetsHierarchically=0 +PowerPortNamesTakePriority=0 +PushECOToAnnotationFile=1 +DItemRevisionGUID= +ReportSuppressedErrorsInMessages=0 +OutputPath= +LogFolderPath= +ManagedProjectGUID= + +[Preferences] +PrefsVaultGUID= +PrefsRevisionGUID= + +[Document1] +DocumentPath=OpenPilot Atom.SchDoc +AnnotationEnabled=1 +AnnotateStartValue=1 +AnnotationIndexControlEnabled=0 +AnnotateSuffix= +AnnotateScope=All +AnnotateOrder=0 +DoLibraryUpdate=1 +DoDatabaseUpdate=1 +ClassGenCCAutoEnabled=1 +ClassGenCCAutoRoomEnabled=0 +ClassGenNCAutoScope=None +DItemRevisionGUID= +GenerateClassCluster=0 +DocumentUniqueId=FWUXSAGW + +[Document2] +DocumentPath=OpenPilot Atom.PcbDoc +AnnotationEnabled=1 +AnnotateStartValue=1 +AnnotationIndexControlEnabled=0 +AnnotateSuffix= +AnnotateScope=All +AnnotateOrder=-1 +DoLibraryUpdate=1 +DoDatabaseUpdate=1 +ClassGenCCAutoEnabled=1 +ClassGenCCAutoRoomEnabled=1 +ClassGenNCAutoScope=None +DItemRevisionGUID= +GenerateClassCluster=0 +DocumentUniqueId=CPDAMJJA + +[Document3] +DocumentPath=Assembly.OutJob +AnnotationEnabled=1 +AnnotateStartValue=1 +AnnotationIndexControlEnabled=0 +AnnotateSuffix= +AnnotateScope=All +AnnotateOrder=-1 +DoLibraryUpdate=1 +DoDatabaseUpdate=1 +ClassGenCCAutoEnabled=1 +ClassGenCCAutoRoomEnabled=1 +ClassGenNCAutoScope=None +DItemRevisionGUID= +GenerateClassCluster=0 +DocumentUniqueId= + +[SearchPath1] +Path=..\..\..\..\Program Files (x86)\Altium Designer Summer 09\Library\*.* +IncludeSubFolders=1 + +[Configuration1] +Name=Default Configuration +ParameterCount=0 +ConstraintFileCount=0 +ReleaseItemId= +CurrentRevision= +Variant=[No Variations] +GenerateBOM=0 +OutputJobsCount=0 + +[Generic_SmartPDF] +AutoOpenFile=-1 +AutoOpenOutJob=-1 + +[Generic_SmartPDFSettings] +ProjectMode=0 +ZoomPrecision=50 +AddNetsInformation=-1 +AddNetPins=-1 +AddNetNetLabels=-1 +AddNetPorts=-1 +ExportBOM=0 +TemplateFilename= +TemplateStoreRelative=-1 +PCB_PrintColor=0 +SCH_PrintColor=0 +SCH_ShowNoErc=0 +SCH_ShowParameter=0 +SCH_ShowProbes=0 +SCH_ShowBlankets=0 +OutputFileName=CopterControl.SchDoc=C:\Users\David\Documents\SVN\WIP\trunk\hardware\production\CopterControl\CopterControl Schematic.pdf +SCH_ExpandLogicalToPhysical=0 +SCH_VariantName=[No Variations] +SCH_ExpandComponentDesignators=-1 +SCH_ExpandNetlabels=0 +SCH_ExpandPorts=0 +SCH_ExpandSheetNumber=0 +SCH_ExpandDocumentNumber=0 +SCH_HasExpandLogicalToPhysicalSheets=-1 +SaveSettingsToOutJob=0 +SCH_NoERCSymbolsToShow="Thin Cross","Thick Cross","Small Cross",Checkbox,Triangle +SCH_ShowNote=-1 +SCH_ShowNoteCollapsed=-1 + +[Generic_EDE] +OutputDir= + +[OutputGroup1] +Name=Netlist Outputs +Description= +TargetPrinter=Microsoft XPS Document Writer +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintJobKind=1|PrintWhat=1 +OutputType1=SIMetrixNetlist +OutputName1=SIMetrix +OutputDocumentPath1= +OutputVariantName1= +OutputDefault1=0 +OutputType2=SIMPLISNetlist +OutputName2=SIMPLIS +OutputDocumentPath2= +OutputVariantName2= +OutputDefault2=0 +OutputType3=XSpiceNetlist +OutputName3=XSpice Netlist +OutputDocumentPath3= +OutputVariantName3= +OutputDefault3=0 +OutputType4=PCADNetlist +OutputName4=PCAD Netlist +OutputDocumentPath4= +OutputVariantName4= +OutputDefault4=0 +OutputType5=Verilog +OutputName5=Verilog File +OutputDocumentPath5= +OutputVariantName5= +OutputDefault5=0 +OutputType6=VHDL +OutputName6=VHDL File +OutputDocumentPath6= +OutputVariantName6= +OutputDefault6=0 + +[OutputGroup2] +Name=Simulator Outputs +Description= +TargetPrinter=Microsoft XPS Document Writer +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintJobKind=1|PrintWhat=1 +OutputType1=AdvSimNetlist +OutputName1=Mixed Sim +OutputDocumentPath1= +OutputVariantName1= +OutputDefault1=0 +OutputType2=SIMetrixSimulation +OutputName2=SIMetrix +OutputDocumentPath2= +OutputVariantName2= +OutputDefault2=0 +OutputType3=SIMPLISSimulation +OutputName3=SIMPLIS +OutputDocumentPath3= +OutputVariantName3= +OutputDefault3=0 + +[OutputGroup3] +Name=Documentation Outputs +Description= +TargetPrinter=Virtual Printer +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintJobKind=1|PrintWhat=1 +OutputType1=Composite +OutputName1=Composite Drawing +OutputDocumentPath1=C:\Users\David\Documents\SVN\OP-WIP\trunk\hardware\production\CopterControl\CopterControl.PcbDoc +OutputVariantName1= +OutputDefault1=0 +PageOptions1=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=0|PaperKind=A4 +Configuration1_Name1=OutputConfigurationParameter1 +Configuration1_Item1=PrintArea=DesignExtent|PrintAreaLowerLeftCornerX=0|PrintAreaLowerLeftCornerY=0|PrintAreaUpperRightCornerX=0|PrintAreaUpperRightCornerY=0|Record=PcbPrintView +Configuration1_Name2=OutputConfigurationParameter2 +Configuration1_Item2=IncludeBottomLayerComponents=True|IncludeMultiLayerComponents=True|IncludeTopLayerComponents=True|Index=0|Mirror=False|Name=Multilayer Composite Print|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration1_Name3=OutputConfigurationParameter3 +Configuration1_Item3=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=TopOverlay|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name4=OutputConfigurationParameter4 +Configuration1_Item4=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=BottomOverlay|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name5=OutputConfigurationParameter5 +Configuration1_Item5=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=TopLayer|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name6=OutputConfigurationParameter6 +Configuration1_Item6=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=MidLayer1|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name7=OutputConfigurationParameter7 +Configuration1_Item7=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=BottomLayer|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name8=OutputConfigurationParameter8 +Configuration1_Item8=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical1|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name9=OutputConfigurationParameter9 +Configuration1_Item9=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical5|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name10=OutputConfigurationParameter10 +Configuration1_Item10=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical6|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name11=OutputConfigurationParameter11 +Configuration1_Item11=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical7|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name12=OutputConfigurationParameter12 +Configuration1_Item12=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical13|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name13=OutputConfigurationParameter13 +Configuration1_Item13=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical15|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration1_Name14=OutputConfigurationParameter14 +Configuration1_Item14=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=MultiLayer|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +OutputType2=PCB 3D Print +OutputName2=PCB 3D Prints +OutputDocumentPath2= +OutputVariantName2=[No Variations] +OutputDefault2=0 +PageOptions2=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType3=PCB Print +OutputName3=PCB Prints +OutputDocumentPath3= +OutputVariantName3= +OutputDefault3=0 +PageOptions3=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType4=Schematic Print +OutputName4=Schematic Prints +OutputDocumentPath4= +OutputVariantName4= +OutputDefault4=0 +PageOptions4=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType5=SimView Print +OutputName5=SimView Prints +OutputDocumentPath5= +OutputVariantName5= +OutputDefault5=0 +PageOptions5=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType6=Wave Print +OutputName6=Wave Prints +OutputDocumentPath6= +OutputVariantName6= +OutputDefault6=0 +PageOptions6=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType7=WaveSim Print +OutputName7=WaveSim Prints +OutputDocumentPath7= +OutputVariantName7= +OutputDefault7=0 +PageOptions7=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType8=PCB 3D Video +OutputName8=PCB 3D Video +OutputDocumentPath8= +OutputVariantName8=[No Variations] +OutputDefault8=0 +PageOptions8=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType9=Report Print +OutputName9=Report Prints +OutputDocumentPath9= +OutputVariantName9= +OutputDefault9=0 +PageOptions9=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType10=PCBLIB Print +OutputName10=PCBLIB Prints +OutputDocumentPath10= +OutputVariantName10= +OutputDefault10=0 +PageOptions10=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType11=Assembler Source Print +OutputName11=Assembler Source Prints +OutputDocumentPath11= +OutputVariantName11= +OutputDefault11=0 +PageOptions11=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType12=C Source Print +OutputName12=C Source Prints +OutputDocumentPath12= +OutputVariantName12= +OutputDefault12=0 +PageOptions12=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType13=C/C++ Header Print +OutputName13=C/C++ Header Prints +OutputDocumentPath13= +OutputVariantName13= +OutputDefault13=0 +PageOptions13=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType14=C++ Source Print +OutputName14=C++ Source Prints +OutputDocumentPath14= +OutputVariantName14= +OutputDefault14=0 +PageOptions14=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType15=FSM Print +OutputName15=FSM Prints +OutputDocumentPath15= +OutputVariantName15= +OutputDefault15=0 +PageOptions15=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType16=OpenBus Print +OutputName16=OpenBus Prints +OutputDocumentPath16= +OutputVariantName16= +OutputDefault16=0 +PageOptions16=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType17=VHDL Print +OutputName17=VHDL Prints +OutputDocumentPath17= +OutputVariantName17= +OutputDefault17=0 +PageOptions17=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 + +[OutputGroup4] +Name=Assembly Outputs +Description= +TargetPrinter=Microsoft XPS Document Writer +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintJobKind=1|PrintWhat=1 +OutputType1=Test Points For Assembly +OutputName1=Test Point Report +OutputDocumentPath1= +OutputVariantName1=[No Variations] +OutputDefault1=0 +OutputType2=Pick Place +OutputName2=Generates pick and place files +OutputDocumentPath2= +OutputVariantName2=[No Variations] +OutputDefault2=0 +Configuration2_Name1=OutputConfigurationParameter1 +Configuration2_Item1=Record=PickPlaceView|Units=Imperial|GenerateCSVFormat=True|GenerateTextFormat=False +OutputType3=Assembly +OutputName3=Assembly Drawings +OutputDocumentPath3= +OutputVariantName3=[No Variations] +OutputDefault3=0 +PageOptions3=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 + +[OutputGroup5] +Name=Fabrication Outputs +Description= +TargetPrinter=Microsoft XPS Document Writer +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintJobKind=1|PrintWhat=1 +OutputType1=ODB +OutputName1=ODB++ Files +OutputDocumentPath1= +OutputVariantName1=[No Variations] +OutputDefault1=0 +OutputType2=NC Drill +OutputName2=NC Drill Files +OutputDocumentPath2= +OutputVariantName2= +OutputDefault2=0 +Configuration2_Name1=OutputConfigurationParameter1 +Configuration2_Item1=BoardEdgeRoutToolDia=2000000|GenerateBoardEdgeRout=False|GenerateDrilledSlotsG85=False|GenerateEIADrillFile=False|GenerateSeparatePlatedNonPlatedFiles=False|NumberOfDecimals=5|NumberOfUnits=2|OptimizeChangeLocationCommands=True|OriginPosition=Relative|Record=DrillView|Units=Imperial|ZeroesMode=SuppressTrailingZeroes +OutputType3=Plane +OutputName3=Power-Plane Prints +OutputDocumentPath3= +OutputVariantName3= +OutputDefault3=0 +PageOptions3=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType4=Mask +OutputName4=Solder/Paste Mask Prints +OutputDocumentPath4= +OutputVariantName4= +OutputDefault4=0 +PageOptions4=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType5=Drill +OutputName5=Drill Drawing/Guides +OutputDocumentPath5= +OutputVariantName5= +OutputDefault5=0 +PageOptions5=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=9.20|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +Configuration5_Name1=OutputConfigurationParameter1 +Configuration5_Item1=PrintArea=DesignExtent|PrintAreaLowerLeftCornerX=0|PrintAreaLowerLeftCornerY=0|PrintAreaUpperRightCornerX=0|PrintAreaUpperRightCornerY=0|Record=PcbPrintView +Configuration5_Name2=OutputConfigurationParameter2 +Configuration5_Item2=IncludeBottomLayerComponents=True|IncludeMultiLayerComponents=True|IncludeTopLayerComponents=True|Index=0|Mirror=False|Name=Drill Drawing For (Bottom Layer,Top Layer)|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration5_Name3=OutputConfigurationParameter3 +Configuration5_Item3=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=BottomLayer|DLayer2=TopLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=DrillDrawing|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration5_Name4=OutputConfigurationParameter4 +Configuration5_Item4=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical1|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration5_Name5=OutputConfigurationParameter5 +Configuration5_Item5=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical5|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration5_Name6=OutputConfigurationParameter6 +Configuration5_Item6=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical6|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration5_Name7=OutputConfigurationParameter7 +Configuration5_Item7=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical7|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration5_Name8=OutputConfigurationParameter8 +Configuration5_Item8=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical13|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration5_Name9=OutputConfigurationParameter9 +Configuration5_Item9=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical15|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration5_Name10=OutputConfigurationParameter10 +Configuration5_Item10=IncludeBottomLayerComponents=True|IncludeMultiLayerComponents=True|IncludeTopLayerComponents=True|Index=1|Mirror=False|Name=Drill Guide For (Bottom Layer,Top Layer)|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration5_Name11=OutputConfigurationParameter11 +Configuration5_Item11=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=BottomLayer|DLayer2=TopLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=DrillGuide|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration5_Name12=OutputConfigurationParameter12 +Configuration5_Item12=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical1|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration5_Name13=OutputConfigurationParameter13 +Configuration5_Item13=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical5|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration5_Name14=OutputConfigurationParameter14 +Configuration5_Item14=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical6|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration5_Name15=OutputConfigurationParameter15 +Configuration5_Item15=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical7|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration5_Name16=OutputConfigurationParameter16 +Configuration5_Item16=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical13|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration5_Name17=OutputConfigurationParameter17 +Configuration5_Item17=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical15|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +OutputType6=CompositeDrill +OutputName6=Composite Drill Drawing +OutputDocumentPath6= +OutputVariantName6= +OutputDefault6=0 +PageOptions6=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType7=Final +OutputName7=Final Artwork Prints +OutputDocumentPath7= +OutputVariantName7=[No Variations] +OutputDefault7=0 +PageOptions7=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType8=Test Points +OutputName8=Test Point Report +OutputDocumentPath8= +OutputVariantName8= +OutputDefault8=0 +OutputType9=Gerber +OutputName9=Gerber Files +OutputDocumentPath9= +OutputVariantName9=[No Variations] +OutputDefault9=0 +Configuration9_Name1=OutputConfigurationParameter1 +Configuration9_Item1=AddToAllPlots.Set=SerializeLayerHash.Version~2,ClassName~TLayerToBoolean|CentrePlots=False|DrillDrawingSymbol=GraphicsSymbol|DrillDrawingSymbolSize=500000|EmbeddedApertures=True|FilmBorderSize=10000000|FilmXSize=200000000|FilmYSize=160000000|FlashAllFills=False|FlashPadShapes=True|G54OnApertureChange=False|GenerateDRCRulesFile=True|GenerateReliefShapes=True|GerberUnit=Imperial|IncludeUnconnectedMidLayerPads=False|LeadingAndTrailingZeroesMode=SuppressLeadingZeroes|MaxApertureSize=2500000|MinusApertureTolerance=50|Mirror.Set=SerializeLayerHash.Version~2,ClassName~TLayerToBoolean|MirrorDrillDrawingPlots=False|MirrorDrillGuidePlots=False|NumberOfDecimals=5|OptimizeChangeLocationCommands=True|OriginPosition=Relative|Panelize=False|Plot.Set=SerializeLayerHash.Version~2,ClassName~TLayerToBoolean,16973830~1,16973832~1,16973834~1,16777217~1,16777218~1,16777219~1,16842751~1,16973835~1,16973833~1,16973831~1,16908294~1,16908295~1,16973837~1,16973848~1,16973849~1|PlotPositivePlaneLayers=False|PlotUsedDrillDrawingLayerPairs=True|PlotUsedDrillGuideLayerPairs=True|PlusApertureTolerance=50|Record=GerberView|SoftwareArcs=False|Sorted=False +OutputType10=Board Stack Report +OutputName10=Report Board Stack +OutputDocumentPath10= +OutputVariantName10= +OutputDefault10=0 +PageOptions10=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType11=Gerber X2 +OutputName11=Gerber X2 Files +OutputDocumentPath11= +OutputVariantName11= +OutputDefault11=0 +OutputType12=IPC2581 +OutputName12=IPC-2581 Files +OutputDocumentPath12= +OutputVariantName12= +OutputDefault12=0 + +[OutputGroup6] +Name=Report Outputs +Description= +TargetPrinter=Microsoft XPS Document Writer +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintJobKind=1|PrintWhat=1 +OutputType1=ComponentCrossReference +OutputName1=Component Cross Reference Report +OutputDocumentPath1= +OutputVariantName1=[No Variations] +OutputDefault1=0 +OutputType2=ReportHierarchy +OutputName2=Report Project Hierarchy +OutputDocumentPath2= +OutputVariantName2=[No Variations] +OutputDefault2=0 +OutputType3=SinglePinNetReporter +OutputName3=Report Single Pin Nets +OutputDocumentPath3= +OutputVariantName3=[No Variations] +OutputDefault3=0 +OutputType4=SimpleBOM +OutputName4=Simple BOM +OutputDocumentPath4= +OutputVariantName4=[No Variations] +OutputDefault4=0 +OutputType5=Script +OutputName5=Script Output +OutputDocumentPath5= +OutputVariantName5=[No Variations] +OutputDefault5=0 +OutputType6=BOM_PartType +OutputName6=Bill of Materials +OutputDocumentPath6= +OutputVariantName6=[No Variations] +OutputDefault6=0 +PageOptions6=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +Configuration6_Name1=Filter +Configuration6_Item1=545046300E5446696C74657257726170706572000D46696C7465722E416374697665090F46696C7465722E43726974657269610A04000000000000000000 +Configuration6_Name2=General +Configuration6_Item2=OpenExported=True|AddToProject=False|ForceFit=False|NotFitted=False|Database=False|IncludePCBData=False|IncludeVaultData=False|ShowExportOptions=True|TemplateFilename=..\Altium\BOM OpenPilot.xlt|BatchMode=5|FormWidth=1401|FormHeight=754|SupplierProdQty=1|SupplierAutoQty=True|SupplierUseCachedPricing=True|SupplierCurrency= +Configuration6_Name3=GroupOrder +Configuration6_Item3=Supplier Part Number 1=True +Configuration6_Name4=OutputConfigurationParameter1 +Configuration6_Item4=Record=BOMPrintView|ShowNoERC=True|ShowParamSet=True|ShowProbe=True|ShowBlanket=True|ExpandDesignator=True|ExpandNetLabel=False|ExpandPort=False|ExpandSheetNum=False|ExpandDocNum=False +Configuration6_Name5=PCBDocument +Configuration6_Item5= +Configuration6_Name6=SortOrder +Configuration6_Item6=Description=Up +Configuration6_Name7=VisibleOrder +Configuration6_Item7=Description=113|Comment=77|Footprint=59|Value=40|Designator=56|Quantity=37|Supplier Part Number 1=174|Supplier Order Qty 1=30|Supplier Stock 1=38|Supplier Unit Price 1=29|Supplier Subtotal 1=30|Supplier 1=23|Manufacturer 1=32|Manufacturer Part Number 1=20 + +[OutputGroup7] +Name=Other Outputs +Description= +TargetPrinter=Microsoft XPS Document Writer +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintJobKind=1|PrintWhat=1 +OutputType1=Text Print +OutputName1=Text Print +OutputDocumentPath1= +OutputVariantName1= +OutputDefault1=0 +PageOptions1=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType2=Text Print +OutputName2=Text Print +OutputDocumentPath2= +OutputVariantName2= +OutputDefault2=0 +PageOptions2=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType3=Text Print +OutputName3=Text Print +OutputDocumentPath3= +OutputVariantName3= +OutputDefault3=0 +PageOptions3=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType4=Text Print +OutputName4=Text Print +OutputDocumentPath4= +OutputVariantName4= +OutputDefault4=0 +PageOptions4=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType5=Text Print +OutputName5=Text Print +OutputDocumentPath5= +OutputVariantName5= +OutputDefault5=0 +PageOptions5=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType6=Text Print +OutputName6=Text Print +OutputDocumentPath6= +OutputVariantName6= +OutputDefault6=0 +PageOptions6=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType7=Text Print +OutputName7=Text Print +OutputDocumentPath7= +OutputVariantName7= +OutputDefault7=0 +PageOptions7=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType8=Text Print +OutputName8=Text Print +OutputDocumentPath8= +OutputVariantName8= +OutputDefault8=0 +PageOptions8=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType9=Text Print +OutputName9=Text Print +OutputDocumentPath9= +OutputVariantName9= +OutputDefault9=0 +PageOptions9=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType10=Text Print +OutputName10=Text Print +OutputDocumentPath10= +OutputVariantName10= +OutputDefault10=0 +PageOptions10=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType11=Text Print +OutputName11=Text Print +OutputDocumentPath11= +OutputVariantName11= +OutputDefault11=0 +PageOptions11=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType12=Text Print +OutputName12=Text Print +OutputDocumentPath12= +OutputVariantName12= +OutputDefault12=0 +PageOptions12=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType13=Text Print +OutputName13=Text Print +OutputDocumentPath13= +OutputVariantName13= +OutputDefault13=0 +PageOptions13=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType14=Text Print +OutputName14=Text Print +OutputDocumentPath14= +OutputVariantName14= +OutputDefault14=0 +PageOptions14=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType15=Text Print +OutputName15=Text Print +OutputDocumentPath15= +OutputVariantName15= +OutputDefault15=0 +PageOptions15=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType16=Text Print +OutputName16=Text Print +OutputDocumentPath16= +OutputVariantName16= +OutputDefault16=0 +PageOptions16=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType17=Text Print +OutputName17=Text Print +OutputDocumentPath17= +OutputVariantName17= +OutputDefault17=0 +PageOptions17=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType18=Text Print +OutputName18=Text Print +OutputDocumentPath18= +OutputVariantName18= +OutputDefault18=0 +PageOptions18=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType19=Text Print +OutputName19=Text Print +OutputDocumentPath19= +OutputVariantName19= +OutputDefault19=0 +PageOptions19=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType20=Text Print +OutputName20=Text Print +OutputDocumentPath20= +OutputVariantName20= +OutputDefault20=0 +PageOptions20=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType21=Text Print +OutputName21=Text Print +OutputDocumentPath21= +OutputVariantName21= +OutputDefault21=0 +PageOptions21=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType22=Text Print +OutputName22=Text Print +OutputDocumentPath22= +OutputVariantName22= +OutputDefault22=0 +PageOptions22=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType23=Text Print +OutputName23=Text Print +OutputDocumentPath23= +OutputVariantName23= +OutputDefault23=0 +PageOptions23=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType24=Text Print +OutputName24=Text Print +OutputDocumentPath24= +OutputVariantName24= +OutputDefault24=0 +PageOptions24=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType25=Text Print +OutputName25=Text Print +OutputDocumentPath25= +OutputVariantName25= +OutputDefault25=0 +PageOptions25=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType26=Text Print +OutputName26=Text Print +OutputDocumentPath26= +OutputVariantName26= +OutputDefault26=0 +PageOptions26=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType27=Text Print +OutputName27=Text Print +OutputDocumentPath27= +OutputVariantName27= +OutputDefault27=0 +PageOptions27=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType28=Text Print +OutputName28=Text Print +OutputDocumentPath28= +OutputVariantName28= +OutputDefault28=0 +PageOptions28=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 +OutputType29=Text Print +OutputName29=Text Print +OutputDocumentPath29= +OutputVariantName29= +OutputDefault29=0 +PageOptions29=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-3|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4|PaperIndex=9 + +[OutputGroup8] +Name=Validation Outputs +Description= +TargetPrinter=Microsoft XPS Document Writer +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintJobKind=1|PrintWhat=1 +OutputType1=Design Rules Check +OutputName1=Design Rules Check +OutputDocumentPath1= +OutputVariantName1= +OutputDefault1=0 +PageOptions1=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType2=Electrical Rules Check +OutputName2=Electrical Rules Check +OutputDocumentPath2= +OutputVariantName2= +OutputDefault2=0 +PageOptions2=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType3=Differences Report +OutputName3=Differences Report +OutputDocumentPath3= +OutputVariantName3= +OutputDefault3=0 +PageOptions3=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +OutputType4=Footprint Comparison Report +OutputName4=Footprint Comparison Report +OutputDocumentPath4= +OutputVariantName4= +OutputDefault4=0 +OutputType5=Configuration compliance +OutputName5=Environment configuration compliance check +OutputDocumentPath5= +OutputVariantName5= +OutputDefault5=0 + +[OutputGroup9] +Name=Export Outputs +Description= +TargetPrinter=Microsoft XPS Document Writer +PrinterOptions=Record=PrinterOptions|Copies=1|Duplex=1|TrueTypeOptions=3|Collate=1|PrintJobKind=1|PrintWhat=1 +OutputType1=ExportSTEP +OutputName1=Export STEP +OutputDocumentPath1= +OutputVariantName1=[No Variations] +OutputDefault1=0 +OutputType2=AutoCAD dwg/dxf PCB +OutputName2=AutoCAD dwg/dxf File PCB +OutputDocumentPath2= +OutputVariantName2= +OutputDefault2=0 +OutputType3=ExportIDF +OutputName3=Export IDF +OutputDocumentPath3= +OutputVariantName3= +OutputDefault3=0 +OutputType4=AutoCAD dwg/dxf Schematic +OutputName4=AutoCAD dwg/dxf File Schematic +OutputDocumentPath4= +OutputVariantName4= +OutputDefault4=0 + +[Modification Levels] +Type1=1 +Type2=1 +Type3=1 +Type4=1 +Type5=1 +Type6=1 +Type7=1 +Type8=1 +Type9=1 +Type10=1 +Type11=1 +Type12=1 +Type13=1 +Type14=1 +Type15=1 +Type16=1 +Type17=1 +Type18=1 +Type19=1 +Type20=1 +Type21=1 +Type22=1 +Type23=1 +Type24=1 +Type25=1 +Type26=1 +Type27=1 +Type28=1 +Type29=1 +Type30=1 +Type31=1 +Type32=1 +Type33=1 +Type34=1 +Type35=1 +Type36=1 +Type37=1 +Type38=1 +Type39=1 +Type40=1 +Type41=1 +Type42=1 +Type43=1 +Type44=1 +Type45=1 +Type46=1 +Type47=1 +Type48=1 +Type49=1 +Type50=1 +Type51=1 +Type52=1 +Type53=1 +Type54=1 +Type55=1 +Type56=1 +Type57=1 +Type58=1 +Type59=1 +Type60=1 +Type61=1 +Type62=1 +Type63=1 +Type64=1 +Type65=1 +Type66=1 +Type67=1 +Type68=1 +Type69=1 +Type70=1 +Type71=1 +Type72=1 +Type73=1 +Type74=1 +Type75=1 +Type76=1 +Type77=1 +Type78=1 + +[Difference Levels] +Type1=1 +Type2=1 +Type3=1 +Type4=1 +Type5=1 +Type6=1 +Type7=1 +Type8=1 +Type9=1 +Type10=1 +Type11=1 +Type12=1 +Type13=1 +Type14=1 +Type15=1 +Type16=1 +Type17=1 +Type18=1 +Type19=1 +Type20=1 +Type21=1 +Type22=1 +Type23=1 +Type24=1 +Type25=1 +Type26=1 +Type27=1 +Type28=1 +Type29=1 +Type30=1 +Type31=1 +Type32=1 +Type33=1 +Type34=1 +Type35=1 +Type36=1 +Type37=1 +Type38=1 +Type39=1 +Type40=1 +Type41=1 +Type42=1 +Type43=1 + +[Electrical Rules Check] +Type1=1 +Type2=1 +Type3=2 +Type4=1 +Type5=2 +Type6=2 +Type7=1 +Type8=1 +Type9=1 +Type10=1 +Type11=2 +Type12=2 +Type13=2 +Type14=1 +Type15=1 +Type16=1 +Type17=1 +Type18=1 +Type19=1 +Type20=1 +Type21=1 +Type22=1 +Type23=1 +Type24=1 +Type25=2 +Type26=2 +Type27=2 +Type28=1 +Type29=1 +Type30=1 +Type31=1 +Type32=2 +Type33=2 +Type34=2 +Type35=1 +Type36=2 +Type37=1 +Type38=2 +Type39=2 +Type40=2 +Type41=0 +Type42=2 +Type43=1 +Type44=1 +Type45=2 +Type46=1 +Type47=2 +Type48=2 +Type49=1 +Type50=2 +Type51=1 +Type52=1 +Type53=1 +Type54=1 +Type55=1 +Type56=2 +Type57=1 +Type58=1 +Type59=0 +Type60=1 +Type61=2 +Type62=2 +Type63=1 +Type64=0 +Type65=2 +Type66=3 +Type67=2 +Type68=2 +Type69=1 +Type70=2 +Type71=2 +Type72=2 +Type73=2 +Type74=1 +Type75=2 +Type76=1 +Type77=1 +Type78=1 +Type79=1 +Type80=2 +Type81=3 +Type82=3 +Type83=3 +Type84=3 +Type85=3 +Type86=2 +Type87=2 +Type88=2 +Type89=1 +Type90=1 +Type91=3 +Type92=3 +Type93=2 +Type94=2 +Type95=2 +Type96=2 +Type97=2 +Type98=0 +Type99=1 +Type100=2 +Type101=1 +Type102=2 +Type103=2 +Type104=1 +Type105=2 +Type106=2 +Type107=2 +Type108=2 +Type109=1 +Type110=0 + +[ERC Connection Matrix] +L1=NNNNNNNNNNNWNNNWW +L2=NNWNNNNWWWNWNWNWN +L3=NWEENEEEENEWNEEWN +L4=NNENNNWEENNWNENWN +L5=NNNNNNNNNNNNNNNNN +L6=NNENNNNEENNWNENWN +L7=NNEWNNWEENNWNENWN +L8=NWEENEENEEENNEENN +L9=NWEENEEEENEWNEEWW +L10=NWNNNNNENNEWNNEWN +L11=NNENNNNEEENWNENWN +L12=WWWWNWWNWWWNWWWNN +L13=NNNNNNNNNNNWNNNWW +L14=NWEENEEEENEWNEEWW +L15=NNENNNNEEENWNENWW +L16=WWWWNWWNWWWNWWWNW +L17=WNNNNNNNWNNNWWWWN + +[Annotate] +SortOrder=3 +SortLocation=0 +MatchParameter1=Comment +MatchStrictly1=1 +MatchParameter2=Library Reference +MatchStrictly2=1 +PhysicalNamingFormat=$Component_$RoomName +GlobalIndexSortOrder=3 +GlobalIndexSortLocation=0 + +[PrjClassGen] +CompClassManualEnabled=0 +CompClassManualRoomEnabled=0 +NetClassAutoBusEnabled=1 +NetClassAutoCompEnabled=0 +NetClassAutoNamedHarnessEnabled=0 +NetClassManualEnabled=1 + +[LibraryUpdateOptions] +SelectedOnly=0 +UpdateVariants=1 +PartTypes=0 +ComponentLibIdentifierKind0=Library Name And Type +ComponentLibraryIdentifier0=CopterControl.SchLib +ComponentDesignItemID0=CC-STM32F103CBT6 +ComponentSymbolReference0=CC-STM32F103CBT6 +ComponentUpdate0=1 +ComponentIsDeviceSheet0=0 +FullReplace=1 +UpdateDesignatorLock=1 +UpdatePartIDLock=1 +PreserveParameterLocations=1 +PreserveParameterVisibility=1 +DoGraphics=1 +DoParameters=1 +DoModels=1 +AddParameters=0 +RemoveParameters=0 +AddModels=1 +RemoveModels=1 +UpdateCurrentModels=1 +ParameterName0=Comment +ParameterUpdate0=1 +ParameterName1=Component Kind +ParameterUpdate1=1 +ParameterName2=ComponentLink1Description +ParameterUpdate2=1 +ParameterName3=ComponentLink1URL +ParameterUpdate3=1 +ParameterName4=ComponentLink2Description +ParameterUpdate4=1 +ParameterName5=ComponentLink2URL +ParameterUpdate5=1 +ParameterName6=DatasheetVersion +ParameterUpdate6=1 +ParameterName7=Description +ParameterUpdate7=1 +ParameterName8=Library Reference +ParameterUpdate8=1 +ParameterName9=PackageDescription +ParameterUpdate9=1 +ParameterName10=PackageReference +ParameterUpdate10=1 +ParameterName11=PackageVersion +ParameterUpdate11=1 +ParameterName12=Published +ParameterUpdate12=1 +ParameterName13=Publisher +ParameterUpdate13=1 +ParameterName14=Supplier 1 +ParameterUpdate14=1 +ParameterName15=Supplier Part Number 1 +ParameterUpdate15=1 +ModelTypeGroup0=PCBLIB +ModelTypeUpdate0=1 +ModelType0=PCBLIB +ModelName0=LQFP48_L +ModelUpdate0=1 +ModelType1=PCBLIB +ModelName1=LQFP48_M +ModelUpdate1=1 +ModelType2=PCBLIB +ModelName2=LQFP48_N +ModelUpdate2=1 + +[DatabaseUpdateOptions] +SelectedOnly=0 +UpdateVariants=1 +PartTypes=0 + +[Comparison Options] +ComparisonOptions0=Kind=Net|MinPercent=75|MinMatch=3|ShowMatch=-1|Confirm=-1|UseName=-1|InclAllRules=0 +ComparisonOptions1=Kind=Net Class|MinPercent=75|MinMatch=3|ShowMatch=-1|Confirm=-1|UseName=-1|InclAllRules=0 +ComparisonOptions2=Kind=Component Class|MinPercent=75|MinMatch=3|ShowMatch=-1|Confirm=-1|UseName=-1|InclAllRules=0 +ComparisonOptions3=Kind=Rule|MinPercent=75|MinMatch=3|ShowMatch=-1|Confirm=-1|UseName=-1|InclAllRules=0 +ComparisonOptions4=Kind=Differential Pair|MinPercent=50|MinMatch=1|ShowMatch=0|Confirm=0|UseName=0|InclAllRules=0 +ComparisonOptions5=Kind=Code Memory|MinPercent=75|MinMatch=3|ShowMatch=-1|Confirm=-1|UseName=-1|InclAllRules=0 + +[SmartPDF] +PageOptions=Record=PageOptions|CenterHorizontal=True|CenterVertical=True|PrintScale=1.00|XCorrection=1.00|YCorrection=1.00|PrintKind=1|BorderSize=5000000|LeftOffset=0|BottomOffset=0|Orientation=2|PaperLength=1000|PaperWidth=1000|Scale=100|PaperSource=7|PrintQuality=-4|MediaType=1|DitherType=10|PrintScaleMode=1|PaperKind=A4 +Configuration_Name1=OutputConfigurationParameter1 +Configuration_Item1=PrintArea=DesignExtent|PrintAreaLowerLeftCornerX=0|PrintAreaLowerLeftCornerY=0|PrintAreaUpperRightCornerX=0|PrintAreaUpperRightCornerY=0|Record=PcbPrintView +Configuration_Name2=OutputConfigurationParameter2 +Configuration_Item2=IncludeBottomLayerComponents=False|IncludeMultiLayerComponents=False|IncludeTopLayerComponents=False|Index=0|Mirror=False|Name=Panel Details|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration_Name3=OutputConfigurationParameter3 +Configuration_Item3=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical16|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration_Name4=OutputConfigurationParameter4 +Configuration_Item4=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical1|Polygon=Full|PrintOutIndex=0|Record=PcbPrintLayer +Configuration_Name5=OutputConfigurationParameter5 +Configuration_Item5=IncludeBottomLayerComponents=False|IncludeMultiLayerComponents=False|IncludeTopLayerComponents=False|Index=1|Mirror=False|Name=Top SilkScreen|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration_Name6=OutputConfigurationParameter6 +Configuration_Item6=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=TopOverlay|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration_Name7=OutputConfigurationParameter7 +Configuration_Item7=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical16|Polygon=Full|PrintOutIndex=1|Record=PcbPrintLayer +Configuration_Name8=OutputConfigurationParameter8 +Configuration_Item8=IncludeBottomLayerComponents=False|IncludeMultiLayerComponents=False|IncludeTopLayerComponents=False|Index=2|Mirror=False|Name=Top Copper|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration_Name9=OutputConfigurationParameter9 +Configuration_Item9=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=TopLayer|Polygon=Full|PrintOutIndex=2|Record=PcbPrintLayer +Configuration_Name10=OutputConfigurationParameter10 +Configuration_Item10=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical16|Polygon=Full|PrintOutIndex=2|Record=PcbPrintLayer +Configuration_Name11=OutputConfigurationParameter11 +Configuration_Item11=IncludeBottomLayerComponents=False|IncludeMultiLayerComponents=False|IncludeTopLayerComponents=False|Index=3|Mirror=False|Name=Groud Copper|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration_Name12=OutputConfigurationParameter12 +Configuration_Item12=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=MidLayer1|Polygon=Full|PrintOutIndex=3|Record=PcbPrintLayer +Configuration_Name13=OutputConfigurationParameter13 +Configuration_Item13=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical16|Polygon=Full|PrintOutIndex=3|Record=PcbPrintLayer +Configuration_Name14=OutputConfigurationParameter14 +Configuration_Item14=IncludeBottomLayerComponents=False|IncludeMultiLayerComponents=False|IncludeTopLayerComponents=False|Index=4|Mirror=False|Name=Power Layer|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration_Name15=OutputConfigurationParameter15 +Configuration_Item15=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=MidLayer2|Polygon=Full|PrintOutIndex=4|Record=PcbPrintLayer +Configuration_Name16=OutputConfigurationParameter16 +Configuration_Item16=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical16|Polygon=Full|PrintOutIndex=4|Record=PcbPrintLayer +Configuration_Name17=OutputConfigurationParameter17 +Configuration_Item17=IncludeBottomLayerComponents=False|IncludeMultiLayerComponents=False|IncludeTopLayerComponents=False|Index=5|Mirror=False|Name=Bottom Copper|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration_Name18=OutputConfigurationParameter18 +Configuration_Item18=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=BottomLayer|Polygon=Full|PrintOutIndex=5|Record=PcbPrintLayer +Configuration_Name19=OutputConfigurationParameter19 +Configuration_Item19=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical16|Polygon=Full|PrintOutIndex=5|Record=PcbPrintLayer +Configuration_Name20=OutputConfigurationParameter20 +Configuration_Item20=IncludeBottomLayerComponents=False|IncludeMultiLayerComponents=False|IncludeTopLayerComponents=False|Index=6|Mirror=False|Name=Bottom Silk Screen|PadNumberFontSize=14|Record=PcbPrintOut|ShowHoles=False|ShowPadNets=False|ShowPadNumbers=False|SubstituteFonts=False +Configuration_Name21=OutputConfigurationParameter21 +Configuration_Item21=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=BottomOverlay|Polygon=Full|PrintOutIndex=6|Record=PcbPrintLayer +Configuration_Name22=OutputConfigurationParameter22 +Configuration_Item22=CArc=Full|CFill=Full|Comment=Full|Coordinate=Full|CPad=Full|CRegion=Full|CText=Full|CTrack=Full|CVia=Full|DDSymbolKind=0|DDSymbolSize=500000|DDSymbolSortKind=0|Designator=Full|Dimension=Full|DLayer1=TopLayer|DLayer2=BottomLayer|FArc=Full|FFill=Full|FPad=Full|FRegion=Full|FText=Full|FTrack=Full|FVia=Full|Layer=Mechanical16|Polygon=Full|PrintOutIndex=6|Record=PcbPrintLayer + diff --git a/hardware/Production/OpenPilot Atom/OpenPilot Atom.SchDoc b/hardware/Production/OpenPilot Atom/OpenPilot Atom.SchDoc new file mode 100644 index 000000000..cf8671134 Binary files /dev/null and b/hardware/Production/OpenPilot Atom/OpenPilot Atom.SchDoc differ diff --git a/make/common-defs.mk b/make/common-defs.mk index d7383e703..2ff43ed91 100644 --- a/make/common-defs.mk +++ b/make/common-defs.mk @@ -164,7 +164,6 @@ ifeq ($(USE_CXX), YES) CPPFLAGS += -DPIOS_ENABLE_CXX endif - # Compiler flags to generate dependency files CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index ed6322131..aadf90e8f 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -2,7 +2,7 @@ Settings for the @ref AltitudeHold module - + diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index f12df3c88..2bce75833 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -83,34 +83,35 @@ limits="\ %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ - %0903NE:POI:PathPlanner:AutoCruise:Land;\ + %0903NE:POI:PathPlanner:AutoCruise;\ \ %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ - %0903NE:POI:PathPlanner:AutoCruise:Land;\ + %0903NE:POI:PathPlanner:AutoCruise;\ \ %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ - %0903NE:POI:PathPlanner:AutoCruise:Land;\ + %0903NE:POI:PathPlanner:AutoCruise;\ \ %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ - %0903NE:POI:PathPlanner:AutoCruise:Land;\ + %0903NE:POI:PathPlanner:AutoCruise;\ \ %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ - %0903NE:POI:PathPlanner:AutoCruise:Land;\ + %0903NE:POI:PathPlanner:AutoCruise;\ \ %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\ - %0903NE:POI:PathPlanner:AutoCruise:Land;"/> + %0903NE:POI:PathPlanner:AutoCruise;"/> - + + diff --git a/shared/uavobjectdefinition/groundpathfollowersettings.xml b/shared/uavobjectdefinition/groundpathfollowersettings.xml new file mode 100644 index 000000000..cac52fc02 --- /dev/null +++ b/shared/uavobjectdefinition/groundpathfollowersettings.xml @@ -0,0 +1,30 @@ + + + Settings for the @ref GroundPathFollowerModule + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/nedaccel.xml b/shared/uavobjectdefinition/nedaccel.xml index ce1636530..f9e6fa4b9 100644 --- a/shared/uavobjectdefinition/nedaccel.xml +++ b/shared/uavobjectdefinition/nedaccel.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/pathaction.xml b/shared/uavobjectdefinition/pathaction.xml index 29000ffd3..73038d71b 100644 --- a/shared/uavobjectdefinition/pathaction.xml +++ b/shared/uavobjectdefinition/pathaction.xml @@ -2,9 +2,10 @@ A waypoint command the pathplanner is to use at a certain waypoint - + + + - + - + + + diff --git a/shared/uavobjectdefinition/pathsummary.xml b/shared/uavobjectdefinition/pathsummary.xml index 0f8bec9eb..f781f24f8 100644 --- a/shared/uavobjectdefinition/pathsummary.xml +++ b/shared/uavobjectdefinition/pathsummary.xml @@ -11,6 +11,8 @@ + diff --git a/shared/uavobjectdefinition/pidstatus.xml b/shared/uavobjectdefinition/pidstatus.xml new file mode 100644 index 000000000..54fe10bfd --- /dev/null +++ b/shared/uavobjectdefinition/pidstatus.xml @@ -0,0 +1,18 @@ + + + Status of a PID loop for debugging + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/statusgrounddrive.xml b/shared/uavobjectdefinition/statusgrounddrive.xml new file mode 100644 index 000000000..a6d142f51 --- /dev/null +++ b/shared/uavobjectdefinition/statusgrounddrive.xml @@ -0,0 +1,14 @@ + + + Status of a Ground drive sequence + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/statusvtolland.xml b/shared/uavobjectdefinition/statusvtolland.xml new file mode 100644 index 000000000..36b04a600 --- /dev/null +++ b/shared/uavobjectdefinition/statusvtolland.xml @@ -0,0 +1,19 @@ + + + Status of a Vtol landing sequence + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index ec698b810..0021312ef 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -1,14 +1,14 @@ Settings for the @ref VtolPathFollowerModule - + - + @@ -23,6 +23,7 @@ +