void showSensorsPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static const char *format = "%s %5d %5d %5d";
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(" X Y Z");
if (sensors(SENSOR_ACC)) {
tfp_sprintf(lineBuffer, format, "ACC", accSmooth[X], accSmooth[Y], accSmooth[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
if (sensors(SENSOR_GYRO)) {
tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
#ifdef MAG
if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, format, "MAG", magADC[X], magADC[Y], magADC[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
#endif
tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
/*
uint8_t length;
ftoa(EstG.A[X], lineBuffer);
length = strlen(lineBuffer);
while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
lineBuffer[length++] = ' ';
lineBuffer[length+1] = 0;
}
ftoa(EstG.A[Y], lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
ftoa(EstG.A[Z], lineBuffer);
length = strlen(lineBuffer);
while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
lineBuffer[length++] = ' ';
lineBuffer[length+1] = 0;
}
ftoa(smallAngle, lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
*/
}
void annexCode(void)
{
int32_t tmp, tmp2;
int32_t axis, prop1 = 0, prop2;
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop2 = 100 - currentControlRateProfile->dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile->rcControlsConfig.deadband) {
if (tmp > currentProfile->rcControlsConfig.deadband) {
tmp -= currentProfile->rcControlsConfig.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) {
if (currentProfile->rcControlsConfig.yaw_deadband) {
if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
tmp -= currentProfile->rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
}
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
if (axis == YAW) {
PIDweight[axis] = 100;
}
else {
PIDweight[axis] = prop2;
}
if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
}
}
if (feature(FEATURE_CURRENT_METER)) {
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}
beeperUpdate(); //call periodic beeper handler
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
}
warningLedUpdate();
}
#ifdef TELEMETRY
telemetryCheckState();
#endif
handleSerial();
#ifdef GPS
if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTime);
}
#endif
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
void mavlinkSendPosition(void)
{
uint16_t msgLength;
uint8_t gpsFixType = 0;
if (!sensors(SENSOR_GPS))
return;
if (!STATE(GPS_FIX)) {
gpsFixType = 1;
}
else {
if (GPS_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
GPS_coord[LAT],
// lon Longitude in 1E7 degrees
GPS_coord[LON],
// alt Altitude in 1E3 meters (millimeters) above MSL
GPS_altitude * 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
GPS_speed,
// cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
GPS_ground_course * 10,
// satellites_visible Number of satellites visible. If unknown, set to 255
GPS_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
GPS_coord[LAT],
// lon Longitude in 1E7 degrees
GPS_coord[LON],
// alt Altitude in 1E3 meters (millimeters) above MSL
GPS_altitude * 1000,
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
#if defined(BARO) || defined(SONAR)
(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() * 10 : GPS_altitude * 1000,
#else
GPS_altitude * 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);
}
void mavlinkSendHUDAndHeartbeat(void)
{
uint16_t msgLength;
float mavAltitude = 0;
float mavGroundSpeed = 0;
float mavAirSpeed = 0;
float mavClimbRate = 0;
#if defined(GPS)
// use ground speed if source available
if (sensors(SENSOR_GPS)) {
mavGroundSpeed = GPS_speed / 100.0f;
}
#endif
// select best source for altitude
#if defined(BARO) || defined(SONAR)
if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
// Baro or sonar generally is a better estimate of altitude than GPS MSL altitude
mavAltitude = altitudeHoldGetEstimatedAltitude() / 100.0;
}
#if defined(GPS)
else if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
}
#endif
#elif defined(GPS)
if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
}
#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(masterConfig.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) || FLIGHT_MODE(SONAR_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);
}
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;
}
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
int axis, prop;
int32_t error, errorAngle;
int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static int32_t delta1[3], delta2[3];
int32_t deltaSum;
int32_t delta;
UNUSED(controlRateConfig);
// **** PITCH & ROLL & YAW PID ****
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
// observe max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
#ifdef AUTOTUNE
if (shouldAutotune()) {
errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
}
#endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
}
if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroADC[axis] / 4;
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if ((ABS(gyroADC[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100))
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64;
}
if (FLIGHT_MODE(HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
} else {
if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
axisPID_D[axis] = -DTerm;
#endif
}
/*
* processRx called from taskUpdateRxMain
*/
bool processRx(timeUs_t currentTimeUs)
{
static bool armedBeeperOn = false;
if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
return false;
}
// 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))
disarm();
}
updateRSSI(currentTimeUs);
if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
failsafeStartMonitoring();
}
failsafeUpdateState();
const throttleStatus_e throttleStatus = calculateThrottleStatus();
const uint8_t throttlePercent = calculateThrottlePercent();
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (throttlePercent >= rxConfig()->airModeActivateThreshold) {
airmodeIsActivated = true; // Prevent Iterm from being reset
}
} else {
airmodeIsActivated = false;
}
/* 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 */
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
pidResetITerm();
if (currentPidProfile->pidAtMinThrottle)
pidStabilisationState(PID_STABILISATION_ON);
else
pidStabilisationState(PID_STABILISATION_OFF);
} else {
pidStabilisationState(PID_STABILISATION_ON);
}
#ifdef USE_RUNAWAY_TAKEOFF
// If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle
// is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected
// to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low.
// Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable
// prevention for the remainder of the battery.
if (ARMING_FLAG(ARMED)
&& pidConfig()->runaway_takeoff_prevention
&& !runawayTakeoffCheckDisabled
&& !flipOverAfterCrashMode
&& !runawayTakeoffTemporarilyDisabled
&& !STATE(FIXED_WING)) {
// Determine if we're in "flight"
// - motors running
// - throttle over runaway_takeoff_deactivate_throttle_percent
// - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent
// - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
bool inStableFlight = false;
if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running?
const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle;
const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT);
if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit))
&& (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) {
inStableFlight = true;
if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs;
}
}
}
// If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds
if (inStableFlight) {
if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs;
}
uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay;
// at high throttle levels reduce deactivation delay by 50%
if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) {
deactivateDelay = deactivateDelay / 2;
}
if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) {
runawayTakeoffCheckDisabled = true;
}
} else {
if (runawayTakeoffDeactivateUs != 0) {
runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs);
}
runawayTakeoffDeactivateUs = 0;
}
if (runawayTakeoffDeactivateUs == 0) {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000);
} else {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000);
}
} else {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE);
}
#endif
// 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)
&& !feature(FEATURE_3D)
&& !isAirmodeActive()
) {
if (isUsingSticksForArming()) {
if (throttleStatus == THROTTLE_LOW) {
if (armingConfig()->auto_disarm_delay != 0
&& (int32_t)(disarmAt - millis()) < 0
) {
// auto-disarm configured and delay is over
disarm();
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();
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
}
updateActivatedModes();
#ifdef USE_DSHOT
/* Enable beep warning when the crash flip mode is active */
if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
beeper(BEEPER_CRASH_FLIP_MODE);
}
#endif
if (!cliMode) {
updateAdjustmentStates();
processRcAdjustments(currentControlRateProfile);
}
bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (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);
}
#ifdef USE_GPS_RESCUE
if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) {
if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
}
#endif
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
LED1_ON;
// increase frequency of attitude task to reduce drift when in angle or horizon mode
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
} else {
LED1_OFF;
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
}
if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
}
#if defined(USE_ACC) || defined(USE_MAG)
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
#if defined(USE_GPS) || defined(USE_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);
}
#endif
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)) {
if (imuQuaternionHeadfreeOffsetSet()){
beeper(BEEPER_RX_SET);
}
}
}
#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 USE_TELEMETRY
static bool sharedPortTelemetryEnabled = false;
if (feature(FEATURE_TELEMETRY)) {
bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY));
if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) {
mspSerialReleaseSharedTelemetryPorts();
telemetryCheckState();
sharedPortTelemetryEnabled = true;
} else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) {
// the telemetry state must be checked immediately so that shared serial ports are released.
telemetryCheckState();
mspSerialAllocatePorts();
sharedPortTelemetryEnabled = false;
}
}
#endif
#ifdef USE_VTX_CONTROL
vtxUpdateActivatedChannel();
if (canUpdateVTX()) {
handleVTXControlButton();
}
#endif
#ifdef USE_ACRO_TRAINER
pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC));
#endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER
if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) {
beeper(BEEPER_RC_SMOOTHING_INIT_FAIL);
}
#endif
pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
return true;
}
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 annexCode(void)
{
int32_t tmp, tmp2;
int32_t axis, prop1 = 0, prop2;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop2 = 100 - currentControlRateProfile->dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (rcControlsConfig()->deadband) {
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) {
if (rcControlsConfig()->yaw_deadband) {
if (tmp > rcControlsConfig()->yaw_deadband) {
tmp -= rcControlsConfig()->yaw_deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -rcControlsConfig()->yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
}
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100;
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
if (axis == YAW) {
PIDweight[axis] = 100;
}
else {
PIDweight[axis] = prop2;
}
if (rcData[axis] < rxConfig()->midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating() || isSystemOverloaded()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
}
warningLedUpdate();
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
/*
This function processes RX dependent coefficients when new RX commands are available
Those are: TPA, throttle expo
*/
void processRxDependentCoefficients(void) {
int32_t tmp, tmp2;
int32_t axis, prop1 = 0, prop2;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop2 = 100 - currentControlRateProfile->dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (rcControlsConfig()->deadband) {
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) {
if (rcControlsConfig()->yaw_deadband) {
if (tmp > rcControlsConfig()->yaw_deadband) {
tmp -= rcControlsConfig()->yaw_deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -rcControlsConfig()->yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
}
#ifndef SKIP_PID_MW23
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100;
#endif
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
if (axis == YAW) {
PIDweight[axis] = 100;
}
else {
PIDweight[axis] = prop2;
}
if (rcData[axis] < rxConfig()->midrc)
rcCommand[axis] = -rcCommand[axis];
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
}
void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput)
{
const timeMs_t currentTimeMs = millis();
const float absDesiredRateDps = fabsf(desiredRateDps);
float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
pidAutotuneState_e newState;
// Use different max desired rate in ANGLE for pitch and roll
// Maximum reasonable error in ANGLE mode is 200% of angle inclination (control dublet), but we are conservative and tune for control singlet.
if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) {
float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
maxDesiredRate = MIN(maxDesiredRate, maxDesiredRateInAngleMode);
}
if (fabsf(pidOutput) >= pidProfile()->pidSumLimit) {
// If we have saturated the pid output by P+FF don't increase the gain
tuneCurrent[axis].pidSaturated = true;
}
if (absDesiredRateDps < (pidAutotuneConfig()->fw_max_rate_threshold / 100.0f) * maxDesiredRate) {
// We can make decisions only when we are demanding at least 50% of max configured rate
newState = DEMAND_TOO_LOW;
}
else if (fabsf(reachedRateDps) > absDesiredRateDps) {
newState = DEMAND_OVERSHOOT;
}
else {
newState = DEMAND_UNDERSHOOT;
}
if (newState != tuneCurrent[axis].state) {
const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime;
bool gainsUpdated = false;
switch (tuneCurrent[axis].state) {
case DEMAND_TOO_LOW:
break;
case DEMAND_OVERSHOOT:
if (stateTimeMs >= pidAutotuneConfig()->fw_overshoot_time) {
tuneCurrent[axis].gainD = tuneCurrent[axis].gainD * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainD < AUTOTUNE_FIXED_WING_MIN_FF) {
tuneCurrent[axis].gainD = AUTOTUNE_FIXED_WING_MIN_FF;
}
gainsUpdated = true;
}
break;
case DEMAND_UNDERSHOOT:
if (stateTimeMs >= pidAutotuneConfig()->fw_undershoot_time && !tuneCurrent[axis].pidSaturated) {
tuneCurrent[axis].gainD = tuneCurrent[axis].gainD * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainD > AUTOTUNE_FIXED_WING_MAX_FF) {
tuneCurrent[axis].gainD = AUTOTUNE_FIXED_WING_MAX_FF;
}
gainsUpdated = true;
}
break;
}
if (gainsUpdated) {
// Set P-gain to 10% of FF gain (quite agressive - FIXME)
tuneCurrent[axis].gainP = tuneCurrent[axis].gainD * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
// Set integrator gain to reach the same response as FF gain in 0.667 second
tuneCurrent[axis].gainI = (tuneCurrent[axis].gainD / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f);
autotuneUpdateGains(tuneCurrent);
switch (axis) {
case FD_ROLL:
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D, tuneCurrent[axis].gainD);
break;
case FD_PITCH:
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D, tuneCurrent[axis].gainD);
break;
case FD_YAW:
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D, tuneCurrent[axis].gainD);
break;
}
}
// Change state and reset saturation flag
tuneCurrent[axis].state = newState;
tuneCurrent[axis].stateEnterTime = currentTimeMs;
tuneCurrent[axis].pidSaturated = false;
}
}