static void updateLEDs(void)
{
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating() || (averageSystemLoadPercent > 100)) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
}
warningLedUpdate();
}
}
void annexCode(void)
{
int32_t throttleValue;
// Compute ROLL PITCH and YAW command
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband);
rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband);
rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, currentProfile->rcControlsConfig.yaw_deadband);
//Compute THROTTLE command
throttleValue = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
throttleValue = (uint32_t)(throttleValue - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);
if (FLIGHT_MODE(HEADFREE_MODE)) {
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
const float cosDiff = cos_approx(radDiff);
const float sinDiff = sin_approx(radDiff);
const 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 (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating() || isSystemOverloaded()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#if defined(NAV)
if (naivationBlockArming()) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#endif
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);
}
void TouchTracker::Calibrator::cancel()
{
if(isCalibrating())
{
mActive = false;
MLConsole() << "\nCalibration cancelled.\n";
}
}
//--------------------------------------------------------------
string ofxOpenNIUser::getDebugInfo(){
ostringstream info;
info << "XnUserID: " << ofToString(XnID) << " ";
info << "autoCalibrate: " << boolToString(bUseAutoCalibration) << " ";
info << "isFound: " << boolToString(isFound()) << " ";
info << "isTracking: " << boolToString(isTracking()) << " ";
info << "isSkeleton: " << boolToString(isSkeleton()) << " ";
info << "isCalibrating: " << boolToString(isCalibrating()) << endl;
info << "useMaskPixels: " << boolToString(getUseMaskPixels()) << " ";
info << "useMaskTexture: " << boolToString(getUseMaskTexture()) << " ";
info << "usePointCloud: " << boolToString(getUsePointCloud()) << " ";
if (bUsePointCloud) {
info << "pointCloudDrawSize: " << ofToString(pointCloudDrawSize) << " ";
info << "pointCloudResolution: " << ofToString(pointCloudResolution) << " ";
}
return info.str();
}
armingPreventedReason_e getArmingPreventionBlinkMask(void)
{
if (isCalibrating()) {
return ARM_PREV_CALIB;
}
if (rcModeIsActive(BOXFAILSAFE) || failsafePhase() == FAILSAFE_LANDED) {
return ARM_PREV_FAILSAFE;
}
if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) {
return ARM_PREV_ANGLE;
}
if (cliMode) {
return ARM_PREV_CLI;
}
if (isSystemOverloaded()) {
return ARM_PREV_OVERLOAD;
}
return ARM_PREV_NONE;
}
void annexCode(void)
{
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 (rcModeIsActive(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);
}
static void updateLEDs(void)
{
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (rcModeIsActive(BOXARM) == 0) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating() || isSystemOverloaded()) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
uint32_t nextBlinkMask = getArmingPreventionBlinkMask();
warningLedSetBlinkMask(nextBlinkMask);
warningLedUpdate();
}
}
void annexCode(void)
{
int32_t tmp, tmp2;
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 (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (rcModeIsActive(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);
}
void annexCode(void)
{
int32_t tmp, tmp2;
int32_t axis, prop1, prop2;
static uint8_t batteryWarningEnabled = false;
static uint8_t vbatTimer = 0;
static int32_t vbatCycleTime = 0;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < currentProfile.tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentProfile.dynThrPID * (rcData[THROTTLE] - currentProfile.tpa_breakpoint) / (2000 - currentProfile.tpa_breakpoint);
} else {
prop2 = 100 - currentProfile.dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile.deadband) {
if (tmp > currentProfile.deadband) {
tmp -= currentProfile.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
}
if (axis == YAW) {
if (currentProfile.yaw_deadband) {
if (tmp > currentProfile.yaw_deadband) {
tmp -= currentProfile.yaw_deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * 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;
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 (f.HEADFREE_MODE) {
float radDiff = degreesToRadians(heading - headFreeModeHold);
float cosDiff = cosf(radDiff);
float sinDiff = sinf(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 | FEATURE_CURRENT_METER)) {
vbatCycleTime += cycleTime;
if (!(++vbatTimer % VBATFREQ)) {
if (feature(FEATURE_VBAT)) {
updateBatteryVoltage();
batteryWarningEnabled = shouldSoundBatteryAlarm();
}
if (feature(FEATURE_CURRENT_METER)) {
updateCurrentMeter(vbatCycleTime);
}
vbatCycleTime = 0;
}
}
beepcodeUpdateState(batteryWarningEnabled);
if (f.ARMED) {
LED0_ON;
} else {
if (isCalibrating()) {
LED0_TOGGLE;
f.OK_TO_ARM = 0;
}
f.OK_TO_ARM = 1;
if (!f.SMALL_ANGLE) {
f.OK_TO_ARM = 0;
}
if (rcOptions[BOXAUTOTUNE]) {
f.OK_TO_ARM = 0;
}
if (f.OK_TO_ARM) {
disableWarningLed();
} else {
enableWarningLed(currentTime);
}
updateWarningLed(currentTime);
}
#ifdef TELEMETRY
checkTelemetryState();
#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 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 annexCode(void)
{
int32_t tmp;
for (int 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;
}
}
rcCommand[axis] = rcLookupPitchRoll(tmp);
} else if (axis == YAW) {
if (currentProfile->rcControlsConfig.yaw_deadband) {
if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
tmp -= currentProfile->rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = rcLookupYaw(tmp) * -1;
}
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]
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
if (FLIGHT_MODE(HEADFREE_MODE)) {
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
const float cosDiff = cos_approx(radDiff);
const float sinDiff = sin_approx(radDiff);
const 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 (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating() || isSystemOverloaded()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#if defined(NAV)
if (naivationBlockArming()) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#endif
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);
}
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);
debug[3] = 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();
}
}
debug[3] = ARMING_FLAG(OK_TO_ARM);
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);
}
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 = gpsSol.groundSpeed / 100.0f;
}
#endif
// select best source for altitude
#if defined(NAV)
mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
#elif defined(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(NAV_ALTHOLD_MODE))
mavCustomMode = 2; //Alt Hold
if (FLIGHT_MODE(NAV_RTH_MODE))
mavCustomMode = 6; //Return to Launch
if (FLIGHT_MODE(NAV_POSHOLD_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 if (isCalibrating()) {
mavSystemState = MAV_STATE_CALIBRATING;
}
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 updateArmingStatus(void)
{
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
// Check if the power on arming grace time has elapsed
if ((getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME) && (millis() >= systemConfig()->powerOnArmingGraceTime * 1000)) {
// If so, unset the grace time arming disable flag
unsetArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
}
// Clear the crash flip active status
flipOverAfterCrashMode = false;
// If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault
if (!isUsingSticksForArming()) {
static bool hadRx = false;
const bool haveRx = rxIsReceivingSignal();
const bool justGotRxBack = !hadRx && haveRx;
if (justGotRxBack && IS_RC_MODE_ACTIVE(BOXARM)) {
// If the RX has just started to receive a signal again and the arm switch is on, apply arming restriction
setArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY);
} else if (haveRx && !IS_RC_MODE_ACTIVE(BOXARM)) {
// If RX signal is OK and the arm switch is off, remove arming restriction
unsetArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY);
}
hadRx = haveRx;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
} else {
unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
}
if (calculateThrottleStatus() != THROTTLE_LOW) {
setArmingDisabled(ARMING_DISABLED_THROTTLE);
} else {
unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
}
if (!STATE(SMALL_ANGLE) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
setArmingDisabled(ARMING_DISABLED_ANGLE);
} else {
unsetArmingDisabled(ARMING_DISABLED_ANGLE);
}
if (averageSystemLoadPercent > 100) {
setArmingDisabled(ARMING_DISABLED_LOAD);
} else {
unsetArmingDisabled(ARMING_DISABLED_LOAD);
}
if (isCalibrating()) {
setArmingDisabled(ARMING_DISABLED_CALIBRATING);
} else {
unsetArmingDisabled(ARMING_DISABLED_CALIBRATING);
}
if (isModeActivationConditionPresent(BOXPREARM)) {
if (IS_RC_MODE_ACTIVE(BOXPREARM) && !ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
unsetArmingDisabled(ARMING_DISABLED_NOPREARM);
} else {
setArmingDisabled(ARMING_DISABLED_NOPREARM);
}
}
#ifdef USE_GPS_RESCUE
if (isModeActivationConditionPresent(BOXGPSRESCUE)) {
if (!gpsRescueConfig()->minSats || STATE(GPS_FIX_HOME) || ARMING_FLAG(WAS_EVER_ARMED)) {
unsetArmingDisabled(ARMING_DISABLED_GPS);
} else {
setArmingDisabled(ARMING_DISABLED_GPS);
}
}
#endif
if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) {
setArmingDisabled(ARMING_DISABLED_PARALYZE);
}
if (!isUsingSticksForArming()) {
/* Ignore ARMING_DISABLED_CALIBRATING if we are going to calibrate gyro on first arm */
bool ignoreGyro = armingConfig()->gyro_cal_on_first_arm
&& !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_CALIBRATING));
/* Ignore ARMING_DISABLED_THROTTLE (once arm switch is on) if we are in 3D mode */
bool ignoreThrottle = feature(FEATURE_3D)
&& !IS_RC_MODE_ACTIVE(BOX3D)
&& !flight3DConfig()->switched_mode3d
&& !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE));
#ifdef USE_RUNAWAY_TAKEOFF
if (!IS_RC_MODE_ACTIVE(BOXARM)) {
unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
}
#endif
// If arming is disabled and the ARM switch is on
if (isArmingDisabled()
&& !ignoreGyro
&& !ignoreThrottle
&& IS_RC_MODE_ACTIVE(BOXARM)) {
setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
} else if (!IS_RC_MODE_ACTIVE(BOXARM)) {
unsetArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
}
}
if (isArmingDisabled()) {
warningLedFlash();
} else {
warningLedDisable();
}
warningLedUpdate();
}
}
