void readEEPROM(void)
{
memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t)); // Read flash
ForceRCExpInit = true; // Trigger ini of rc lists
cfg.tri_ymid = constrain(cfg.tri_ymid, cfg.tri_ymin, cfg.tri_ymax); // REAR
GPS_set_pids(); // Set GPS PIDS in any case
GPS_reset_nav();
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate our current speed vector from gps&acc position data
// This is another important part of the gps ins
void GPS_calc_velocity(void) // actual_speed[GPS_Y] y_GPS_speed positve = Up (NORTH) // actual_speed[GPS_X] x_GPS_speed positve = Right (EAST)
{
static uint32_t LastTimestampNewGPSdata;
static bool INSusable;
float gpsHz, tmp0;
uint32_t RealGPSDeltaTime;
uint8_t i;
GPS_calc_longitude_scaling(false); // Init CosLatScaleLon if not already done to avoid div by zero etc..
RealGPSDeltaTime = TimestampNewGPSdata - LastTimestampNewGPSdata; // RealGPSDeltaTime in ms! NOT us!
LastTimestampNewGPSdata = TimestampNewGPSdata;
if (RealGPSDeltaTime) // New GPS Data?
{
if (GPS_update) GPS_update = 0; // Will change state every 2nd run. Blinks Mag Gui Ring
else GPS_update = 1;
INSusable = false; // Set INS to ununsable in advance we will see later
if (RealGPSDeltaTime < 400) // In Time? 2,5Hz-XXHz
{
Real_GPS_coord[LON] = IRQGPS_coord[LON]; // Make Interrupt GPS coords available here and are IN TIME!
Real_GPS_coord[LAT] = IRQGPS_coord[LAT]; // That makes shure they are synchronized and don't randomly appear in the code..
GPS_speed = IRQGPS_speed;
GPS_ground_course = IRQGPS_grcrs;
INSusable = true; // INS is alive
gpsHz = 1000.0f / (float)RealGPSDeltaTime; // Set GPS Hz, try to filter below
if (RealGPSDeltaTime > 10 && RealGPSDeltaTime < 30) gpsHz = 50.0f;// 50Hz Data 20ms filter out timejitter
if (RealGPSDeltaTime > 40 && RealGPSDeltaTime < 60) gpsHz = 20.0f;// 20Hz Data 50ms filter out timejitter
if (RealGPSDeltaTime > 80 && RealGPSDeltaTime < 120) gpsHz = 10.0f;// 10Hz Data 100ms filter out timejitter
if (RealGPSDeltaTime > 180 && RealGPSDeltaTime < 220) gpsHz = 5.0f; // 5Hz Data 200ms
if (RealGPSDeltaTime > 230 && RealGPSDeltaTime < 270) gpsHz = 4.0f; // 4Hz Data 250ms
tmp0 = MagicEarthNumber * gpsHz;
Real_GPS_speed[LON] = (float)(Real_GPS_coord[LON] - Last_Real_GPS_coord[LON]) * tmp0 * CosLatScaleLon ; // cm/s
Real_GPS_speed[LAT] = (float)(Real_GPS_coord[LAT] - Last_Real_GPS_coord[LAT]) * tmp0; // cm/s
for (i = 0; i < 2; i++)
{
Last_Real_GPS_coord[i] = Real_GPS_coord[i];
ACC_speed[i] = (ACC_speed[i] * cfg.gps_ins_vel) + (Real_GPS_speed[i] * (1.0f - cfg.gps_ins_vel)); // CF: GPS Correction
}
}
} // End of X Hz Loop
if ((millis() - TimestampNewGPSdata) > 500) INSusable = false; // INS is NOT OK, too long (500ms) no correction
if (INSusable) for (i = 0; i < 2; i++) MIX_speed[i] = (ACC_speed[i] + Real_GPS_speed[i]) * 0.5f;
else GPS_reset_nav(); // Ins is not possible, reset stuff
}
void readEEPROM(void)
{
uint8_t i;
memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t)); // Read flash
for (i = 0; i < 6; i++)
lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500;
for (i = 0; i < 11; i++)
{
int16_t tmp = 10 * i - cfg.thrMid8;
uint8_t y = 1;
if (tmp > 0) y = 100 - cfg.thrMid8;
if (tmp < 0) y = cfg.thrMid8;
lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; // [0;1000]
lookupThrottleRC[i] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
}
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
GPS_set_pids(); // Set GPS PIDS in any case
GPS_reset_nav();
}
void updateGpsWaypointsAndMode(void)
{
static uint8_t GPSNavReset = 1;
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
// if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
if (rcOptions[BOXGPSHOME]) {
if (!STATE(GPS_HOME_MODE)) {
ENABLE_FLIGHT_MODE(GPS_HOME_MODE);
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPSNavReset = 0;
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
DISABLE_STATE(GPS_HOME_MODE);
if (rcOptions[BOXGPSHOLD] && areSticksInApModePosition(gpsProfile->ap_mode)) {
if (!FLIGHT_MODE(GPS_HOLD_MODE)) {
ENABLE_STATE(GPS_HOLD_MODE);
GPSNavReset = 0;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
} else {
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
// both boxes are unselected here, nav is reset if not already done
if (GPSNavReset == 0) {
GPSNavReset = 1;
GPS_reset_nav();
}
}
}
} else {
DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
nav_mode = NAV_MODE_NONE;
}
}
void loop(void)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
uint8_t axis, i;
int16_t error, errorAngle;
int16_t delta, deltaSum;
int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static int16_t delta1[3], delta2[3];
static int16_t errorGyroI[3] = { 0, 0, 0 };
static int16_t errorAngleI[2] = { 0, 0 };
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
static uint32_t loopTime;
uint16_t auxState = 0;
int16_t prop;
// this will return false if spektrum is disabled. shrug.
if (spektrumFrameComplete())
computeRC();
if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
rcTime = currentTime + 20000;
// TODO clean this up. computeRC should handle this check
if (!feature(FEATURE_SPEKTRUM))
computeRC();
// Failsafe routine
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
for (i = 0; i < 3; i++)
rcData[i] = cfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = cfg.failsafe_throttle;
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec)
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED) { // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeCnt++;
}
if (rcData[THROTTLE] < cfg.mincheck) {
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && !f.ARMED) {
if (rcDelayCommand == 20) {
calibratingG = 1000;
if (feature(FEATURE_GPS))
GPS_reset_home_position();
}
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (!f.ARMED && rcData[YAW] < cfg.mincheck && rcData[PITCH] > cfg.maxcheck && rcData[ROLL] > cfg.maxcheck)) {
if (rcDelayCommand == 20) {
if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
} else {
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
if (AccInflightCalibrationArmed) {
toggleBeep = 2;
} else {
toggleBeep = 3;
}
}
}
} else if (cfg.activate[BOXARM] > 0) {
if (rcOptions[BOXARM] && f.OK_TO_ARM) {
// TODO: feature(FEATURE_FAILSAFE) && failsafeCnt == 0
f.ARMED = 1;
headFreeModeHold = heading;
} else if (f.ARMED)
f.ARMED = 0;
rcDelayCommand = 0;
} else if ((rcData[YAW] < cfg.mincheck || (cfg.retarded_arm == 1 && rcData[ROLL] < cfg.mincheck)) && f.ARMED) {
if (rcDelayCommand == 20)
f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ((rcData[YAW] > cfg.maxcheck || (rcData[ROLL] > cfg.maxcheck && cfg.retarded_arm == 1)) && rcData[PITCH] < cfg.maxcheck && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
f.ARMED = 1;
headFreeModeHold = heading;
}
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > cfg.maxcheck && !f.ARMED) {
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20)
calibratingA = 400;
rcDelayCommand++;
} else if (rcData[YAW] > cfg.maxcheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=right, pitch=min
if (rcDelayCommand == 20)
f.CALIBRATE_MAG = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > cfg.maxcheck) {
cfg.angleTrim[PITCH] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[PITCH] < cfg.mincheck) {
cfg.angleTrim[PITCH] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] > cfg.maxcheck) {
cfg.angleTrim[ROLL] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] < cfg.mincheck) {
cfg.angleTrim[ROLL] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else {
rcDelayCommand = 0;
}
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > cfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
InflightcalibratingA = 50;
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
}
for(i = 0; i < 4; i++)
auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
for(i = 0; i < CHECKBOXITEMS; i++)
rcOptions[i] = (auxState & cfg.activate[i]) > 0;
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
if (!f.ANGLE_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
f.ANGLE_MODE = 1;
}
} else {
f.ANGLE_MODE = 0; // failsave support
}
if (rcOptions[BOXHORIZON]) {
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
f.HORIZON_MODE = 1;
}
} else {
f.HORIZON_MODE = 0;
}
if ((rcOptions[BOXARM]) == 0)
f.OK_TO_ARM = 1;
if (f.ANGLE_MODE || f.HORIZON_MODE) {
LED1_ON;
} else {
LED1_OFF;
}
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID = 0;
}
} else
f.BARO_MODE = 0;
}
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
if (rcOptions[BOXMAG]) {
if (!f.MAG_MODE) {
f.MAG_MODE = 1;
magHold = heading;
}
} else
f.MAG_MODE = 0;
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
}
} else {
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = heading; // acquire new heading
}
}
#endif
if (sensors(SENSOR_GPS)) {
if (f.GPS_FIX && GPS_numSat >= 5) {
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
} else {
f.GPS_HOLD_MODE = 0;
}
}
}
if (rcOptions[BOXPASSTHRU]) {
f.PASSTHRU_MODE = 1;
} else {
f.PASSTHRU_MODE = 0;
}
if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING || cfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
f.HEADFREE_MODE = 0;
}
} else { // not in rc loop
static int8_t taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
switch (taskOrder++ % 4) {
case 0:
#ifdef MAG
if (sensors(SENSOR_MAG))
Mag_getADC();
#endif
break;
case 1:
#ifdef BARO
if (sensors(SENSOR_BARO))
Baro_update();
#endif
break;
case 2:
#ifdef BARO
if (sensors(SENSOR_BARO))
getEstimatedAltitude();
#endif
break;
case 3:
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
Sonar_update();
debug[2] = sonarAlt;
}
#endif
break;
default:
taskOrder = 0;
break;
}
}
currentTime = micros();
if (cfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + cfg.looptime;
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
#ifdef MPU6050_DMP
mpu6050DmpLoop();
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
int16_t dif = heading - magHold;
if (dif <= -180)
dif += 360;
if (dif >= +180)
dif -= 360;
if (f.SMALL_ANGLES_25)
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
} else
magHold = heading;
}
#endif
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (f.BARO_MODE) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
f.BARO_MODE = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
}
#endif
if (sensors(SENSOR_GPS)) {
// Check that we really need to navigate ?
if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME) {
// If not. Reset nav loops and all nav related parameters
GPS_reset_nav();
} else {
float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f);
if (cfg.nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.nav_slew_rate);
GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else {
GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
}
}
}
// **** PITCH & ROLL & YAW PID ****
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
#ifdef LEVEL_PDF
PTermACC = -(int32_t)angle[axis] * cfg.P8[PIDLEVEL] / 100;
#else
PTermACC = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;
}
if (!f.ANGLE_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate our current speed vector from gps&acc position data
// This is another important part of the gps ins
void GPS_calc_velocity(void) // actual_speed[GPS_Y] y_GPS_speed positve = Up (NORTH) // actual_speed[GPS_X] x_GPS_speed positve = Right (EAST)
{
static uint32_t LastTimestampNewGPSdata;
static float GPSmovementAdder[2];
static bool INSusable;
float gpsHz, tmp0;
uint32_t RealGPSDeltaTime;
uint8_t i;
if (CosLatScaleLon == 0.0f) GPS_calc_longitude_scaling(); // Init CosLatScaleLon if not already done to avoid div by zero etc..
RealGPSDeltaTime = TimestampNewGPSdata - LastTimestampNewGPSdata; // RealGPSDeltaTime in ms! NOT us!
LastTimestampNewGPSdata = TimestampNewGPSdata;
if (RealGPSDeltaTime != 0) // New GPS Data?
{
INSusable = false; // Set INS to ununsable in advance we will see later
if (RealGPSDeltaTime < 400) // In Time? 2,5Hz-XXHz
{
INSusable = true; // INS is alive
gpsHz = 1000.0f/(float)RealGPSDeltaTime; // Set GPS Hz, try to filter below
if (RealGPSDeltaTime > 80 && RealGPSDeltaTime < 120) gpsHz = 10.0f;// 10Hz Data 100ms filter out timejitter
if (RealGPSDeltaTime > 180 && RealGPSDeltaTime < 220) gpsHz = 5.0f; // 5Hz Data 200ms
if (RealGPSDeltaTime > 230 && RealGPSDeltaTime < 270) gpsHz = 4.0f; // 4Hz Data 250ms
switch (cfg.gps_ins_mdl) // GPS ins model.
{
case 1: // 1 = Based on lat/lon
default:
tmp0 = MagicEarthNumber * gpsHz;
Real_GPS_speed[LON] = (float)(Real_GPS_coord[LON] - Last_Real_GPS_coord[LON]) * tmp0 * CosLatScaleLon ; // cm/s
Real_GPS_speed[LAT] = (float)(Real_GPS_coord[LAT] - Last_Real_GPS_coord[LAT]) * tmp0; // cm/s
break;
case 2: // 2 = based on Groundcourse & Speed
tmp0 = GPS_ground_course * RADX10;
Real_GPS_speed[LON] = (float)GPS_speed * sinf(tmp0); // cm/s
Real_GPS_speed[LAT] = (float)GPS_speed * cosf(tmp0);
break;
}
for (i = 0; i < 2; i++)
{
Last_Real_GPS_coord[i] = Real_GPS_coord[i];
ACC_speed[i] = (ACC_speed[i] * cfg.gps_ins_vel) + (Real_GPS_speed[i] * (1.0f - cfg.gps_ins_vel)); // CF: GPS Correction
GPSmovementAdder[i] = 0; // This float accumulates the tiny acc movements between GPS reads
}
}
} // End of X Hz Loop
if ((millis() - TimestampNewGPSdata) > 500) INSusable = false; // INS is NOT OK, too long (500ms) no correction
if (INSusable)
{
for (i = 0; i < 2; i++)
{
GPSmovementAdder[i] += (ACC_speed[i] * ACCDeltaTimeINS * OneCmTo[i]);
GPS_coord[i] = Real_GPS_coord[i] + (int32_t)GPSmovementAdder[i];// Bridge the time between GPS reads with acc data
MIX_speed[i] = (ACC_speed[i] + Real_GPS_speed[i]) * 0.5f;
}
}
else
{
GPS_reset_nav(); // Ins is f****d, reset stuff
GPSmovementAdder[LAT] = GPSmovementAdder[LON] = 0;
}
}
void updateGpsWaypointsAndMode(void)
{
bool resetNavNow = false;
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
//
// process HOME mode
//
// HOME mode takes priority over HOLD mode.
if (IS_RC_MODE_ACTIVE(BOXGPSHOME)) {
if (!FLIGHT_MODE(GPS_HOME_MODE)) {
// Transition to HOME mode
ENABLE_FLIGHT_MODE(GPS_HOME_MODE);
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
resetNavNow = true;
}
} else {
if (FLIGHT_MODE(GPS_HOME_MODE)) {
// Transition from HOME mode
DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
nav_mode = NAV_MODE_NONE;
resetNavNow = true;
}
//
// process HOLD mode
//
if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile->ap_mode)) {
if (!FLIGHT_MODE(GPS_HOLD_MODE)) {
// Transition to HOLD mode
ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
resetNavNow = true;
}
} else {
if (FLIGHT_MODE(GPS_HOLD_MODE)) {
// Transition from HOLD mode
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
nav_mode = NAV_MODE_NONE;
resetNavNow = true;
}
}
}
} else {
if (FLIGHT_MODE(GPS_HOLD_MODE | GPS_HOME_MODE)) {
// Transition from HOME or HOLD mode
DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
nav_mode = NAV_MODE_NONE;
resetNavNow = true;
}
}
if (resetNavNow) {
GPS_reset_nav();
}
}
void loop(void)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
uint8_t stTmp = 0;
uint8_t axis, i;
int16_t error, errorAngle;
int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t errorGyroI[3] = { 0, 0, 0 };
static int16_t errorAngleI[2] = { 0, 0 };
int16_t delta;
static int16_t lastGyro[3] = { 0, 0, 0 };
static int16_t delta1[3], delta2[3];
int16_t deltaSum;
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
static uint32_t loopTime;
uint16_t auxState = 0;
int16_t prop;
static uint8_t GPSNavReset = 1;
// this will return false if spektrum is disabled. shrug.
if (spektrumFrameComplete())
computeRC();
if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
rcTime = currentTime + 20000;
// TODO clean this up. computeRC should handle this check
if (!feature(FEATURE_SPEKTRUM))
computeRC();
if (feature(FEATURE_GPS) && mcfg.gps_type == GPS_I2C)
GPS_NewData('c');
// Failsafe routine
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
for (i = 0; i < 3; i++)
rcData[i] = mcfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = cfg.failsafe_throttle;
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec)
mwDisarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED) { // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
mwDisarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeCnt++;
}
// end of failsafe routine - next change is made with RcOptions setting
// ------------------ STICKS COMMAND HANDLER --------------------
// checking sticks positions
for (i = 0; i < 4; i++) {
stTmp >>= 2;
if (rcData[i] > mcfg.mincheck)
stTmp |= 0x80; // check for MIN
if (rcData[i] < mcfg.maxcheck)
stTmp |= 0x40; // check for MAX
}
if (stTmp == rcSticks) {
if (rcDelayCommand < 250)
rcDelayCommand++;
} else
rcDelayCommand = 0;
rcSticks = stTmp;
// perform actions
if (rcData[THROTTLE] < mcfg.mincheck) {
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
if (rcOptions[BOXARM] && f.OK_TO_ARM)
mwArm();
else if (f.ARMED)
mwDisarm();
}
}
if (rcDelayCommand == 20) {
if (f.ARMED) { // actions during armed
// Disarm on throttle down + yaw
if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
mwDisarm();
// Disarm on roll (only when retarded_arm is enabled)
if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
mwDisarm();
} else { // actions during not armed
i = 0;
// GYRO calibration
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
calibratingG = 1000;
if (feature(FEATURE_GPS))
GPS_reset_home_position();
if (sensors(SENSOR_BARO))
calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
// Inflight ACC Calibration
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
} else {
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
if (AccInflightCalibrationArmed) {
toggleBeep = 2;
} else {
toggleBeep = 3;
}
}
}
// Multiple configuration profiles
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1
i = 1;
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2
i = 2;
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
i = 3;
if (i) {
mcfg.current_profile = i - 1;
writeEEPROM(0, false);
blinkLED(2, 40, i);
// TODO alarmArray[0] = i;
}
// Arm via YAW
if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE))
mwArm();
// Arm via ROLL
else if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI))
mwArm();
// Calibrating Acc
else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
calibratingA = 400;
// Calibrating Mag
else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
f.CALIBRATE_MAG = 1;
i = 0;
// Acc Trim
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
cfg.angleTrim[PITCH] += 2;
i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
cfg.angleTrim[PITCH] -= 2;
i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
cfg.angleTrim[ROLL] += 2;
i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
cfg.angleTrim[ROLL] -= 2;
i = 1;
}
if (i) {
writeEEPROM(1, false);
rcDelayCommand = 0; // allow autorepetition
}
}
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
InflightcalibratingA = 50;
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
}
// Check AUX switches
for (i = 0; i < 4; i++)
auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
for (i = 0; i < CHECKBOXITEMS; i++)
rcOptions[i] = (auxState & cfg.activate[i]) > 0;
// note: if FAILSAFE is disable, failsafeCnt > 5 * FAILSAVE_DELAY is always false
if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
if (!f.ANGLE_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
f.ANGLE_MODE = 1;
}
} else {
f.ANGLE_MODE = 0; // failsave support
}
if (rcOptions[BOXHORIZON]) {
f.ANGLE_MODE = 0;
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
f.HORIZON_MODE = 1;
}
} else {
f.HORIZON_MODE = 0;
}
if ((rcOptions[BOXARM]) == 0)
f.OK_TO_ARM = 1;
if (f.ANGLE_MODE || f.HORIZON_MODE) {
LED1_ON;
} else {
LED1_OFF;
}
#ifdef BARO
if (sensors(SENSOR_BARO)) {
// Baro alt hold activate
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID = 0;
}
} else {
f.BARO_MODE = 0;
}
// Vario signalling activate
if (feature(FEATURE_VARIO)) {
if (rcOptions[BOXVARIO]) {
if (!f.VARIO_MODE) {
f.VARIO_MODE = 1;
}
} else {
f.VARIO_MODE = 0;
}
}
}
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
if (rcOptions[BOXMAG]) {
if (!f.MAG_MODE) {
f.MAG_MODE = 1;
magHold = heading;
}
} else {
f.MAG_MODE = 0;
}
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
}
} else {
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = heading; // acquire new heading
}
}
#endif
if (sensors(SENSOR_GPS)) {
if (f.GPS_FIX && GPS_numSat >= 5) {
// if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
f.GPS_HOLD_MODE = 0;
GPSNavReset = 0;
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < cfg.ap_mode && abs(rcCommand[PITCH]) < cfg.ap_mode) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPSNavReset = 0;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
} else {
f.GPS_HOLD_MODE = 0;
// both boxes are unselected here, nav is reset if not already done
if (GPSNavReset == 0) {
GPSNavReset = 1;
GPS_reset_nav();
}
}
}
} else {
f.GPS_HOME_MODE = 0;
f.GPS_HOLD_MODE = 0;
nav_mode = NAV_MODE_NONE;
}
}
if (rcOptions[BOXPASSTHRU]) {
f.PASSTHRU_MODE = 1;
} else {
f.PASSTHRU_MODE = 0;
}
if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
f.HEADFREE_MODE = 0;
}
} else { // not in rc loop
void updateGpsWaypointsAndMode(void)
{
bool resetNavNow = false;
static bool gpsReadyBeepDone = false;
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
//
// process HOME mode
//
// HOME mode takes priority over HOLD mode.
if (rcModeIsActive(BOXGPSHOME)) {
if (!FLIGHT_MODE(GPS_HOME_MODE)) {
// Transition to HOME mode
ENABLE_FLIGHT_MODE(GPS_HOME_MODE);
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
resetNavNow = true;
}
} else {
if (FLIGHT_MODE(GPS_HOME_MODE)) {
// Transition from HOME mode
DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
nav_mode = NAV_MODE_NONE;
resetNavNow = true;
}
//
// process HOLD mode
//
if (rcModeIsActive(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile()->ap_mode)) {
if (!FLIGHT_MODE(GPS_HOLD_MODE)) {
// Transition to HOLD mode
ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
resetNavNow = true;
}
} else {
if (FLIGHT_MODE(GPS_HOLD_MODE)) {
// Transition from HOLD mode
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
nav_mode = NAV_MODE_NONE;
resetNavNow = true;
}
}
}
if (!gpsReadyBeepDone) { //if 'ready' beep not yet done
beeper(BEEPER_READY_BEEP); //do ready beep now
gpsReadyBeepDone = true; //only beep once
}
} else {
if (FLIGHT_MODE(GPS_HOLD_MODE | GPS_HOME_MODE)) {
// Transition from HOME or HOLD mode
DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
nav_mode = NAV_MODE_NONE;
resetNavNow = true;
}
}
if (resetNavNow) {
GPS_reset_nav();
}
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void)
{
/* Basic chip initialization is taken care of in SystemInit() called
* from the startup code. SystemInit() and chip settings are defined
* in the CMSIS system_<part family>.c file.
*/
setup();
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
uint8_t axis,i;
int16_t delta,deltaSum;
int16_t PTerm,ITerm,DTerm;
static int16_t lastGyro[3] = {0,0,0};
static int16_t delta1[3],delta2[3];
static int16_t errorAngleI[2] = {0,0};
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
int16_t error,errorAngle =0;
#if PIDcontroller ==1
static int16_t errorGyroI[3] = {0,0,0};
#elif PIDcontroller == 2
int32_t AngleRateTmp = 0, RateError = 0, AngleITerm = 0;
static int32_t errorGyroI[3] = {0,0,0};
#endif
#define RC_FREQ 50
while(1)
{
#ifdef LCD_TELEMETRY_STEP
static char telemetryStepSequence [] = LCD_TELEMETRY_STEP;
static uint8_t telemetryStepIndex = 0;
#endif
#if defined(SPEKTRUM)
if (rcFrameComplete) computeRC();
#endif
#if defined(OPENLRSv2MULTI)
Read_OpenLRS_RC();
#endif
if ((currentTime > rcTime) || rcTime > (currentTime + 20000)) { // 50Hz + additional check to ensure rcTime can reach currentTime
rcTime = currentTime + 20000;
computeRC();
//reset watchdog timer in RC loop to ensure user is not locked out of control
feedWatchDog();
// Failsafe routine - added by MIS
#if defined(FAILSAFE)
if ( failsafeCnt > (5*FAILSAVE_DELAY) && f.ARMED) { // Stabilize, and set Throttle to specified level
for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = FAILSAVE_THROTTLE;
if (failsafeCnt > 5*(FAILSAVE_DELAY+FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
if ( failsafeCnt > (5*FAILSAVE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeCnt++;
#endif
// end of failsave routine - next change is made with RcOptions setting
if (rcData[THROTTLE] < MINCHECK) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
calibratingG=400;
#if GPS
GPS_reset_home_position();
#endif
}
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
#ifdef TRI
servo[5] = 1500; // we center the yaw servo in conf mode
writeServos();
#endif
#ifdef FLYING_WING
servo[0] = conf.wing_left_mid;
servo[1] = conf.wing_right_mid;
writeServos();
#endif
#ifdef AIRPLANE
for(i = 4; i<7 ;i++) servo[i] = 1500;
writeServos();
#endif
#if defined(LCD_CONF)
configurationLoop(); // beginning LCD configuration
#endif
previousTime = micros();
}
}
#if defined(INFLIGHT_ACC_CALIBRATION)
else if (!f.ARMED && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK){
if (rcDelayCommand == 20){
if (AccInflightCalibrationMeasurementDone){ // trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}else{
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
#if defined(BUZZER)
if (AccInflightCalibrationArmed){
toggleBeep = 2;
} else {
toggleBeep = 3;
}
#endif
}
}
}
#endif
else if (conf.activate[BOXARM] > 0) {
if ( rcOptions[BOXARM] && f.OK_TO_ARM
#if defined(FAILSAFE)
&& failsafeCnt <= 1
#endif
) {
f.ARMED = 1;
headFreeModeHold = heading;
} else if (f.ARMED) f.ARMED = 0;
rcDelayCommand = 0;
#ifdef ALLOW_ARM_DISARM_VIA_TX_YAW
} else if ( (rcData[YAW] < MINCHECK ) && f.ARMED) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[YAW] > MAXCHECK ) && rcData[PITCH] < MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
f.ARMED = 1;
headFreeModeHold = heading;
}
#endif
#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL
} else if ( (rcData[ROLL] < MINCHECK) && f.ARMED) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[ROLL] > MAXCHECK) && rcData[PITCH] < MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
f.ARMED = 1;
headFreeModeHold = heading;
}
#endif
#ifdef LCD_TELEMETRY_AUTO
} else if (rcData[ROLL] < MINCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
if (telemetry_auto) {
telemetry_auto = 0;
telemetry = 0;
} else
telemetry_auto = 1;
}
#endif
#ifdef LCD_TELEMETRY_STEP
} else if (rcData[ROLL] > MAXCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)];
LCDclear(); // make sure to clear away remnants
}
#endif
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > MAXCHECK && !f.ARMED) {
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20) calibratingA=400;
rcDelayCommand++;
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] < MINCHECK) { // throttle=max, yaw=right, pitch=min
if (rcDelayCommand == 20) f.CALIBRATE_MAG = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > MAXCHECK) {
conf.angleTrim[PITCH]+=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[PITCH] < MINCHECK) {
conf.angleTrim[PITCH]-=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] > MAXCHECK) {
conf.angleTrim[ROLL]+=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] < MINCHECK) {
conf.angleTrim[ROLL]-=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else {
rcDelayCommand = 0;
}
}
#if defined(LED_FLASHER)
led_flasher_autoselect_sequence();
#endif
#if defined(INFLIGHT_ACC_CALIBRATION)
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){
InflightcalibratingA = 50;
}
}else if(AccInflightCalibrationMeasurementDone && !f.ARMED){
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
#endif
uint16_t auxState = 0;
for(i=0;i<4;i++)
auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
for(i=0;i<CHECKBOXITEMS;i++)
rcOptions[i] = (auxState & conf.activate[i])>0;
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if (( rcOptions[BOXACC] || (failsafeCnt > 5*FAILSAVE_DELAY) ) && ACC ) {
// bumpless transfer to Level mode
if (!f.ACC_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.ACC_MODE = 1;
}
} else {
// failsafe support
f.ACC_MODE = 0;
}
if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
//if (f.ACC_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
#if BARO
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID=0;
}
} else {
f.BARO_MODE = 0;
}
#endif
#if MAG
if (rcOptions[BOXMAG]) {
if (!f.MAG_MODE) {
f.MAG_MODE = 1;
magHold = heading;
}
} else {
f.MAG_MODE = 0;
}
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
}
} else {
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = heading; // acquire new heading
}
#endif
#if GPS
#if defined(I2C_NAV)
static uint8_t GPSNavReset = 1;
if (f.GPS_FIX && GPS_numSat >= 5 ) {
if (!rcOptions[BOXGPSHOME] && !rcOptions[BOXGPSHOLD] )
{ //Both boxes are unselected
if (GPSNavReset == 0 ) {
GPSNavReset = 1;
GPS_reset_nav();
}
}
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
GPSNavReset = 0;
GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero
}
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (!f.GPS_HOLD_MODE & !f.GPS_HOME_MODE) {
f.GPS_HOLD_MODE = 1;
GPSNavReset = 0;
GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);
}
} else {
f.GPS_HOLD_MODE = 0;
}
}
#endif
#if defined(GPS_SERIAL) || defined(TINY_GPS) || defined(GPS_FROM_OSD)||defined(I2C_GPS)
if (f.GPS_FIX && GPS_numSat >= 5 ) {
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
} else {
f.GPS_HOLD_MODE = 0;
}
}
#endif
#endif
#ifdef FLOW
if(rcOptions[BOXFLOWHOLD])
{
if (!f.FLOW_HOLD_MODE)
{
f.FLOW_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
flow_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
//debug[1] = 1;
}
else
{
f.FLOW_HOLD_MODE = 0;
//debug[1] = 0;
}
#endif
if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;}
else {f.PASSTHRU_MODE = 0;}
#ifdef FIXEDWING
f.HEADFREE_MODE = 0;
#endif
} else { // not in rc loop
static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes
if(taskOrder>4) taskOrder-=5;
switch (taskOrder) {
case 0:
taskOrder++;
#if MAG
if (Mag_getADC()) break; // max 350 µs (HMC5883) // only break when we actually did something
#endif
case 1:
taskOrder++;
#if BARO
if (Baro_update() != 0 ) break;
#endif
case 2:
taskOrder++;
#if BARO
if (getEstimatedAltitude() !=0) break;
#endif
case 3:
taskOrder++;
if(flowUpdate() !=0) break;
#if GPS
if(GPS_Enable) GPS_NewData();
break;
#endif
case 4:
taskOrder++;
#if SONAR
#if !defined(MAXSONAR_PWM) && !defined(FLOW)
Sonar_update();
#endif
//debug[2] = sonarAlt;
#endif
#ifdef LANDING_LIGHTS_DDR
auto_switch_landing_lights();
#endif
#ifdef VARIOMETER
if (f.VARIO_MODE) vario_signaling();
#endif
break;
}
}
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;
//delayMs(500);
//if(cycleTime > 3150)
// debug[0]++;
#if MAG
if (fabs(rcCommand[YAW]) <70 && f.MAG_MODE) {
int16_t dif = heading - magHold;
if (dif <= - 180) dif += 360;
if (dif >= + 180) dif -= 360;
if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.P8[PIDMAG]/30; // 18 deg
} else magHold = heading;
#endif
#if BARO
if (f.BARO_MODE) {
if (fabs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
f.BARO_MODE = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
#endif
#if GPS
if ( (!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME ) {
GPS_reset_nav(); // If GPS is not activated. Reset nav loops and all nav related parameters
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
} else {
float sin_yaw_y = sinf(heading*0.0174532925f);
float cos_yaw_x = cosf(heading*0.0174532925f);
#if defined(NAV_SLEW_RATE)
nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10;
#else
GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
#endif
}
//debug[2] = GPS_angle[ROLL];
//debug[3] = GPS_angle[PITCH];
#endif
#ifdef FLOW
if (!f.FLOW_HOLD_MODE) {
flow_reset_nav(); // If GPS is not activated. Reset nav loops and all nav related parameters
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
} else {
float sin_yaw_y = sinf(heading*0.0174532925f);
float cos_yaw_x = cosf(heading*0.0174532925f);
#if defined(NAV_SLEW_RATE)
nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10;
#else
GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
#endif
}
//debug[2] = GPS_angle[ROLL];
//debug[3] = GPS_angle[PITCH];
#endif
#if PIDcontroller == 1
//**** PITCH & ROLL & YAW PID ****
for(axis=0;axis<3;axis++) {
if (f.ACC_MODE && axis<2 ) { //LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2*rcCommand[axis] + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
#ifdef LEVEL_PDF
PTerm = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ;
#else
PTerm = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTerm = constrain(PTerm,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
ITerm = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
} else { //ACRO MODE or YAW axis
if (fabs(rcCommand[axis])<350) error = rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2)
else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error -= gyroData[axis];
PTerm = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here
if (fabs(gyroData[axis])>640) errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis]/125*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
if (fabs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result
else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
if (fabs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5; // 16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result
else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation