/*
* Set the tun MTU dynamically.
*/
void
frame_set_mtu_dynamic (struct frame *frame, int mtu, unsigned int flags)
{
#ifdef ENABLE_DEBUG
const int orig_mtu = mtu;
const int orig_link_mtu_dynamic = frame->link_mtu_dynamic;
#endif
ASSERT (mtu >= 0);
if (flags & SET_MTU_TUN)
mtu += TUN_LINK_DELTA (frame);
if (!(flags & SET_MTU_UPPER_BOUND) || mtu < frame->link_mtu_dynamic)
{
frame->link_mtu_dynamic = constrain_int (
mtu,
EXPANDED_SIZE_MIN (frame),
EXPANDED_SIZE (frame));
}
dmsg (D_MTU_DEBUG, "MTU DYNAMIC mtu=%d, flags=%u, %d -> %d",
orig_mtu,
flags,
orig_link_mtu_dynamic,
frame->link_mtu_dynamic);
}
void Sonar_update(void)
{
static int16_t LastGoodSonarAlt = -1; // Initialize with errorvalue
static uint32_t AcceptTimer;
static uint8_t Errorcnt = 0; // This is compared to SonarErrorLimit
int16_t LastSonarAlt = sonarAlt; // Save Last Alt here for comparison
uint8_t tilt;
int32_t newdata = GetSnr(); // Keep it running for disconnect detection
if (!GroundAltInitialized) return; // Don't do Sonar until Baro initialized
if (newdata) // 100 ms with Maxbotix, 60ms with HC-SR04
{
sonarAlt = newdata;
tilt = 100 - constrain_int((int32_t)(TiltValue * 100.0f), 0, 100); // We don't care for upsidedownstuff, because althold is disabled than anyway
if (cfg.snr_dbg) { debug[1] = tilt; debug[2] = sonarAlt; }// Give raw tilt & sonar in debugmode
if (sonarAlt >= cfg.snr_min && sonarAlt <= cfg.snr_max && tilt < cfg.snr_tilt)
{
LastGoodSonarAlt = sonarAlt;
Errorcnt = SonarBreach = 0; // 0 = Breach unknown, 1 = breached lower limit, 2 = breached upper limit (not used)
}
else
{ // So sonarvalues are not sane here
Errorcnt = min(Errorcnt + 1, SonarErrorLimit); // Increase Errorcount within upper limit
if (tilt < cfg.snr_tilt && sonarAlt != -1) // Determine Limit breach type independent of tilt
{
if (sonarAlt <= cfg.snr_min) SonarBreach = 1; // We breached lower limit
if (sonarAlt >= cfg.snr_max) SonarBreach = 2; // We breached upper limit
}
else SonarBreach = 0; // 0 = Breach unknown, 1 = breached lower limit, 2 = breached upper limit (not used)
if (Errorcnt != SonarErrorLimit) sonarAlt = LastGoodSonarAlt; // Bridge error with last value, when it's -1 we take care later
else sonarAlt = -1;
}
if (LastSonarAlt != -1 && sonarAlt != -1 && cfg.snr_diff && abs_int(sonarAlt - (int32_t)LastSonarAlt) > cfg.snr_diff) // Too much Difference between reads?
sonarAlt = -1;
if (sonarAlt < 0) // Handle error here separately
{
LastGoodSonarAlt = -1;
SonarStatus = 0;
}
else
{
switch(SonarStatus)
{
case 0:
SonarStatus++; // Definition of "SonarStatus" 0 = no contact, 1 = Made first contact, 2 = Steady contact
AcceptTimer = currentTimeMS + 700; // Set 700 ms timeout before signalizing "steady contact" this exceeds our "bridging" from above
break;
case 1:
if (currentTimeMS >= AcceptTimer) SonarStatus++; // 2 = Steady contact // imu/getEstimatedAltitude will be happy to know
default:
break;
}
}
}
if (cfg.snr_dbg) debug[0] = sonarAlt; // Display Sonaralt like seen by althold
}
bool
set_debug_level (const int level, const unsigned int flags)
{
const int ceiling = 15;
if (level >= 0 && level <= ceiling)
{
x_debug_level = level;
return true;
}
else if (flags & SDL_CONSTRAIN)
{
x_debug_level = constrain_int (level, 0, ceiling);
return true;
}
return false;
}
void
tls_ctx_restrict_ciphers(struct tls_root_ctx *ctx, const char *ciphers)
{
size_t begin_of_cipher, end_of_cipher;
const char *current_cipher;
size_t current_cipher_len;
const tls_cipher_name_pair *cipher_pair;
const size_t openssl_ciphers_size = 4096;
char openssl_ciphers[openssl_ciphers_size];
size_t openssl_ciphers_len = 0;
openssl_ciphers[0] = '\0';
ASSERT(NULL != ctx);
// Translate IANA cipher suite names to OpenSSL names
begin_of_cipher = end_of_cipher = 0;
for (; begin_of_cipher < strlen(ciphers); begin_of_cipher = end_of_cipher) {
end_of_cipher += strcspn(&ciphers[begin_of_cipher], ":");
cipher_pair = tls_get_cipher_name_pair(&ciphers[begin_of_cipher], end_of_cipher - begin_of_cipher);
if (NULL == cipher_pair)
{
// No translation found, use original
current_cipher = &ciphers[begin_of_cipher];
current_cipher_len = end_of_cipher - begin_of_cipher;
// Issue warning on missing translation
// %.*s format specifier expects length of type int, so guarantee
// that length is small enough and cast to int.
msg (M_WARN, "No valid translation found for TLS cipher '%.*s'",
constrain_int(current_cipher_len, 0, 256), current_cipher);
}
else
{
// Use OpenSSL name
current_cipher = cipher_pair->openssl_name;
current_cipher_len = strlen(current_cipher);
if (end_of_cipher - begin_of_cipher == current_cipher_len &&
0 == memcmp (&ciphers[begin_of_cipher], cipher_pair->openssl_name, end_of_cipher - begin_of_cipher))
{
// Non-IANA name used, show warning
msg (M_WARN, "Deprecated TLS cipher name '%s', please use IANA name '%s'", cipher_pair->openssl_name, cipher_pair->iana_name);
}
}
// Make sure new cipher name fits in cipher string
if (((openssl_ciphers_size-1) - openssl_ciphers_len) < current_cipher_len) {
msg(M_SSLERR, "Failed to set restricted TLS cipher list, too long (>%zu).", openssl_ciphers_size-1);
}
// Concatenate cipher name to OpenSSL cipher string
memcpy(&openssl_ciphers[openssl_ciphers_len], current_cipher, current_cipher_len);
openssl_ciphers_len += current_cipher_len;
openssl_ciphers[openssl_ciphers_len] = ':';
openssl_ciphers_len++;
end_of_cipher++;
}
if (openssl_ciphers_len > 0)
openssl_ciphers[openssl_ciphers_len-1] = '\0';
// Set OpenSSL cipher list
if(!SSL_CTX_set_cipher_list(ctx->ctx, openssl_ciphers))
msg(M_SSLERR, "Failed to set restricted TLS cipher list: %s", openssl_ciphers);
}
void SensorDetectAndINI(void) // "enabledSensors" is "0" in config.c so all sensors disabled per default
{
int16_t deg, min;
uint8_t sig = 0;
bool ack = false;
bool haveMpu6k = false;
GyroScale16 = (16.0f / 16.4f) * RADX; // GYRO part. RAD per SECOND, take into account that gyrodata are div by X
if (mpu6050Detect(&acc, &gyro)) // Autodetect gyro hardware. We have MPU3050 or MPU6050.
{
haveMpu6k = true; // this filled up acc.* struct with init values
}
else if (l3g4200dDetect(&gyro))
{
havel3g4200d = true;
GyroScale16 = (16.0f / 14.2857f) * RADX; // GYRO part. RAD per SECOND, take into account that gyrodata are div by X
}
else if (!mpu3050Detect(&gyro))
{
failureMode(3); // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
}
sensorsSet(SENSOR_ACC); // ACC part. Will be cleared if not available
retry:
switch (cfg.acc_hdw)
{
case 0: // autodetect
case 1: // ADXL345
if (adxl345Detect(&acc)) accHardware = ACC_ADXL345;
if (cfg.acc_hdw == ACC_ADXL345) break;
case 2: // MPU6050
if (haveMpu6k)
{
mpu6050Detect(&acc, &gyro); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050;
if (cfg.acc_hdw == ACC_MPU6050) break;
}
case 3: // MMA8452
if (mma8452Detect(&acc))
{
accHardware = ACC_MMA8452;
if (cfg.acc_hdw == ACC_MMA8452) break;
}
}
if (accHardware == ACC_DEFAULT) // Found anything? Check if user f****d up or ACC is really missing.
{
if (cfg.acc_hdw > ACC_DEFAULT)
{
cfg.acc_hdw = ACC_DEFAULT; // Nothing was found and we have a forced sensor type. User probably chose a sensor that isn't present.
goto retry;
}
else sensorsClear(SENSOR_ACC); // We're really screwed
}
if (sensors(SENSOR_ACC)) acc.init();
if (haveMpu6k && accHardware == ACC_MPU6050) MpuSpecial = true;
else MpuSpecial = false;
if (feature(FEATURE_PASS)) return; // Stop here we need just ACC for Vibrationmonitoring if present
if (feature(FEATURE_GPS) && !SerialRCRX) gpsInit(cfg.gps_baudrate);// SerialRX and GPS can not coexist.
gyro.init(); // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
if (havel3g4200d) l3g4200dConfig();
else if (!haveMpu6k) mpu3050Config();
Gyro_Calibrate(); // Do Gyrocalibration here (is blocking), provides nice Warmuptime for the following rest!
#ifdef MAG
if (hmc5883lDetect())
{
sensorsSet(SENSOR_MAG);
hmc5883lInit(magCal); // Crashpilot: Calculate Gains / Scale
deg = cfg.mag_dec / 100; // calculate magnetic declination
min = cfg.mag_dec % 100;
magneticDeclination = ((float)deg + ((float)min / 60.0f));// heading is in decimaldeg units NO 0.1 deg shit here
}
#endif
#ifdef BARO // No delay necessary since Gyrocal blocked a lot already
ack = i2cRead(0x77, 0x77, 1, &sig); // Check Baroadr.(MS & BMP) BMP will say hello here, MS not
if ( ack) ack = bmp085Detect(&baro); // Are we really dealing with BMP?
if (!ack) ack = ms5611Detect(&baro); // No, Check for MS Baro
if (ack) sensorsSet(SENSOR_BARO);
if(cfg.esc_nfly) ESCnoFlyThrottle = constrain_int(cfg.esc_nfly, cfg.esc_min, cfg.esc_max); // Set the ESC PWM signal threshold for not flyable RPM
else ESCnoFlyThrottle = cfg.esc_min + (((cfg.esc_max - cfg.esc_min) * 5) / 100); // If not configured, take 5% above esc_min
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) Sonar_init(); // Initialize Sonars here depending on Rc configuration.
SonarLandWanted = cfg.snr_land; // Variable may be overwritten by failsave
#endif
MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
}
