static bool UBLOX_parse_gps(void) { int i; switch (_msg_id) { case MSG_POSLLH: //i2c_dataset.time = _buffer.posllh.time; GPS_coord[LON] = _buffer.posllh.longitude; GPS_coord[LAT] = _buffer.posllh.latitude; GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m if (next_fix) { ENABLE_STATE(GPS_FIX); } else { DISABLE_STATE(GPS_FIX); } _new_position = true; break; case MSG_STATUS: next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); if (!next_fix) DISABLE_STATE(GPS_FIX); break; case MSG_SOL: next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); if (!next_fix) DISABLE_STATE(GPS_FIX); GPS_numSat = _buffer.solution.satellites; GPS_hdop = _buffer.solution.position_DOP; break; case MSG_VELNED: // speed_3d = _buffer.velned.speed_3d; // cm/s GPS_speed = _buffer.velned.speed_2d; // cm/s GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 _new_speed = true; break; case MSG_SVINFO: GPS_numCh = _buffer.svinfo.numCh; if (GPS_numCh > 16) GPS_numCh = 16; for (i = 0; i < GPS_numCh; i++){ GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn; GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid; GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality; GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno; } break; default: return false; } // we only return true when we get new position and speed data // this ensures we don't use stale data if (_new_position && _new_speed) { _new_speed = _new_position = false; return true; } return false; }
void servoConfigureOutput(void) { if (useServo) { servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; if (servoMixers[currentMixerMode].rule) { for (int i = 0; i < servoRuleCount; i++) currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; } } // set flag that we're on something with wings if (currentMixerMode == MIXER_FLYING_WING || currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE ) { ENABLE_STATE(FIXED_WING); if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) { loadCustomServoMixer(); } } else { DISABLE_STATE(FIXED_WING); if (currentMixerMode == MIXER_CUSTOM_TRI) { loadCustomServoMixer(); } } }
static void gpsHandleProtocol(void) { bool newDataReceived = false; // Call protocol-specific code if (gpsProviders[gpsState.gpsConfig->provider].read) { newDataReceived = gpsProviders[gpsState.gpsConfig->provider].read(); } // Received new update for solution data if (newDataReceived) { // Set GPS fix flag only if we have 3D fix if (gpsSol.fixType == GPS_FIX_3D) { ENABLE_STATE(GPS_FIX); } else { DISABLE_STATE(GPS_FIX); } // Update GPS coordinates etc sensorsSet(SENSOR_GPS); onNewGPSData(); // Update timeout gpsState.lastLastMessageMs = gpsState.lastMessageMs; gpsState.lastMessageMs = millis(); // Update statistics gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs; } }
void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) { static uint32_t tCal = 0; static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; magDev.read(&magDev, magADCRaw); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { mag.magADC[axis] = magADCRaw[axis]; } alignSensors(mag.magADC, magDev.magAlign); if (STATE(CALIBRATE_MAG)) { tCal = currentTime; for (int axis = 0; axis < 3; axis++) { magZero->raw[axis] = 0; magZeroTempMin.raw[axis] = mag.magADC[axis]; magZeroTempMax.raw[axis] = mag.magADC[axis]; } DISABLE_STATE(CALIBRATE_MAG); } if (magInit) { // we apply offset only once mag calibration is done mag.magADC[X] -= magZero->raw[X]; mag.magADC[Y] -= magZero->raw[Y]; mag.magADC[Z] -= magZero->raw[Z]; } if (tCal != 0) { if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions LED0_TOGGLE; for (int axis = 0; axis < 3; axis++) { if (mag.magADC[axis] < magZeroTempMin.raw[axis]) magZeroTempMin.raw[axis] = mag.magADC[axis]; if (mag.magADC[axis] > magZeroTempMax.raw[axis]) magZeroTempMax.raw[axis] = mag.magADC[axis]; } } else { tCal = 0; for (int axis = 0; axis < 3; axis++) { magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets } saveConfigAndNotify(); } } }
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) { /* Compute pitch/roll angles */ attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2])); attitude.values.pitch = RADIANS_TO_DECIDEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0])); attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])) + magneticDeclination; if (attitude.values.yaw < 0) attitude.values.yaw += 3600; /* Update small angle state */ if (rMat[2][2] > smallAngleCosZ) { ENABLE_STATE(SMALL_ANGLE); } else { DISABLE_STATE(SMALL_ANGLE); } }
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) { /* Compute pitch/roll angles */ attitude.values.roll = lrintf(atan2f(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf)); attitude.values.pitch = lrintf(((0.5f * M_PIf) - acosf(-rMat[2][0])) * (1800.0f / M_PIf)); attitude.values.yaw = lrintf((-atan2f(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf) + magneticDeclination)); if (attitude.values.yaw < 0) attitude.values.yaw += 3600; /* Update small angle state */ if (rMat[2][2] > smallAngleCosZ) { ENABLE_STATE(SMALL_ANGLE); } else { DISABLE_STATE(SMALL_ANGLE); } }
void gpsThread(void) { // read out available GPS bytes if (gpsPort) { while (serialRxBytesWaiting(gpsPort)) gpsNewData(serialRead(gpsPort)); } switch (gpsData.state) { case GPS_UNKNOWN: break; case GPS_INITIALIZING: case GPS_CHANGE_BAUD: case GPS_CONFIGURE: gpsInitHardware(); break; case GPS_LOST_COMMUNICATION: gpsData.timeouts++; if (gpsConfig()->autoBaud) { // try another rate gpsData.baudrateIndex++; gpsData.baudrateIndex %= GPS_INIT_ENTRIES; } gpsData.lastMessage = millis(); // TODO - move some / all of these into gpsData GPS_numSat = 0; DISABLE_STATE(GPS_FIX); gpsSetState(GPS_INITIALIZING); break; case GPS_RECEIVING_DATA: // check for no data/gps timeout/cable disconnection etc if (millis() - gpsData.lastMessage > GPS_TIMEOUT) { // remove GPS from capability sensorsClear(SENSOR_GPS); gpsSetState(GPS_LOST_COMMUNICATION); } break; } }
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; } }
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) { #ifndef GPS // this local variable should be optimized out when GPS is not used. float magneticDeclination = 0.0f; #endif /* Compute pitch/roll angles */ attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf)); attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf)); attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf) + magneticDeclination)); if (attitude.values.yaw < 0) attitude.values.yaw += 3600; /* Update small angle state */ if (rMat[2][2] > smallAngleCosZ) { ENABLE_STATE(SMALL_ANGLE); } else { DISABLE_STATE(SMALL_ANGLE); } }
static void gpsHandleProtocol(void) { bool newDataReceived = false; // Call protocol-specific code if (gpsProviders[gpsState.gpsConfig->provider].read) { newDataReceived = gpsProviders[gpsState.gpsConfig->provider].read(); } // Received new update for solution data if (newDataReceived) { // Set GPS fix flag only if we have 3D fix if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { ENABLE_STATE(GPS_FIX); } else { /* When no fix available - reset flags as well */ gpsSol.flags.validVelNE = 0; gpsSol.flags.validVelD = 0; gpsSol.flags.validEPE = 0; DISABLE_STATE(GPS_FIX); } // Update GPS coordinates etc sensorsSet(SENSOR_GPS); onNewGPSData(); // Update timeout gpsState.lastLastMessageMs = gpsState.lastMessageMs; gpsState.lastMessageMs = millis(); // Update statistics gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs; } }
static bool gpsNewFrameNMEA(char c) { static gpsDataNmea_t gps_Msg; uint8_t frameOK = 0; static uint8_t param = 0, offset = 0, parity = 0; static char string[15]; static uint8_t checksum_param, gps_frame = NO_FRAME; static uint8_t svMessageNum = 0; uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0; switch (c) { case '$': param = 0; offset = 0; parity = 0; break; case ',': case '*': string[offset] = 0; if (param == 0) { //frame identification gps_frame = NO_FRAME; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') gps_frame = FRAME_GGA; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') gps_frame = FRAME_RMC; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V') gps_frame = FRAME_GSV; } switch (gps_frame) { case FRAME_GGA: //************* GPGGA FRAME parsing switch(param) { // case 1: // Time information // break; case 2: gps_Msg.latitude = GPS_coord_to_degrees(string); break; case 3: if (string[0] == 'S') gps_Msg.latitude *= -1; break; case 4: gps_Msg.longitude = GPS_coord_to_degrees(string); break; case 5: if (string[0] == 'W') gps_Msg.longitude *= -1; break; case 6: if (string[0] > '0') { ENABLE_STATE(GPS_FIX); } else { DISABLE_STATE(GPS_FIX); } break; case 7: gps_Msg.numSat = grab_fields(string, 0); break; case 9: gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis break; } break; case FRAME_RMC: //************* GPRMC FRAME parsing switch(param) { case 7: gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis break; case 8: gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10 break; } break; case FRAME_GSV: switch(param) { /*case 1: // Total number of messages of this type in this cycle break; */ case 2: // Message number svMessageNum = grab_fields(string, 0); break; case 3: // Total number of SVs visible GPS_numCh = grab_fields(string, 0); break; } if(param < 4) break; svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4 svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite if(svSatNum > GPS_SV_MAXSATS) break; switch(svSatParam) { case 1: // SV PRN number GPS_svinfo_chn[svSatNum - 1] = svSatNum; GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0); break; /*case 2: // Elevation, in degrees, 90 maximum break; case 3: // Azimuth, degrees from True North, 000 through 359 break; */ case 4: // SNR, 00 through 99 dB (null when not tracking) GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0); GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox break; } GPS_svInfoReceivedCount++; break; } param++; offset = 0; if (c == '*') checksum_param = 1; else parity ^= c; break; case '\r': case '\n': if (checksum_param) { //parity checksum shiftPacketLog(); uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); if (checksum == parity) { *gpsPacketLogChar = LOG_IGNORED; GPS_packetCount++; switch (gps_frame) { case FRAME_GGA: *gpsPacketLogChar = LOG_NMEA_GGA; frameOK = 1; if (STATE(GPS_FIX)) { GPS_coord[LAT] = gps_Msg.latitude; GPS_coord[LON] = gps_Msg.longitude; GPS_numSat = gps_Msg.numSat; GPS_altitude = gps_Msg.altitude; } break; case FRAME_RMC: *gpsPacketLogChar = LOG_NMEA_RMC; GPS_speed = gps_Msg.speed; GPS_ground_course = gps_Msg.ground_course; break; } // end switch } else { *gpsPacketLogChar = LOG_ERROR; } } checksum_param = 0; break; default: if (offset < 15) string[offset++] = c; if (!checksum_param) parity ^= c; } return frameOK; }
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers) { uint8_t i; // set flag that we're on something with wings if (currentMixerMode == MIXER_FLYING_WING || currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE ) { ENABLE_STATE(FIXED_WING); } else { DISABLE_STATE(FIXED_WING); } currentMixerMode = mixerMode; customMixers = initialCustomMotorMixers; customServoMixers = initialCustomServoMixers; minServoIndex = servoMixers[currentMixerMode].minServoIndex; maxServoIndex = servoMixers[currentMixerMode].maxServoIndex; // enable servos for mixes that require them. note, this shifts motor counts. mixerUsesServos = mixers[currentMixerMode].useServo; // if we want camstab/trig, that also enables servos, even if mixer doesn't servoOutputEnabled = mixerUsesServos || feature(FEATURE_SERVO_TILT) || feature(FEATURE_CHANNEL_FORWARDING); // give all servos a default command for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = DEFAULT_SERVO_MIDDLE; } /* * If mixer has predefined servo mixes, load them */ if (mixerUsesServos) { servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; if (servoMixers[currentMixerMode].rule) { for (i = 0; i < servoRuleCount; i++) currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; } } /* * When we are dealing with CUSTOM mixers, load mixes defined with * smix and update internal variables */ if (currentMixerMode == MIXER_CUSTOM_AIRPLANE || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM) { loadCustomServoMixer(); // If there are servo rules after all, update variables if (servoRuleCount > 0) { servoOutputEnabled = 1; mixerUsesServos = 1; } } }
void processRx(void) { static bool armedBeeperOn = false; calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (masterConfig.auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over mwDisarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (masterConfig.auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch, masterConfig.fixed_wing_auto_arm); updateActivatedModes(currentProfile->modeActivationConditions, currentProfile->modeActivationOperator); if (!cliMode) { updateAdjustmentStates(currentProfile->adjustmentRanges); processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive()) || naivationRequiresAngleMode()) && sensors(SENSOR_ACC)) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } /* Heading lock mode */ if (IS_RC_MODE_ACTIVE(BOXHEADINGLOCK)) { if (!FLIGHT_MODE(HEADING_LOCK)) { ENABLE_FLIGHT_MODE(HEADING_LOCK); } } else { DISABLE_FLIGHT_MODE(HEADING_LOCK); } /* Flaperon mode */ if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) { if (!FLIGHT_MODE(FLAPERON)) { ENABLE_FLIGHT_MODE(FLAPERON); } } else { DISABLE_FLIGHT_MODE(FLAPERON); } /* Turn assistant mode */ if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) { if (!FLIGHT_MODE(TURN_ASSISTANT)) { ENABLE_FLIGHT_MODE(TURN_ASSISTANT); } } else { DISABLE_FLIGHT_MODE(TURN_ASSISTANT); } #if defined(MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); ENABLE_FLIGHT_MODE(MAG_MODE); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } #endif // Navigation may override PASSTHRU_MODE if (IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !naivationRequiresAngleMode()) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */ if (FLIGHT_MODE(PASSTHRU_MODE) || !ARMING_FLAG(ARMED)) { /* In PASSTHRU mode we reset integrators prevent I-term wind-up (PID output is not used in PASSTHRU) */ pidResetErrorAccumulators(); } else { if (throttleStatus == THROTTLE_LOW) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig); // ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) { ENABLE_STATE(ANTI_WINDUP); } else { DISABLE_STATE(ANTI_WINDUP); } } else { DISABLE_STATE(ANTI_WINDUP); pidResetErrorAccumulators(); } } else { DISABLE_STATE(ANTI_WINDUP); } } if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) || (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspSerialAllocatePorts(); } } #endif }
static void imuCalculateEstimatedAttitude(void) { int32_t axis; int32_t accMag = 0; static t_fp_vector EstM; static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } }; static float accLPF[3]; static uint32_t previousT; uint32_t currentT = micros(); uint32_t deltaT; float scale; fp_angles_t deltaGyroAngle; deltaT = currentT - previousT; scale = deltaT * gyroScaleRad; previousT = currentT; // Initialization for (axis = 0; axis < 3; axis++) { deltaGyroAngle.raw[axis] = gyroADC[axis] * scale; if (imuRuntimeConfig->acc_lpf_factor > 0) { accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / imuRuntimeConfig->acc_lpf_factor)) + accADC[axis] * (1.0f / imuRuntimeConfig->acc_lpf_factor); accSmooth[axis] = accLPF[axis]; } else { accSmooth[axis] = accADC[axis]; } accMag += (int32_t)accSmooth[axis] * accSmooth[axis]; } accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G); rotateV(&EstG.V, &deltaGyroAngle); // Apply complimentary filter (Gyro drift correction) // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation. // To do that, we just skip filter, as EstV already rotated by Gyro float invGyroComplimentaryFilterFactor = (1.0f / (imuRuntimeConfig->gyro_cmpf_factor + 1.0f)); if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) { for (axis = 0; axis < 3; axis++) EstG.A[axis] = (EstG.A[axis] * imuRuntimeConfig->gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor; } if (EstG.A[Z] > smallAngle) { ENABLE_STATE(SMALL_ANGLE); } else { DISABLE_STATE(SMALL_ANGLE); } // Attitude of the estimated vector anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z); anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)); inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf)); inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf)); if (sensors(SENSOR_MAG)) { rotateV(&EstM.V, &deltaGyroAngle); // FIXME what does the _M_ mean? float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f)); for (axis = 0; axis < 3; axis++) { EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor; } heading = imuCalculateHeading(&EstM); } else { rotateV(&EstN.V, &deltaGyroAngle); normalizeV(&EstN.V, &EstN.V); heading = imuCalculateHeading(&EstN); } imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame }
void processRx(void) { static bool armedBeeperOn = false; calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle); rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(rxConfig()); /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */ if (throttleStatus == THROTTLE_LOW) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { if (rollPitchStatus == CENTERED) { ENABLE_STATE(ANTI_WINDUP); } else { DISABLE_STATE(ANTI_WINDUP); } } else { pidResetErrorAngle(); pidResetErrorGyro(); } } else { DISABLE_STATE(ANTI_WINDUP); } // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (armingConfig()->auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over mwDisarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (armingConfig()->auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(rxConfig(), throttleStatus, armingConfig()->retarded_arm, armingConfig()->disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(modeActivationProfile()->modeActivationConditions); if (!cliMode) { updateAdjustmentStates(adjustmentProfile()->adjustmentRanges); processRcAdjustments(currentControlRateProfile, rxConfig()); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) || (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspAllocateSerialPorts(); } } #endif }
void gpsThread(void) { /* Extra delay for at least 2 seconds after booting to give GPS time to initialise */ if (!isMPUSoftReset() && (millis() < GPS_BOOT_DELAY)) { sensorsClear(SENSOR_GPS); DISABLE_STATE(GPS_FIX); return; } #ifdef USE_FAKE_GPS gpsFakeGPSUpdate(); #else // Serial-based GPS if ((gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL) && (gpsState.gpsPort != NULL)) { switch (gpsState.state) { default: case GPS_INITIALIZING: if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) { // Reset internals DISABLE_STATE(GPS_FIX); gpsSol.fixType = GPS_NO_FIX; gpsState.hwVersion = 0; gpsState.autoConfigStep = 0; gpsState.autoConfigPosition = 0; gpsState.autoBaudrateIndex = 0; // Reset solution gpsResetSolution(); // Call protocol handler - switch to next state is done there gpsHandleProtocol(); } break; case GPS_CHANGE_BAUD: // Call protocol handler - switch to next state is done there gpsHandleProtocol(); break; case GPS_CHECK_VERSION: case GPS_CONFIGURE: case GPS_RECEIVING_DATA: gpsHandleProtocol(); if ((millis() - gpsState.lastMessageMs) > GPS_TIMEOUT) { // Check for GPS timeout sensorsClear(SENSOR_GPS); DISABLE_STATE(GPS_FIX); gpsSol.fixType = GPS_NO_FIX; gpsSetState(GPS_LOST_COMMUNICATION); } break; case GPS_LOST_COMMUNICATION: gpsStats.timeouts++; // Handle autobaud - switch to next port baud rate if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) { gpsState.baudrateIndex++; gpsState.baudrateIndex %= GPS_BAUDRATE_COUNT; } gpsSetState(GPS_INITIALIZING); break; } } // Driver-based GPS (I2C) else if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_BUS) { switch (gpsState.state) { default: case GPS_INITIALIZING: // Detect GPS unit if ((millis() - gpsState.lastStateSwitchMs) >= GPS_BUS_INIT_DELAY) { gpsResetSolution(); if (gpsProviders[gpsState.gpsConfig->provider].detect && gpsProviders[gpsState.gpsConfig->provider].detect()) { gpsState.hwVersion = 0; gpsState.autoConfigStep = 0; gpsState.autoConfigPosition = 0; gpsState.lastMessageMs = millis(); sensorsSet(SENSOR_GPS); gpsSetState(GPS_CHANGE_BAUD); } else { sensorsClear(SENSOR_GPS); } } break; case GPS_CHANGE_BAUD: case GPS_CHECK_VERSION: case GPS_CONFIGURE: case GPS_RECEIVING_DATA: gpsHandleProtocol(); if (millis() - gpsState.lastMessageMs > GPS_TIMEOUT) { // remove GPS from capability gpsSetState(GPS_LOST_COMMUNICATION); } break; case GPS_LOST_COMMUNICATION: // No valid data from GPS unit, cause re-init and re-detection gpsStats.timeouts++; DISABLE_STATE(GPS_FIX); gpsSol.fixType = GPS_NO_FIX; gpsSetState(GPS_INITIALIZING); break; } } else { // GPS_TYPE_NA } #endif }
void mixerUsePWMIOConfiguration(pwmIOConfiguration_t *pwmIOConfiguration) { int i; motorCount = 0; servoCount = pwmIOConfiguration->servoCount; if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { // load custom mixer into currentMixer for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { // check if done if (customMixers[i].throttle == 0.0f) break; currentMixer[i] = customMixers[i]; motorCount++; } } else { motorCount = mixers[currentMixerMode].motorCount; // copy motor-based mixers if (mixers[currentMixerMode].motor) { for (i = 0; i < motorCount; i++) currentMixer[i] = mixers[currentMixerMode].motor[i]; } } if (useServo) { servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; if (servoMixers[currentMixerMode].rule) { for (i = 0; i < servoRuleCount; i++) currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; } } // in 3D mode, mixer gain has to be halved if (feature(FEATURE_3D)) { if (motorCount > 1) { for (i = 0; i < motorCount; i++) { currentMixer[i].pitch *= 0.5f; currentMixer[i].roll *= 0.5f; currentMixer[i].yaw *= 0.5f; } } } // set flag that we're on something with wings if (currentMixerMode == MIXER_FLYING_WING || currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE ) { ENABLE_STATE(FIXED_WING); if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) { loadCustomServoMixer(); } } else { DISABLE_STATE(FIXED_WING); if (currentMixerMode == MIXER_CUSTOM_TRI) { loadCustomServoMixer(); } } mixerResetDisarmedMotors(); }
void onGpsNewData(void) { int axis; static uint32_t nav_loopTimer; uint16_t speed; if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) { return; } if (!ARMING_FLAG(ARMED)) DISABLE_STATE(GPS_FIX_HOME); if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) GPS_reset_home_position(); // Apply moving average filter to GPS data #if defined(GPS_FILTERING) GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH; for (axis = 0; axis < 2; axis++) { GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t // How close we are to a degree line ? its the first three digits from the fractions of degree // later we use it to Check if we are close to a degree line, if yes, disable averaging, fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000; GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index]; GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000); GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index]; GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000); if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode... if (fraction3[axis] > 1 && fraction3[axis] < 999) GPS_coord[axis] = GPS_filtered[axis]; } } #endif // // Calculate time delta for navigation loop, range 0-1.0f, in seconds // // Time for calculating x,y speed and navigation pids dTnav = (float)(millis() - nav_loopTimer) / 1000.0f; nav_loopTimer = millis(); // prevent runup from bad GPS dTnav = min(dTnav, 1.0f); GPS_calculateDistanceAndDirectionToHome(); // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating GPS_calc_velocity(); if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) { // we are navigating // gps nav calculations, these are common for nav and poshold GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); switch (nav_mode) { case NAV_MODE_POSHOLD: // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_poshold(); break; case NAV_MODE_WP: speed = GPS_calc_desired_speed(gpsProfile->nav_speed_max, NAV_SLOW_NAV); // slow navigation // use error as the desired rate towards the target // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_nav_rate(speed); // Tail control if (gpsProfile->nav_controls_heading) { if (NAV_TAIL_FIRST) { magHold = wrap_18000(nav_bearing - 18000) / 100; } else { magHold = nav_bearing / 100; } } // Are we there yet ?(within x meters of the destination) if ((wp_distance <= gpsProfile->gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode nav_mode = NAV_MODE_POSHOLD; if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; } } break; default: break; } } //end of gps calcs }
static bool gpsNewFrameNMEA(char c) { typedef struct gpsdata_s { int32_t latitude; int32_t longitude; uint8_t numSat; uint16_t altitude; uint16_t speed; uint16_t ground_course; } gpsdata_t; static gpsdata_t gps_Msg; uint8_t frameOK = 0; static uint8_t param = 0, offset = 0, parity = 0; static char string[15]; static uint8_t checksum_param, gps_frame = NO_FRAME; switch (c) { case '$': param = 0; offset = 0; parity = 0; break; case ',': case '*': string[offset] = 0; if (param == 0) { //frame identification gps_frame = NO_FRAME; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') gps_frame = FRAME_GGA; if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') gps_frame = FRAME_RMC; } switch (gps_frame) { case FRAME_GGA: //************* GPGGA FRAME parsing switch (param) { // case 1: // Time information // break; case 2: gps_Msg.latitude = GPS_coord_to_degrees(string); break; case 3: if (string[0] == 'S') gps_Msg.latitude *= -1; break; case 4: gps_Msg.longitude = GPS_coord_to_degrees(string); break; case 5: if (string[0] == 'W') gps_Msg.longitude *= -1; break; case 6: if (string[0] > '0') { ENABLE_STATE(GPS_FIX); } else { DISABLE_STATE(GPS_FIX); } break; case 7: gps_Msg.numSat = grab_fields(string, 0); break; case 9: gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis break; } break; case FRAME_RMC: //************* GPRMC FRAME parsing switch (param) { case 7: gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis break; case 8: gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10 break; } break; } param++; offset = 0; if (c == '*') checksum_param = 1; else parity ^= c; break; case '\r': case '\n': if (checksum_param) { //parity checksum uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); if (checksum == parity) { switch (gps_frame) { case FRAME_GGA: frameOK = 1; if (STATE(GPS_FIX)) { GPS_coord[LAT] = gps_Msg.latitude; GPS_coord[LON] = gps_Msg.longitude; GPS_numSat = gps_Msg.numSat; GPS_altitude = gps_Msg.altitude; } break; case FRAME_RMC: GPS_speed = gps_Msg.speed; GPS_ground_course = gps_Msg.ground_course; break; } // end switch } } checksum_param = 0; break; default: if (offset < 15) string[offset++] = c; if (!checksum_param) parity ^= c; } return frameOK; }