/* * GPS G-frame 5Hhz at > 2400 baud * LAT LON SPD ALT SAT/FIX */ static void ltm_gframe(void) { #if defined(USE_GPS) uint8_t gps_fix_type = 0; int32_t ltm_alt; if (!sensors(SENSOR_GPS)) return; if (!STATE(GPS_FIX)) gps_fix_type = 1; else if (gpsSol.numSat < 5) gps_fix_type = 2; else gps_fix_type = 3; ltm_initialise_packet('G'); ltm_serialise_32(gpsSol.llh.lat); ltm_serialise_32(gpsSol.llh.lon); ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100)); #if defined(USE_BARO) || defined(USE_RANGEFINDER) ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100; #else ltm_alt = gpsSol.llh.alt * 100; #endif ltm_serialise_32(ltm_alt); ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type); ltm_finalise(); #endif }
static void osdUpdateStats(void) { int16_t value = 0; #ifdef USE_GPS switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: value = CM_S_TO_MPH(gpsSol.groundSpeed); break; default: value = CM_S_TO_KM_H(gpsSol.groundSpeed); break; } #endif if (stats.max_speed < value) { stats.max_speed = value; } value = getBatteryVoltage(); if (stats.min_voltage > value) { stats.min_voltage = value; } value = getAmperage() / 100; if (stats.max_current < value) { stats.max_current = value; } value = getRssiPercent(); if (stats.min_rssi > value) { stats.min_rssi = value; } int altitude = getEstimatedAltitude(); if (stats.max_altitude < altitude) { stats.max_altitude = altitude; } #ifdef USE_GPS if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { value = GPS_distanceToHome; if (stats.max_distance < GPS_distanceToHome) { stats.max_distance = GPS_distanceToHome; } } #endif }
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; }
void mavlinkSendHUDAndHeartbeat(void) { uint16_t msgLength; float mavAltitude = 0; float mavGroundSpeed = 0; float mavAirSpeed = 0; float mavClimbRate = 0; #if defined(USE_GPS) // use ground speed if source available if (sensors(SENSOR_GPS)) { mavGroundSpeed = gpsSol.groundSpeed / 100.0f; } #endif // select best source for altitude #if defined(USE_BARO) || defined(USE_RANGEFINDER) if (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) { // Baro or sonar generally is a better estimate of altitude than GPS MSL altitude mavAltitude = getEstimatedAltitude() / 100.0; } #if defined(USE_GPS) else if (sensors(SENSOR_GPS)) { // No sonar or baro, just display altitude above MLS mavAltitude = gpsSol.llh.alt; } #endif #elif defined(USE_GPS) if (sensors(SENSOR_GPS)) { // No sonar or baro, just display altitude above MLS mavAltitude = gpsSol.llh.alt; } #endif mavlink_msg_vfr_hud_pack(0, 200, &mavMsg, // airspeed Current airspeed in m/s mavAirSpeed, // groundspeed Current ground speed in m/s mavGroundSpeed, // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw), // throttle Current throttle setting in integer percent, 0 to 100 scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100), // alt Current altitude (MSL), in meters, if we have sonar or baro use them, otherwise use GPS (less accurate) mavAltitude, // climb Current climb rate in meters/second mavClimbRate); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; if (ARMING_FLAG(ARMED)) mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; uint8_t mavSystemType; switch (mixerConfig()->mixerMode) { case MIXER_TRI: mavSystemType = MAV_TYPE_TRICOPTER; break; case MIXER_QUADP: case MIXER_QUADX: case MIXER_Y4: case MIXER_VTAIL4: mavSystemType = MAV_TYPE_QUADROTOR; break; case MIXER_Y6: case MIXER_HEX6: case MIXER_HEX6X: mavSystemType = MAV_TYPE_HEXAROTOR; break; case MIXER_OCTOX8: case MIXER_OCTOFLATP: case MIXER_OCTOFLATX: mavSystemType = MAV_TYPE_OCTOROTOR; break; case MIXER_FLYING_WING: case MIXER_AIRPLANE: case MIXER_CUSTOM_AIRPLANE: mavSystemType = MAV_TYPE_FIXED_WING; break; case MIXER_HELI_120_CCPM: case MIXER_HELI_90_DEG: mavSystemType = MAV_TYPE_HELICOPTER; break; default: mavSystemType = MAV_TYPE_GENERIC; break; } // Custom mode for compatibility with APM OSDs uint8_t mavCustomMode = 1; // Acro by default if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { mavCustomMode = 0; //Stabilize mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; } if (FLIGHT_MODE(BARO_MODE)) { mavCustomMode = 2; //Alt Hold } if (FLIGHT_MODE(GPS_HOME_MODE)) { mavCustomMode = 6; //Return to Launch } if (FLIGHT_MODE(GPS_HOLD_MODE)) { mavCustomMode = 16; //Position Hold (Earlier called Hybrid) } uint8_t mavSystemState = 0; if (ARMING_FLAG(ARMED)) { if (failsafeIsActive()) { mavSystemState = MAV_STATE_CRITICAL; } else { mavSystemState = MAV_STATE_ACTIVE; } } else { mavSystemState = MAV_STATE_STANDBY; } mavlink_msg_heartbeat_pack(0, 200, &mavMsg, // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) mavSystemType, // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM MAV_AUTOPILOT_GENERIC, // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h mavModes, // custom_mode A bitfield for use for autopilot-specific flags. mavCustomMode, // system_status System status flag, see MAV_STATE ENUM mavSystemState); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); }
void mavlinkSendPosition(void) { uint16_t msgLength; uint8_t gpsFixType = 0; if (!sensors(SENSOR_GPS)) return; if (!STATE(GPS_FIX)) { gpsFixType = 1; } else { if (gpsSol.numSat < 5) { gpsFixType = 2; } else { gpsFixType = 3; } } mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) micros(), // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. gpsFixType, // lat Latitude in 1E7 degrees gpsSol.llh.lat, // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL gpsSol.llh.alt * 1000, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 gpsSol.groundSpeed, // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 gpsSol.groundCourse * 10, // satellites_visible Number of satellites visible. If unknown, set to 255 gpsSol.numSat); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); // Global position mavlink_msg_global_position_int_pack(0, 200, &mavMsg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) micros(), // lat Latitude in 1E7 degrees gpsSol.llh.lat, // lon Longitude in 1E7 degrees gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL gpsSol.llh.alt * 1000, // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) #if defined(USE_BARO) || defined(USE_RANGEFINDER) (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000, #else gpsSol.llh.alt * 1000, #endif // Ground X Speed (Latitude), expressed as m/s * 100 0, // Ground Y Speed (Longitude), expressed as m/s * 100 0, // Ground Z Speed (Altitude), expressed as m/s * 100 0, // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw) ); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg, // latitude Latitude (WGS84), expressed as * 1E7 GPS_home[LAT], // longitude Longitude (WGS84), expressed as * 1E7 GPS_home[LON], // altitude Altitude(WGS84), expressed as * 1000 0); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); }
static bool osdDrawSingleElement(uint8_t item) { if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) { return false; } uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]); uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]); char buff[OSD_ELEMENT_BUFFER_LENGTH] = ""; switch (item) { case OSD_RSSI_VALUE: { uint16_t osdRssi = getRssi() * 100 / 1024; // change range if (osdRssi >= 100) osdRssi = 99; tfp_sprintf(buff, "%c%2d", SYM_RSSI, osdRssi); break; } case OSD_MAIN_BATT_VOLTAGE: buff[0] = osdGetBatterySymbol(osdGetBatteryAverageCellVoltage()); tfp_sprintf(buff + 1, "%2d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT); break; case OSD_CURRENT_DRAW: { const int32_t amperage = getAmperage(); tfp_sprintf(buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP); break; } case OSD_MAH_DRAWN: tfp_sprintf(buff, "%4d%c", getMAhDrawn(), SYM_MAH); break; #ifdef USE_GPS case OSD_GPS_SATS: tfp_sprintf(buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat); break; case OSD_GPS_SPEED: // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K (M for MPH) switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: tfp_sprintf(buff, "%3dM", CM_S_TO_MPH(gpsSol.groundSpeed)); break; default: tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed)); break; } break; case OSD_GPS_LAT: // The SYM_LAT symbol in the actual font contains only blank, so we use the SYM_ARROW_NORTH osdFormatCoordinate(buff, SYM_ARROW_NORTH, gpsSol.llh.lat); break; case OSD_GPS_LON: // The SYM_LON symbol in the actual font contains only blank, so we use the SYM_ARROW_EAST osdFormatCoordinate(buff, SYM_ARROW_EAST, gpsSol.llh.lon); break; case OSD_HOME_DIR: if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { if (GPS_distanceToHome > 0) { const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw); buff[0] = osdGetDirectionSymbolFromHeading(h); } else { // We don't have a HOME symbol in the font, by now we use this buff[0] = SYM_THR1; } } else { // We use this symbol when we don't have a FIX buff[0] = SYM_COLON; } buff[1] = 0; break; case OSD_HOME_DIST: if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome); tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol()); } else { // We use this symbol when we don't have a FIX buff[0] = SYM_COLON; // overwrite any previous distance with blanks memset(buff + 1, SYM_BLANK, 6); buff[7] = '\0'; } break; #endif // GPS case OSD_COMPASS_BAR: memcpy(buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9); buff[9] = 0; break; case OSD_ALTITUDE: osdFormatAltitudeString(buff, getEstimatedAltitude()); break; case OSD_ITEM_TIMER_1: case OSD_ITEM_TIMER_2: osdFormatTimer(buff, true, true, item - OSD_ITEM_TIMER_1); break; case OSD_REMAINING_TIME_ESTIMATE: { const int mAhDrawn = getMAhDrawn(); const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn); if (mAhDrawn < 0.1 * osdConfig()->cap_alarm) { tfp_sprintf(buff, "--:--"); } else if (mAhDrawn > osdConfig()->cap_alarm) { tfp_sprintf(buff, "00:00"); } else { osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time); } break; } case OSD_FLYMODE: { if (FLIGHT_MODE(FAILSAFE_MODE)) { strcpy(buff, "!FS!"); } else if (FLIGHT_MODE(ANGLE_MODE)) { strcpy(buff, "STAB"); } else if (FLIGHT_MODE(HORIZON_MODE)) { strcpy(buff, "HOR "); } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) { strcpy(buff, "RESC"); } else if (isAirmodeActive()) { strcpy(buff, "AIR "); } else { strcpy(buff, "ACRO"); } break; } case OSD_ANTI_GRAVITY: { if (pidItermAccelerator() > 1.0f) { strcpy(buff, "AG"); } break; } case OSD_CRAFT_NAME: // This does not strictly support iterative updating if the craft name changes at run time. But since the craft name is not supposed to be changing this should not matter, and blanking the entire length of the craft name string on update will make it impossible to configure elements to be displayed on the right hand side of the craft name. //TODO: When iterative updating is implemented, change this so the craft name is only printed once whenever the OSD 'flight' screen is entered. if (strlen(pilotConfig()->name) == 0) { strcpy(buff, "CRAFT_NAME"); } else { unsigned i; for (i = 0; i < MAX_NAME_LENGTH; i++) { if (pilotConfig()->name[i]) { buff[i] = toupper((unsigned char)pilotConfig()->name[i]); } else { break; } } buff[i] = '\0'; } break; case OSD_THROTTLE_POS: buff[0] = SYM_THR; buff[1] = SYM_THR1; tfp_sprintf(buff + 2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); break; #if defined(USE_VTX_COMMON) case OSD_VTX_CHANNEL: { const char vtxBandLetter = vtx58BandLetter[vtxSettingsConfig()->band]; const char *vtxChannelName = vtx58ChannelNames[vtxSettingsConfig()->channel]; uint8_t vtxPower = vtxSettingsConfig()->power; const vtxDevice_t *vtxDevice = vtxCommonDevice(); if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) { vtxCommonGetPowerIndex(vtxDevice, &vtxPower); } tfp_sprintf(buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower); break; } #endif case OSD_CROSSHAIRS: buff[0] = SYM_AH_CENTER_LINE; buff[1] = SYM_AH_CENTER; buff[2] = SYM_AH_CENTER_LINE_RIGHT; buff[3] = 0; break; case OSD_ARTIFICIAL_HORIZON: { // Get pitch and roll limits in tenths of degrees const int maxPitch = osdConfig()->ahMaxPitch * 10; const int maxRoll = osdConfig()->ahMaxRoll * 10; const int rollAngle = constrain(attitude.values.roll, -maxRoll, maxRoll); int pitchAngle = constrain(attitude.values.pitch, -maxPitch, maxPitch); // Convert pitchAngle to y compensation value // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees pitchAngle = ((pitchAngle * 25) / maxPitch) - 41; // 41 = 4 * AH_SYMBOL_COUNT + 5 for (int x = -4; x <= 4; x++) { const int y = ((-rollAngle * x) / 64) - pitchAngle; if (y >= 0 && y <= 81) { displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + (y / AH_SYMBOL_COUNT), (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT))); } } return true; } case OSD_HORIZON_SIDEBARS: { // Draw AH sides const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; for (int y = -hudheight; y <= hudheight; y++) { displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION); displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION); } // AH level indicators displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT); displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT); return true; } case OSD_ROLL_PIDS: osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]); break; case OSD_PITCH_PIDS: osdFormatPID(buff, "PIT", ¤tPidProfile->pid[PID_PITCH]); break; case OSD_YAW_PIDS: osdFormatPID(buff, "YAW", ¤tPidProfile->pid[PID_YAW]); break; case OSD_POWER: tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000); break; case OSD_PIDRATE_PROFILE: tfp_sprintf(buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1); break; case OSD_WARNINGS: { #define OSD_WARNINGS_MAX_SIZE 11 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1) STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size); const batteryState_e batteryState = getBatteryState(); if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW"); break; } #ifdef USE_ADC_INTERNAL uint8_t coreTemperature = getCoreTemperatureCelsius(); if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) { char coreTemperatureWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, coreTemperatureWarningMsg); break; } #endif #ifdef USE_ESC_SENSOR // Show warning if we lose motor output, the ESC is overheating or excessive current draw if (feature(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) { char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; unsigned pos = 0; const char *title = "ESC"; // center justify message while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) { escWarningMsg[pos++] = ' '; } strcpy(escWarningMsg + pos, title); pos += strlen(title); unsigned i = 0; unsigned escWarningCount = 0; while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) { escSensorData_t *escData = getEscSensorData(i); const char motorNumber = '1' + i; // if everything is OK just display motor number else R, T or C char warnFlag = motorNumber; if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) { warnFlag = 'R'; } if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) { warnFlag = 'T'; } if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) { warnFlag = 'C'; } escWarningMsg[pos++] = warnFlag; if (warnFlag != motorNumber) { escWarningCount++; } i++; } escWarningMsg[pos] = '\0'; if (escWarningCount > 0) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg); } break; } #endif // Warn when in flip over after crash mode if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashMode()) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "CRASH FLIP"); break; } // Show most severe reason for arming being disabled if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE) && IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) { const armingDisableFlags_e flags = getArmingDisableFlags(); for (int i = 0; i < ARMING_DISABLE_FLAGS_COUNT; i++) { if (flags & (1 << i)) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[i]); break; } } break; } if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY"); break; } // Show warning if battery is not fresh if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK) && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "BATT < FULL"); break; } // Visual beeper if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && showVisualBeeper) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " * * * *"); break; } osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, NULL); break; } case OSD_AVG_CELL_VOLTAGE: { const int cellV = osdGetBatteryAverageCellVoltage(); buff[0] = osdGetBatterySymbol(cellV); tfp_sprintf(buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT); break; } case OSD_DEBUG: tfp_sprintf(buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]); break; case OSD_PITCH_ANGLE: case OSD_ROLL_ANGLE: { const int angle = (item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll; tfp_sprintf(buff, "%c%02d.%01d", angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10)); break; } case OSD_MAIN_BATT_USAGE: { // Set length of indicator bar #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered. // Calculate constrained value const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity); // Calculate mAh used progress const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS))); // Create empty battery indicator bar buff[0] = SYM_PB_START; for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) { buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY; } buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE; if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) { buff[1 + mAhUsedProgress] = SYM_PB_END; } buff[MAIN_BATT_USAGE_STEPS+2] = '\0'; break; } case OSD_DISARMED: if (!ARMING_FLAG(ARMED)) { tfp_sprintf(buff, "DISARMED"); } else { if (!lastArmState) { // previously disarmed - blank out the message one time tfp_sprintf(buff, " "); } } break; case OSD_NUMERICAL_HEADING: { const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw); tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading); break; } case OSD_NUMERICAL_VARIO: { const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario()); const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SOUTH : SYM_ARROW_NORTH; tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10)); break; } #ifdef USE_ESC_SENSOR case OSD_ESC_TMP: if (feature(FEATURE_ESC_SENSOR)) { tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); } break; case OSD_ESC_RPM: if (feature(FEATURE_ESC_SENSOR)) { tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm)); } break; #endif #ifdef USE_RTC_TIME case OSD_RTC_DATETIME: osdFormatRtcDateTime(&buff[0]); break; #endif #ifdef USE_OSD_ADJUSTMENTS case OSD_ADJUSTMENT_RANGE: tfp_sprintf(buff, "%s: %3d", adjustmentRangeName, adjustmentRangeValue); break; #endif #ifdef USE_ADC_INTERNAL case OSD_CORE_TEMPERATURE: tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); break; #endif default: return false; } displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); return true; }
void osdUpdateAlarms(void) { // This is overdone? int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100; if (getRssiPercent() < osdConfig()->rssi_alarm) { SET_BLINK(OSD_RSSI_VALUE); } else { CLR_BLINK(OSD_RSSI_VALUE); } if (getBatteryState() == BATTERY_OK) { CLR_BLINK(OSD_WARNINGS); CLR_BLINK(OSD_MAIN_BATT_VOLTAGE); CLR_BLINK(OSD_AVG_CELL_VOLTAGE); } else { SET_BLINK(OSD_WARNINGS); SET_BLINK(OSD_MAIN_BATT_VOLTAGE); SET_BLINK(OSD_AVG_CELL_VOLTAGE); } if (STATE(GPS_FIX) == 0) { SET_BLINK(OSD_GPS_SATS); } else { CLR_BLINK(OSD_GPS_SATS); } for (int i = 0; i < OSD_TIMER_COUNT; i++) { const uint16_t timer = osdConfig()->timers[i]; const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer)); const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us if (alarmTime != 0 && time >= alarmTime) { SET_BLINK(OSD_ITEM_TIMER_1 + i); } else { CLR_BLINK(OSD_ITEM_TIMER_1 + i); } } if (getMAhDrawn() >= osdConfig()->cap_alarm) { SET_BLINK(OSD_MAH_DRAWN); SET_BLINK(OSD_MAIN_BATT_USAGE); SET_BLINK(OSD_REMAINING_TIME_ESTIMATE); } else { CLR_BLINK(OSD_MAH_DRAWN); CLR_BLINK(OSD_MAIN_BATT_USAGE); CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE); } if (alt >= osdConfig()->alt_alarm) { SET_BLINK(OSD_ALTITUDE); } else { CLR_BLINK(OSD_ALTITUDE); } #ifdef USE_ESC_SENSOR if (feature(FEATURE_ESC_SENSOR)) { // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escDataCombined->temperature >= osdConfig()->esc_temp_alarm) { SET_BLINK(OSD_ESC_TMP); } else { CLR_BLINK(OSD_ESC_TMP); } } #endif }
/******************************************************************************* ** 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