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; |
} |
|
|