mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
08b376f2a5
On bad (out-of-range) pulses; ROLL, PITCH, YAW will go to `mid_rc` and THROTTLE will go to `rx_min_usec` (to `mid_rc` for 3D mode). So these channels will no longer be set by the user directly. Fallback values for the aux switches (0 .. max) can be set with this version. Since these switches may trigger all kind of things, the user needs control over them in case of a RX failsafe event. A single flight control channel failure (first 4) when using parallel PWM is interpreted as a failure for all flight control channels (first 4), since the craft may be uncontrollable when one channel is down. (+4 squashed commit) Squashed commit: [dbfea9e] Apply fallback values also when serial_rx init failed and/or RX disconnected and/or no signal received. [b5a2ecd] Added get/set MSP commands for RXFAIL config Bumped API minor version up. [c0e31ce] minor change for coding standard [322705f] Added programmable RX channel defaults on rx lost Update #2
985 lines
30 KiB
C
985 lines
30 KiB
C
/*
|
|
* This file is part of Cleanflight.
|
|
*
|
|
* Cleanflight is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* Cleanflight is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#include "build_config.h"
|
|
|
|
#include "common/color.h"
|
|
#include "common/axis.h"
|
|
#include "common/maths.h"
|
|
|
|
#include "drivers/sensor.h"
|
|
#include "drivers/accgyro.h"
|
|
#include "drivers/compass.h"
|
|
#include "drivers/system.h"
|
|
#include "drivers/gpio.h"
|
|
#include "drivers/timer.h"
|
|
#include "drivers/pwm_rx.h"
|
|
#include "drivers/serial.h"
|
|
|
|
#include "sensors/sensors.h"
|
|
#include "sensors/gyro.h"
|
|
#include "sensors/compass.h"
|
|
#include "sensors/acceleration.h"
|
|
#include "sensors/barometer.h"
|
|
#include "sensors/boardalignment.h"
|
|
#include "sensors/battery.h"
|
|
|
|
#include "io/beeper.h"
|
|
#include "io/serial.h"
|
|
#include "io/gimbal.h"
|
|
#include "io/escservo.h"
|
|
#include "io/rc_controls.h"
|
|
#include "io/rc_curves.h"
|
|
#include "io/ledstrip.h"
|
|
#include "io/gps.h"
|
|
|
|
#include "rx/rx.h"
|
|
|
|
#include "telemetry/telemetry.h"
|
|
|
|
#include "flight/mixer.h"
|
|
#include "flight/pid.h"
|
|
#include "flight/imu.h"
|
|
#include "flight/failsafe.h"
|
|
#include "flight/altitudehold.h"
|
|
#include "flight/navigation.h"
|
|
|
|
#include "config/runtime_config.h"
|
|
#include "config/config.h"
|
|
|
|
#include "config/config_profile.h"
|
|
#include "config/config_master.h"
|
|
|
|
#define BRUSHED_MOTORS_PWM_RATE 16000
|
|
#define BRUSHLESS_MOTORS_PWM_RATE 400
|
|
|
|
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
|
|
|
|
#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
|
|
|
|
#if !defined(FLASH_SIZE)
|
|
#error "Flash size not defined for target. (specify in KB)"
|
|
#endif
|
|
|
|
|
|
#ifndef FLASH_PAGE_SIZE
|
|
#ifdef STM32F303xC
|
|
#define FLASH_PAGE_SIZE ((uint16_t)0x800)
|
|
#endif
|
|
|
|
#ifdef STM32F10X_MD
|
|
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
|
|
#endif
|
|
|
|
#ifdef STM32F10X_HD
|
|
#define FLASH_PAGE_SIZE ((uint16_t)0x800)
|
|
#endif
|
|
#endif
|
|
|
|
#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT)
|
|
#ifdef STM32F10X_MD
|
|
#define FLASH_PAGE_COUNT 128
|
|
#endif
|
|
|
|
#ifdef STM32F10X_HD
|
|
#define FLASH_PAGE_COUNT 128
|
|
#endif
|
|
#endif
|
|
|
|
#if defined(FLASH_SIZE)
|
|
#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE)
|
|
#endif
|
|
|
|
#if !defined(FLASH_PAGE_SIZE)
|
|
#error "Flash page size not defined for target."
|
|
#endif
|
|
|
|
#if !defined(FLASH_PAGE_COUNT)
|
|
#error "Flash page count not defined for target."
|
|
#endif
|
|
|
|
// use the last flash pages for storage
|
|
#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
|
|
|
|
master_t masterConfig; // master config struct with data independent from profiles
|
|
profile_t *currentProfile;
|
|
static uint32_t activeFeaturesLatch = 0;
|
|
|
|
static uint8_t currentControlRateProfileIndex = 0;
|
|
controlRateConfig_t *currentControlRateProfile;
|
|
|
|
static const uint8_t EEPROM_CONF_VERSION = 103;
|
|
|
|
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
|
{
|
|
accelerometerTrims->values.pitch = 0;
|
|
accelerometerTrims->values.roll = 0;
|
|
accelerometerTrims->values.yaw = 0;
|
|
}
|
|
|
|
static void resetPidProfile(pidProfile_t *pidProfile)
|
|
{
|
|
pidProfile->pidController = 0;
|
|
|
|
pidProfile->P8[ROLL] = 40;
|
|
pidProfile->I8[ROLL] = 30;
|
|
pidProfile->D8[ROLL] = 23;
|
|
pidProfile->P8[PITCH] = 40;
|
|
pidProfile->I8[PITCH] = 30;
|
|
pidProfile->D8[PITCH] = 23;
|
|
pidProfile->P8[YAW] = 85;
|
|
pidProfile->I8[YAW] = 45;
|
|
pidProfile->D8[YAW] = 0;
|
|
pidProfile->P8[PIDALT] = 50;
|
|
pidProfile->I8[PIDALT] = 0;
|
|
pidProfile->D8[PIDALT] = 0;
|
|
pidProfile->P8[PIDPOS] = 15; // POSHOLD_P * 100;
|
|
pidProfile->I8[PIDPOS] = 0; // POSHOLD_I * 100;
|
|
pidProfile->D8[PIDPOS] = 0;
|
|
pidProfile->P8[PIDPOSR] = 34; // POSHOLD_RATE_P * 10;
|
|
pidProfile->I8[PIDPOSR] = 14; // POSHOLD_RATE_I * 100;
|
|
pidProfile->D8[PIDPOSR] = 53; // POSHOLD_RATE_D * 1000;
|
|
pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10;
|
|
pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100;
|
|
pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000;
|
|
pidProfile->P8[PIDLEVEL] = 90;
|
|
pidProfile->I8[PIDLEVEL] = 10;
|
|
pidProfile->D8[PIDLEVEL] = 100;
|
|
pidProfile->P8[PIDMAG] = 40;
|
|
pidProfile->P8[PIDVEL] = 120;
|
|
pidProfile->I8[PIDVEL] = 45;
|
|
pidProfile->D8[PIDVEL] = 1;
|
|
|
|
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
|
pidProfile->dterm_cut_hz = 0;
|
|
pidProfile->pterm_cut_hz = 0;
|
|
pidProfile->gyro_cut_hz = 0;
|
|
|
|
pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully
|
|
pidProfile->I_f[ROLL] = 0.6f;
|
|
pidProfile->D_f[ROLL] = 0.06f;
|
|
pidProfile->P_f[PITCH] = 2.5f;
|
|
pidProfile->I_f[PITCH] = 0.6f;
|
|
pidProfile->D_f[PITCH] = 0.06f;
|
|
pidProfile->P_f[YAW] = 8.0f;
|
|
pidProfile->I_f[YAW] = 0.5f;
|
|
pidProfile->D_f[YAW] = 0.05f;
|
|
pidProfile->A_level = 5.0f;
|
|
pidProfile->H_level = 3.0f;
|
|
pidProfile->H_sensitivity = 75;
|
|
}
|
|
|
|
#ifdef GPS
|
|
void resetGpsProfile(gpsProfile_t *gpsProfile)
|
|
{
|
|
gpsProfile->gps_wp_radius = 200;
|
|
gpsProfile->gps_lpf = 20;
|
|
gpsProfile->nav_slew_rate = 30;
|
|
gpsProfile->nav_controls_heading = 1;
|
|
gpsProfile->nav_speed_min = 100;
|
|
gpsProfile->nav_speed_max = 300;
|
|
gpsProfile->ap_mode = 40;
|
|
}
|
|
#endif
|
|
|
|
void resetBarometerConfig(barometerConfig_t *barometerConfig)
|
|
{
|
|
barometerConfig->baro_sample_count = 21;
|
|
barometerConfig->baro_noise_lpf = 0.6f;
|
|
barometerConfig->baro_cf_vel = 0.985f;
|
|
barometerConfig->baro_cf_alt = 0.965f;
|
|
}
|
|
|
|
void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
|
|
{
|
|
sensorAlignmentConfig->gyro_align = ALIGN_DEFAULT;
|
|
sensorAlignmentConfig->acc_align = ALIGN_DEFAULT;
|
|
sensorAlignmentConfig->mag_align = ALIGN_DEFAULT;
|
|
}
|
|
|
|
void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
|
|
{
|
|
escAndServoConfig->minthrottle = 1150;
|
|
escAndServoConfig->maxthrottle = 1850;
|
|
escAndServoConfig->mincommand = 1000;
|
|
escAndServoConfig->servoCenterPulse = 1500;
|
|
}
|
|
|
|
void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
|
|
{
|
|
flight3DConfig->deadband3d_low = 1406;
|
|
flight3DConfig->deadband3d_high = 1514;
|
|
flight3DConfig->neutral3d = 1460;
|
|
flight3DConfig->deadband3d_throttle = 50;
|
|
}
|
|
|
|
void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
|
|
{
|
|
telemetryConfig->telemetry_inversion = 0;
|
|
telemetryConfig->telemetry_switch = 0;
|
|
telemetryConfig->gpsNoFixLatitude = 0;
|
|
telemetryConfig->gpsNoFixLongitude = 0;
|
|
telemetryConfig->frsky_coordinate_format = FRSKY_FORMAT_DMS;
|
|
telemetryConfig->frsky_unit = FRSKY_UNIT_METRICS;
|
|
telemetryConfig->frsky_vfas_precision = 0;
|
|
telemetryConfig->hottAlarmSoundInterval = 5;
|
|
}
|
|
|
|
void resetBatteryConfig(batteryConfig_t *batteryConfig)
|
|
{
|
|
batteryConfig->vbatscale = VBAT_SCALE_DEFAULT;
|
|
batteryConfig->vbatmaxcellvoltage = 43;
|
|
batteryConfig->vbatmincellvoltage = 33;
|
|
batteryConfig->vbatwarningcellvoltage = 35;
|
|
batteryConfig->currentMeterOffset = 0;
|
|
batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
|
|
batteryConfig->batteryCapacity = 0;
|
|
batteryConfig->currentMeterType = CURRENT_SENSOR_ADC;
|
|
}
|
|
|
|
#ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS
|
|
#define FIRST_PORT_INDEX 1
|
|
#define SECOND_PORT_INDEX 0
|
|
#else
|
|
#define FIRST_PORT_INDEX 0
|
|
#define SECOND_PORT_INDEX 1
|
|
#endif
|
|
|
|
void resetSerialConfig(serialConfig_t *serialConfig)
|
|
{
|
|
uint8_t index;
|
|
memset(serialConfig, 0, sizeof(serialConfig_t));
|
|
|
|
for (index = 0; index < SERIAL_PORT_COUNT; index++) {
|
|
serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index];
|
|
serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200;
|
|
serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600;
|
|
serialConfig->portConfigs[index].telemetry_baudrateIndex = BAUD_AUTO;
|
|
serialConfig->portConfigs[index].blackbox_baudrateIndex = BAUD_115200;
|
|
}
|
|
|
|
serialConfig->portConfigs[0].functionMask = FUNCTION_MSP;
|
|
|
|
#ifdef CC3D
|
|
// This allows MSP connection via USART & VCP so the board can be reconfigured.
|
|
serialConfig->portConfigs[1].functionMask = FUNCTION_MSP;
|
|
#endif
|
|
|
|
serialConfig->reboot_character = 'R';
|
|
}
|
|
|
|
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
|
|
controlRateConfig->rcRate8 = 90;
|
|
controlRateConfig->rcExpo8 = 65;
|
|
controlRateConfig->thrMid8 = 50;
|
|
controlRateConfig->thrExpo8 = 0;
|
|
controlRateConfig->dynThrPID = 0;
|
|
controlRateConfig->rcYawExpo8 = 0;
|
|
controlRateConfig->tpa_breakpoint = 1500;
|
|
|
|
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
|
|
controlRateConfig->rates[axis] = 0;
|
|
}
|
|
|
|
}
|
|
|
|
void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
|
|
rcControlsConfig->deadband = 0;
|
|
rcControlsConfig->yaw_deadband = 0;
|
|
rcControlsConfig->alt_hold_deadband = 40;
|
|
rcControlsConfig->alt_hold_fast_change = 1;
|
|
}
|
|
|
|
void resetMixerConfig(mixerConfig_t *mixerConfig) {
|
|
mixerConfig->pid_at_min_throttle = 1;
|
|
mixerConfig->yaw_motor_direction = 1;
|
|
mixerConfig->yaw_jump_prevention_limit = 200;
|
|
#ifdef USE_SERVOS
|
|
mixerConfig->tri_unarmed_servo = 1;
|
|
mixerConfig->servo_lowpass_freq = 400;
|
|
mixerConfig->servo_lowpass_enable = 0;
|
|
#endif
|
|
}
|
|
|
|
uint8_t getCurrentProfile(void)
|
|
{
|
|
return masterConfig.current_profile_index;
|
|
}
|
|
|
|
static void setProfile(uint8_t profileIndex)
|
|
{
|
|
currentProfile = &masterConfig.profile[profileIndex];
|
|
}
|
|
|
|
uint8_t getCurrentControlRateProfile(void)
|
|
{
|
|
return currentControlRateProfileIndex;
|
|
}
|
|
|
|
controlRateConfig_t *getControlRateConfig(uint8_t profileIndex) {
|
|
return &masterConfig.controlRateProfiles[profileIndex];
|
|
}
|
|
|
|
static void setControlRateProfile(uint8_t profileIndex)
|
|
{
|
|
currentControlRateProfileIndex = profileIndex;
|
|
currentControlRateProfile = &masterConfig.controlRateProfiles[profileIndex];
|
|
}
|
|
|
|
// Default settings
|
|
static void resetConf(void)
|
|
{
|
|
int i;
|
|
|
|
// Clear all configuration
|
|
memset(&masterConfig, 0, sizeof(master_t));
|
|
setProfile(0);
|
|
setControlRateProfile(0);
|
|
|
|
masterConfig.version = EEPROM_CONF_VERSION;
|
|
masterConfig.mixerMode = MIXER_QUADX;
|
|
featureClearAll();
|
|
#if defined(CJMCU) || defined(SPARKY)
|
|
featureSet(FEATURE_RX_PPM);
|
|
#endif
|
|
|
|
#ifdef BOARD_HAS_VOLTAGE_DIVIDER
|
|
// only enable the VBAT feature by default if the board has a voltage divider otherwise
|
|
// the user may see incorrect readings and unexpected issues with pin mappings may occur.
|
|
featureSet(FEATURE_VBAT);
|
|
#endif
|
|
|
|
featureSet(FEATURE_FAILSAFE);
|
|
|
|
// global settings
|
|
masterConfig.current_profile_index = 0; // default profile
|
|
masterConfig.gyro_cmpf_factor = 600; // default MWC
|
|
masterConfig.gyro_cmpfm_factor = 250; // default MWC
|
|
masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
|
|
|
|
resetAccelerometerTrims(&masterConfig.accZero);
|
|
|
|
resetSensorAlignment(&masterConfig.sensorAlignmentConfig);
|
|
|
|
masterConfig.boardAlignment.rollDegrees = 0;
|
|
masterConfig.boardAlignment.pitchDegrees = 0;
|
|
masterConfig.boardAlignment.yawDegrees = 0;
|
|
masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
|
|
masterConfig.max_angle_inclination = 500; // 50 degrees
|
|
masterConfig.yaw_control_direction = 1;
|
|
masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
|
|
|
|
masterConfig.mag_hardware = MAG_DEFAULT; // default/autodetect
|
|
|
|
resetBatteryConfig(&masterConfig.batteryConfig);
|
|
|
|
resetTelemetryConfig(&masterConfig.telemetryConfig);
|
|
|
|
masterConfig.rxConfig.serialrx_provider = 0;
|
|
masterConfig.rxConfig.spektrum_sat_bind = 0;
|
|
masterConfig.rxConfig.midrc = 1500;
|
|
masterConfig.rxConfig.mincheck = 1100;
|
|
masterConfig.rxConfig.maxcheck = 1900;
|
|
masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection
|
|
masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection
|
|
|
|
for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT-4; i++) {
|
|
masterConfig.rxConfig.rx_fail_usec_steps[i] = CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc);
|
|
}
|
|
|
|
masterConfig.rxConfig.rssi_channel = 0;
|
|
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
|
|
masterConfig.rxConfig.rssi_ppm_invert = 0;
|
|
|
|
masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED;
|
|
|
|
masterConfig.retarded_arm = 0;
|
|
masterConfig.disarm_kill_switch = 1;
|
|
masterConfig.auto_disarm_delay = 5;
|
|
masterConfig.small_angle = 25;
|
|
|
|
resetMixerConfig(&masterConfig.mixerConfig);
|
|
|
|
masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
|
|
|
|
// Motor/ESC/Servo
|
|
resetEscAndServoConfig(&masterConfig.escAndServoConfig);
|
|
resetFlight3DConfig(&masterConfig.flight3DConfig);
|
|
|
|
#ifdef BRUSHED_MOTORS
|
|
masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE;
|
|
#else
|
|
masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE;
|
|
#endif
|
|
masterConfig.servo_pwm_rate = 50;
|
|
|
|
#ifdef GPS
|
|
// gps/nav stuff
|
|
masterConfig.gpsConfig.provider = GPS_NMEA;
|
|
masterConfig.gpsConfig.sbasMode = SBAS_AUTO;
|
|
masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON;
|
|
masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF;
|
|
#endif
|
|
|
|
resetSerialConfig(&masterConfig.serialConfig);
|
|
|
|
masterConfig.looptime = 3500;
|
|
masterConfig.emf_avoidance = 0;
|
|
|
|
resetPidProfile(¤tProfile->pidProfile);
|
|
|
|
resetControlRateConfig(&masterConfig.controlRateProfiles[0]);
|
|
|
|
// for (i = 0; i < CHECKBOXITEMS; i++)
|
|
// cfg.activate[i] = 0;
|
|
|
|
resetRollAndPitchTrims(¤tProfile->accelerometerTrims);
|
|
|
|
currentProfile->mag_declination = 0;
|
|
currentProfile->acc_lpf_factor = 4;
|
|
currentProfile->accz_lpf_cutoff = 5.0f;
|
|
currentProfile->accDeadband.xy = 40;
|
|
currentProfile->accDeadband.z = 40;
|
|
|
|
resetBarometerConfig(¤tProfile->barometerConfig);
|
|
|
|
currentProfile->acc_unarmedcal = 1;
|
|
|
|
// Radio
|
|
parseRcChannels("AETR1234", &masterConfig.rxConfig);
|
|
|
|
resetRcControlsConfig(¤tProfile->rcControlsConfig);
|
|
|
|
currentProfile->throttle_correction_value = 0; // could 10 with althold or 40 for fpv
|
|
currentProfile->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
|
|
|
|
// Failsafe Variables
|
|
masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec
|
|
masterConfig.failsafeConfig.failsafe_off_delay = 200; // 20sec
|
|
masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off.
|
|
|
|
#ifdef USE_SERVOS
|
|
// servos
|
|
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
|
currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN;
|
|
currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX;
|
|
currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
|
|
currentProfile->servoConf[i].rate = 100;
|
|
currentProfile->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE;
|
|
currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE;
|
|
currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
|
|
}
|
|
|
|
// gimbal
|
|
currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL;
|
|
#endif
|
|
|
|
#ifdef GPS
|
|
resetGpsProfile(¤tProfile->gpsProfile);
|
|
#endif
|
|
|
|
// custom mixer. clear by defaults.
|
|
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
|
masterConfig.customMotorMixer[i].throttle = 0.0f;
|
|
|
|
#ifdef LED_STRIP
|
|
applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);
|
|
applyDefaultLedStripConfig(masterConfig.ledConfigs);
|
|
#endif
|
|
|
|
#ifdef BLACKBOX
|
|
#ifdef SPRACINGF3
|
|
featureSet(FEATURE_BLACKBOX);
|
|
masterConfig.blackbox_device = 1;
|
|
#else
|
|
masterConfig.blackbox_device = 0;
|
|
#endif
|
|
masterConfig.blackbox_rate_num = 1;
|
|
masterConfig.blackbox_rate_denom = 1;
|
|
#endif
|
|
|
|
// alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets
|
|
#ifdef ALIENWII32
|
|
featureSet(FEATURE_RX_SERIAL);
|
|
featureSet(FEATURE_MOTOR_STOP);
|
|
#ifdef ALIENWIIF3
|
|
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
|
|
masterConfig.batteryConfig.vbatscale = 20;
|
|
#else
|
|
masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
|
|
#endif
|
|
masterConfig.rxConfig.serialrx_provider = 1;
|
|
masterConfig.rxConfig.spektrum_sat_bind = 5;
|
|
masterConfig.escAndServoConfig.minthrottle = 1000;
|
|
masterConfig.escAndServoConfig.maxthrottle = 2000;
|
|
masterConfig.motor_pwm_rate = 32000;
|
|
masterConfig.looptime = 2000;
|
|
currentProfile->pidProfile.pidController = 3;
|
|
currentProfile->pidProfile.P8[ROLL] = 36;
|
|
currentProfile->pidProfile.P8[PITCH] = 36;
|
|
masterConfig.failsafeConfig.failsafe_delay = 2;
|
|
masterConfig.failsafeConfig.failsafe_off_delay = 0;
|
|
currentControlRateProfile->rcRate8 = 130;
|
|
currentControlRateProfile->rates[FD_PITCH] = 20;
|
|
currentControlRateProfile->rates[FD_ROLL] = 20;
|
|
currentControlRateProfile->rates[FD_YAW] = 100;
|
|
parseRcChannels("TAER1234", &masterConfig.rxConfig);
|
|
|
|
// { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R
|
|
masterConfig.customMotorMixer[0].throttle = 1.0f;
|
|
masterConfig.customMotorMixer[0].roll = -0.414178f;
|
|
masterConfig.customMotorMixer[0].pitch = 1.0f;
|
|
masterConfig.customMotorMixer[0].yaw = -1.0f;
|
|
|
|
// { 1.0f, -0.414178f, -1.0f, 1.0f }, // FRONT_R
|
|
masterConfig.customMotorMixer[1].throttle = 1.0f;
|
|
masterConfig.customMotorMixer[1].roll = -0.414178f;
|
|
masterConfig.customMotorMixer[1].pitch = -1.0f;
|
|
masterConfig.customMotorMixer[1].yaw = 1.0f;
|
|
|
|
// { 1.0f, 0.414178f, 1.0f, 1.0f }, // REAR_L
|
|
masterConfig.customMotorMixer[2].throttle = 1.0f;
|
|
masterConfig.customMotorMixer[2].roll = 0.414178f;
|
|
masterConfig.customMotorMixer[2].pitch = 1.0f;
|
|
masterConfig.customMotorMixer[2].yaw = 1.0f;
|
|
|
|
// { 1.0f, 0.414178f, -1.0f, -1.0f }, // FRONT_L
|
|
masterConfig.customMotorMixer[3].throttle = 1.0f;
|
|
masterConfig.customMotorMixer[3].roll = 0.414178f;
|
|
masterConfig.customMotorMixer[3].pitch = -1.0f;
|
|
masterConfig.customMotorMixer[3].yaw = -1.0f;
|
|
|
|
// { 1.0f, -1.0f, -0.414178f, -1.0f }, // MIDFRONT_R
|
|
masterConfig.customMotorMixer[4].throttle = 1.0f;
|
|
masterConfig.customMotorMixer[4].roll = -1.0f;
|
|
masterConfig.customMotorMixer[4].pitch = -0.414178f;
|
|
masterConfig.customMotorMixer[4].yaw = -1.0f;
|
|
|
|
// { 1.0f, 1.0f, -0.414178f, 1.0f }, // MIDFRONT_L
|
|
masterConfig.customMotorMixer[5].throttle = 1.0f;
|
|
masterConfig.customMotorMixer[5].roll = 1.0f;
|
|
masterConfig.customMotorMixer[5].pitch = -0.414178f;
|
|
masterConfig.customMotorMixer[5].yaw = 1.0f;
|
|
|
|
// { 1.0f, -1.0f, 0.414178f, 1.0f }, // MIDREAR_R
|
|
masterConfig.customMotorMixer[6].throttle = 1.0f;
|
|
masterConfig.customMotorMixer[6].roll = -1.0f;
|
|
masterConfig.customMotorMixer[6].pitch = 0.414178f;
|
|
masterConfig.customMotorMixer[6].yaw = 1.0f;
|
|
|
|
// { 1.0f, 1.0f, 0.414178f, -1.0f }, // MIDREAR_L
|
|
masterConfig.customMotorMixer[7].throttle = 1.0f;
|
|
masterConfig.customMotorMixer[7].roll = 1.0f;
|
|
masterConfig.customMotorMixer[7].pitch = 0.414178f;
|
|
masterConfig.customMotorMixer[7].yaw = -1.0f;
|
|
#endif
|
|
|
|
// copy first profile into remaining profile
|
|
for (i = 1; i < MAX_PROFILE_COUNT; i++) {
|
|
memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t));
|
|
}
|
|
|
|
// copy first control rate config into remaining profile
|
|
for (i = 1; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) {
|
|
memcpy(&masterConfig.controlRateProfiles[i], currentControlRateProfile, sizeof(controlRateConfig_t));
|
|
}
|
|
|
|
for (i = 1; i < MAX_PROFILE_COUNT; i++) {
|
|
masterConfig.profile[i].defaultRateProfileIndex = i % MAX_CONTROL_RATE_PROFILE_COUNT;
|
|
}
|
|
}
|
|
|
|
static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
|
|
{
|
|
uint8_t checksum = 0;
|
|
const uint8_t *byteOffset;
|
|
|
|
for (byteOffset = data; byteOffset < (data + length); byteOffset++)
|
|
checksum ^= *byteOffset;
|
|
return checksum;
|
|
}
|
|
|
|
static bool isEEPROMContentValid(void)
|
|
{
|
|
const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS;
|
|
uint8_t checksum = 0;
|
|
|
|
// check version number
|
|
if (EEPROM_CONF_VERSION != temp->version)
|
|
return false;
|
|
|
|
// check size and magic numbers
|
|
if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
|
|
return false;
|
|
|
|
// verify integrity of temporary copy
|
|
checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
|
|
if (checksum != 0)
|
|
return false;
|
|
|
|
// looks good, let's roll!
|
|
return true;
|
|
}
|
|
|
|
void activateControlRateConfig(void)
|
|
{
|
|
generatePitchRollCurve(currentControlRateProfile);
|
|
generateYawCurve(currentControlRateProfile);
|
|
generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig);
|
|
}
|
|
|
|
void activateConfig(void)
|
|
{
|
|
static imuRuntimeConfig_t imuRuntimeConfig;
|
|
|
|
activateControlRateConfig();
|
|
|
|
resetAdjustmentStates();
|
|
|
|
useRcControlsConfig(
|
|
currentProfile->modeActivationConditions,
|
|
&masterConfig.escAndServoConfig,
|
|
¤tProfile->pidProfile
|
|
);
|
|
|
|
useGyroConfig(&masterConfig.gyroConfig);
|
|
|
|
#ifdef TELEMETRY
|
|
telemetryUseConfig(&masterConfig.telemetryConfig);
|
|
#endif
|
|
|
|
pidSetController(currentProfile->pidProfile.pidController);
|
|
|
|
#ifdef GPS
|
|
gpsUseProfile(¤tProfile->gpsProfile);
|
|
gpsUsePIDs(¤tProfile->pidProfile);
|
|
#endif
|
|
|
|
useFailsafeConfig(&masterConfig.failsafeConfig);
|
|
setAccelerationTrims(&masterConfig.accZero);
|
|
|
|
mixerUseConfigs(
|
|
#ifdef USE_SERVOS
|
|
currentProfile->servoConf,
|
|
¤tProfile->gimbalConfig,
|
|
#endif
|
|
&masterConfig.flight3DConfig,
|
|
&masterConfig.escAndServoConfig,
|
|
&masterConfig.mixerConfig,
|
|
&masterConfig.airplaneConfig,
|
|
&masterConfig.rxConfig
|
|
);
|
|
|
|
imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;
|
|
imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
|
|
imuRuntimeConfig.acc_lpf_factor = currentProfile->acc_lpf_factor;
|
|
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
|
|
imuRuntimeConfig.small_angle = masterConfig.small_angle;
|
|
|
|
imuConfigure(
|
|
&imuRuntimeConfig,
|
|
¤tProfile->pidProfile,
|
|
¤tProfile->accDeadband,
|
|
currentProfile->accz_lpf_cutoff,
|
|
currentProfile->throttle_correction_angle
|
|
);
|
|
|
|
configureAltitudeHold(
|
|
¤tProfile->pidProfile,
|
|
¤tProfile->barometerConfig,
|
|
¤tProfile->rcControlsConfig,
|
|
&masterConfig.escAndServoConfig
|
|
);
|
|
|
|
#ifdef BARO
|
|
useBarometerConfig(¤tProfile->barometerConfig);
|
|
#endif
|
|
}
|
|
|
|
void validateAndFixConfig(void)
|
|
{
|
|
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
|
|
featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
|
|
}
|
|
|
|
if (featureConfigured(FEATURE_RX_PPM)) {
|
|
featureClear(FEATURE_RX_PARALLEL_PWM);
|
|
}
|
|
|
|
if (featureConfigured(FEATURE_RX_MSP)) {
|
|
featureClear(FEATURE_RX_SERIAL);
|
|
featureClear(FEATURE_RX_PARALLEL_PWM);
|
|
featureClear(FEATURE_RX_PPM);
|
|
}
|
|
|
|
if (featureConfigured(FEATURE_RX_SERIAL)) {
|
|
featureClear(FEATURE_RX_PARALLEL_PWM);
|
|
featureClear(FEATURE_RX_PPM);
|
|
}
|
|
|
|
if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
|
|
#if defined(STM32F10X)
|
|
// rssi adc needs the same ports
|
|
featureClear(FEATURE_RSSI_ADC);
|
|
// current meter needs the same ports
|
|
if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
|
|
featureClear(FEATURE_CURRENT_METER);
|
|
}
|
|
#endif
|
|
|
|
#if defined(STM32F10X) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
|
|
// led strip needs the same ports
|
|
featureClear(FEATURE_LED_STRIP);
|
|
#endif
|
|
|
|
// software serial needs free PWM ports
|
|
featureClear(FEATURE_SOFTSERIAL);
|
|
}
|
|
|
|
|
|
#if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
|
|
if (featureConfigured(FEATURE_SOFTSERIAL) && (
|
|
0
|
|
#ifdef USE_SOFTSERIAL1
|
|
|| (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
|
|
#endif
|
|
#ifdef USE_SOFTSERIAL2
|
|
|| (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
|
|
#endif
|
|
)) {
|
|
// led strip needs the same timer as softserial
|
|
featureClear(FEATURE_LED_STRIP);
|
|
}
|
|
#endif
|
|
|
|
#if defined(NAZE) && defined(SONAR)
|
|
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
|
|
featureClear(FEATURE_CURRENT_METER);
|
|
}
|
|
#endif
|
|
|
|
#if defined(OLIMEXINO) && defined(SONAR)
|
|
if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
|
|
featureClear(FEATURE_CURRENT_METER);
|
|
}
|
|
#endif
|
|
|
|
#if defined(CC3D) && defined(DISPLAY) && defined(USE_USART3)
|
|
if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) {
|
|
featureClear(FEATURE_DISPLAY);
|
|
}
|
|
#endif
|
|
|
|
/*
|
|
* The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
|
|
* The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
|
|
*/
|
|
if (masterConfig.retarded_arm && masterConfig.mixerConfig.pid_at_min_throttle) {
|
|
masterConfig.mixerConfig.pid_at_min_throttle = 0;
|
|
}
|
|
|
|
#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1)
|
|
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
|
featureClear(FEATURE_SONAR);
|
|
}
|
|
#endif
|
|
|
|
useRxConfig(&masterConfig.rxConfig);
|
|
|
|
serialConfig_t *serialConfig = &masterConfig.serialConfig;
|
|
|
|
if (!isSerialConfigValid(serialConfig)) {
|
|
resetSerialConfig(serialConfig);
|
|
}
|
|
}
|
|
|
|
void initEEPROM(void)
|
|
{
|
|
}
|
|
|
|
void readEEPROM(void)
|
|
{
|
|
// Sanity check
|
|
if (!isEEPROMContentValid())
|
|
failureMode(10);
|
|
|
|
// Read flash
|
|
memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t));
|
|
|
|
if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check
|
|
masterConfig.current_profile_index = 0;
|
|
|
|
setProfile(masterConfig.current_profile_index);
|
|
|
|
if (currentProfile->defaultRateProfileIndex > MAX_CONTROL_RATE_PROFILE_COUNT - 1) // sanity check
|
|
currentProfile->defaultRateProfileIndex = 0;
|
|
|
|
setControlRateProfile(currentProfile->defaultRateProfileIndex);
|
|
|
|
validateAndFixConfig();
|
|
activateConfig();
|
|
}
|
|
|
|
void readEEPROMAndNotify(void)
|
|
{
|
|
// re-read written data
|
|
readEEPROM();
|
|
beeperConfirmationBeeps(1);
|
|
}
|
|
|
|
void writeEEPROM(void)
|
|
{
|
|
// Generate compile time error if the config does not fit in the reserved area of flash.
|
|
BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
|
|
|
|
FLASH_Status status = 0;
|
|
uint32_t wordOffset;
|
|
int8_t attemptsRemaining = 3;
|
|
|
|
// prepare checksum/version constants
|
|
masterConfig.version = EEPROM_CONF_VERSION;
|
|
masterConfig.size = sizeof(master_t);
|
|
masterConfig.magic_be = 0xBE;
|
|
masterConfig.magic_ef = 0xEF;
|
|
masterConfig.chk = 0; // erase checksum before recalculating
|
|
masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
|
|
|
|
// write it
|
|
FLASH_Unlock();
|
|
while (attemptsRemaining--) {
|
|
#ifdef STM32F303
|
|
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
|
|
#endif
|
|
#ifdef STM32F10X
|
|
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
|
|
#endif
|
|
for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) {
|
|
if (wordOffset % FLASH_PAGE_SIZE == 0) {
|
|
status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
|
|
if (status != FLASH_COMPLETE) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset,
|
|
*(uint32_t *) ((char *) &masterConfig + wordOffset));
|
|
if (status != FLASH_COMPLETE) {
|
|
break;
|
|
}
|
|
}
|
|
if (status == FLASH_COMPLETE) {
|
|
break;
|
|
}
|
|
}
|
|
FLASH_Lock();
|
|
|
|
// Flash write failed - just die now
|
|
if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
|
|
failureMode(10);
|
|
}
|
|
}
|
|
|
|
void ensureEEPROMContainsValidData(void)
|
|
{
|
|
if (isEEPROMContentValid()) {
|
|
return;
|
|
}
|
|
|
|
resetEEPROM();
|
|
}
|
|
|
|
void resetEEPROM(void)
|
|
{
|
|
resetConf();
|
|
writeEEPROM();
|
|
}
|
|
|
|
void saveConfigAndNotify(void)
|
|
{
|
|
writeEEPROM();
|
|
readEEPROMAndNotify();
|
|
}
|
|
|
|
void changeProfile(uint8_t profileIndex)
|
|
{
|
|
masterConfig.current_profile_index = profileIndex;
|
|
writeEEPROM();
|
|
readEEPROM();
|
|
beeperConfirmationBeeps(profileIndex + 1);
|
|
}
|
|
|
|
void changeControlRateProfile(uint8_t profileIndex)
|
|
{
|
|
if (profileIndex > MAX_CONTROL_RATE_PROFILE_COUNT) {
|
|
profileIndex = MAX_CONTROL_RATE_PROFILE_COUNT - 1;
|
|
}
|
|
setControlRateProfile(profileIndex);
|
|
activateControlRateConfig();
|
|
}
|
|
|
|
void handleOneshotFeatureChangeOnRestart(void)
|
|
{
|
|
// Shutdown PWM on all motors prior to soft restart
|
|
StopPwmAllMotors();
|
|
delay(50);
|
|
// Apply additional delay when OneShot125 feature changed from on to off state
|
|
if (feature(FEATURE_ONESHOT125) && !featureConfigured(FEATURE_ONESHOT125)) {
|
|
delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS);
|
|
}
|
|
}
|
|
|
|
void latchActiveFeatures()
|
|
{
|
|
activeFeaturesLatch = masterConfig.enabledFeatures;
|
|
}
|
|
|
|
bool featureConfigured(uint32_t mask)
|
|
{
|
|
return masterConfig.enabledFeatures & mask;
|
|
}
|
|
|
|
bool feature(uint32_t mask)
|
|
{
|
|
return activeFeaturesLatch & mask;
|
|
}
|
|
|
|
void featureSet(uint32_t mask)
|
|
{
|
|
masterConfig.enabledFeatures |= mask;
|
|
}
|
|
|
|
void featureClear(uint32_t mask)
|
|
{
|
|
masterConfig.enabledFeatures &= ~(mask);
|
|
}
|
|
|
|
void featureClearAll()
|
|
{
|
|
masterConfig.enabledFeatures = 0;
|
|
}
|
|
|
|
uint32_t featureMask(void)
|
|
{
|
|
return masterConfig.enabledFeatures;
|
|
}
|
|
|