| 15,6 → 15,7 |
|---|
| #include "isqrt.h" |
| #include "attitude.h" |
| #include "dongfangMath.h" |
| #include "attitude.h" |
| |
| typedef enum { |
| GPS_FLIGHT_MODE_UNDEF, |
| 50,11 → 51,11 |
|---|
| flightMode = GPS_FLIGHT_MODE_HOME; |
| } else { |
| if (dynamicParams.naviMode < 50) |
| flightMode = GPS_FLIGHT_MODE_FREE; |
| flightMode = GPS_FLIGHT_MODE_HOME; |
| else if (dynamicParams.naviMode < 180) |
| flightMode = GPS_FLIGHT_MODE_AID; |
| else |
| flightMode = GPS_FLIGHT_MODE_HOME; |
| flightMode = GPS_FLIGHT_MODE_FREE; |
| } |
| |
| if (flightMode != flightModeOld) { |
| 93,9 → 94,9 |
|---|
| } |
| |
| // checks nick and roll sticks for manual control |
| uint8_t navi_isManuallyControlled(int16_t* sticks) { |
| if (sticks[CONTROL_PITCH] < staticParams.naviStickThreshold |
| && sticks[CONTROL_ROLL] < staticParams.naviStickThreshold) |
| uint8_t navi_isManuallyControlled(int16_t* PRTY) { |
| if (PRTY[CONTROL_PITCH] < staticParams.naviStickThreshold |
| && PRTY[CONTROL_ROLL] < staticParams.naviStickThreshold) |
| return 0; |
| else |
| return 1; |
| 134,7 → 135,7 |
|---|
| // calculates the GPS control stick values from the deviation to target position |
| // if the pointer to the target positin is NULL or is the target position invalid |
| // then the P part of the controller is deactivated. |
| void navi_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) { |
| void navi_PIDController(GPS_Pos_t *pTargetPos, int16_t *PRTY) { |
| static int32_t PID_Nick, PID_Roll; |
| int32_t coscompass, sincompass; |
| int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
| 230,8 → 231,8 |
|---|
| // in the flight.c. Therefore a positive north deviation/velocity should result in a positive |
| // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
| |
| coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW); |
| sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW); |
| |
| PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
| 239,12 → 240,9 |
|---|
| // limit resulting GPS control vector |
| navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |
| |
| debugOut.analog[27] = -PID_Nick; |
| debugOut.analog[28] = -PID_Roll; |
| PRTY[CONTROL_PITCH] += PID_Nick; |
| PRTY[CONTROL_ROLL] += PID_Roll; |
| |
| sticks[CONTROL_PITCH] -= PID_Nick; |
| sticks[CONTROL_ROLL] -= PID_Roll; |
| |
| } else { // invalid GPS data or bad compass reading |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| 252,7 → 250,7 |
|---|
| } |
| } |
| |
| void navigation_periodicTask(int16_t* sticks) { |
| void navigation_periodicTaskAndPRTY(int16_t* PRTY) { |
| static uint8_t GPS_P_Delay = 0; |
| static uint16_t beep_rythm = 0; |
| |
| 296,7 → 294,7 |
|---|
| |
| case GPS_FLIGHT_MODE_AID: |
| if (holdPosition.status != INVALID) { |
| if (navi_isManuallyControlled(sticks)) { // MK controlled by user |
| if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
| // update hold point to current gps position |
| navi_writeCurrPositionTo(&holdPosition); |
| // disable gps control |
| 306,9 → 304,9 |
|---|
| // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
| GPS_P_Delay++; |
| navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position |
| navi_PIDController(NULL, sticks); // activates only the D-Part |
| navi_PIDController(NULL, PRTY); // activates only the D-Part |
| } else |
| navi_PIDController(&holdPosition, sticks); // activates the P&D-Part |
| navi_PIDController(&holdPosition, PRTY); // activates the P&D-Part |
| } |
| } else // invalid Hold Position |
| { // try to catch a valid hold position from gps data input |
| 321,10 → 319,10 |
|---|
| // update hold point to current gps position |
| // to avoid a flight back if home comming is deactivated |
| navi_writeCurrPositionTo(&holdPosition); |
| if (navi_isManuallyControlled(sticks)) // MK controlled by user |
| if (navi_isManuallyControlled(PRTY)) // MK controlled by user |
| { |
| } else {// GPS control active |
| navi_PIDController(&homePosition, sticks); |
| navi_PIDController(&homePosition, PRTY); |
| } |
| } else { |
| // bad home position |
| 332,11 → 330,11 |
|---|
| // try to hold at least the position as a fallback option |
| |
| if (holdPosition.status != INVALID) { |
| if (navi_isManuallyControlled(sticks)) { |
| if (navi_isManuallyControlled(PRTY)) { |
| // MK controlled by user |
| } else { |
| // GPS control active |
| navi_PIDController(&holdPosition, sticks); |
| navi_PIDController(&holdPosition, PRTY); |
| } |
| } else { // try to catch a valid hold position |
| navi_writeCurrPositionTo(&holdPosition); |
| 365,5 → 363,3 |
|---|
| |
| debugOut.analog[11] = GPSInfo.satnum; |
| } |
| |
| |