Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

Split config_storage.h into config_master.h and config_profile.h to

Browse source 
separate concerns and help reduce and clarify dependencies.

cfg and mcfg are too similarly named and are not obvious.  Renamed cfg
to currentProfile and mcfg to masterConfig.  This will increase source
code line length somewhat but that's ok; lines of code doing too much
are now easier to spot.
This commit is contained in:
Dominic Clifton 2014-04-21 23:08:25 +01:00
parent dab04de35e
commit f8d0dd98f7
18 changed files with 677 additions and 672 deletions
Download patch file Download diff file Expand all files Collapse all files

309
src/config.c
View file

@ -30,7 +30,8 @@
#include "runtime_config.h" #include "runtime_config.h"
#include "config.h" #include "config.h"
#include "config_storage.h" #include "config_profile.h"
#include "config_master.h"
void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
@ -41,8 +42,8 @@ void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
#define FLASH_PAGE_SIZE ((uint16_t)0x400) #define FLASH_PAGE_SIZE ((uint16_t)0x400)
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage #define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage
master_t mcfg; // master config struct with data independent from profiles master_t masterConfig; // master config struct with data independent from profiles
config_t cfg; // profile config struct profile_t currentProfile; // profile config struct
static const uint8_t EEPROM_CONF_VERSION = 64; static const uint8_t EEPROM_CONF_VERSION = 64;
static void resetConf(void); static void resetConf(void);
@ -81,12 +82,12 @@ static bool isEEPROMContentValid(void)
void activateConfig(void) void activateConfig(void)
{ {
generatePitchCurve(&cfg.controlRateConfig); generatePitchCurve(&currentProfile.controlRateConfig);
generateThrottleCurve(&cfg.controlRateConfig, mcfg.minthrottle, mcfg.maxthrottle); generateThrottleCurve(&currentProfile.controlRateConfig, masterConfig.minthrottle, masterConfig.maxthrottle);
setPIDController(cfg.pidController); setPIDController(currentProfile.pidController);
gpsSetPIDs(); gpsSetPIDs();
useFailsafeConfig(&cfg.failsafeConfig); useFailsafeConfig(&currentProfile.failsafeConfig);
} }
void readEEPROM(void) void readEEPROM(void)
@ -96,11 +97,11 @@ void readEEPROM(void)
failureMode(10); failureMode(10);
// Read flash // Read flash
memcpy(&mcfg, (char *)FLASH_WRITE_ADDR, sizeof(master_t)); memcpy(&masterConfig, (char *)FLASH_WRITE_ADDR, sizeof(master_t));
// Copy current profile // Copy current profile
if (mcfg.current_profile > 2) // sanity check if (masterConfig.current_profile > 2) // sanity check
mcfg.current_profile = 0; masterConfig.current_profile = 0;
memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); memcpy(&currentProfile, &masterConfig.profile[masterConfig.current_profile], sizeof(profile_t));
activateConfig(); activateConfig();
} }
@ -115,7 +116,7 @@ void readEEPROMAndNotify(void)
void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex) void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex)
{ {
// copy current in-memory profile to stored configuration // copy current in-memory profile to stored configuration
memcpy(&mcfg.profile[profileSlotIndex], &cfg, sizeof(config_t)); memcpy(&masterConfig.profile[profileSlotIndex], &currentProfile, sizeof(profile_t));
} }
void writeEEPROM(void) void writeEEPROM(void)
@ -125,12 +126,12 @@ void writeEEPROM(void)
int8_t attemptsRemaining = 3; int8_t attemptsRemaining = 3;
// prepare checksum/version constants // prepare checksum/version constants
mcfg.version = EEPROM_CONF_VERSION; masterConfig.version = EEPROM_CONF_VERSION;
mcfg.size = sizeof(master_t); masterConfig.size = sizeof(master_t);
mcfg.magic_be = 0xBE; masterConfig.magic_be = 0xBE;
mcfg.magic_ef = 0xEF; masterConfig.magic_ef = 0xEF;
mcfg.chk = 0; // erase checksum before recalculating masterConfig.chk = 0; // erase checksum before recalculating
mcfg.chk = calculateChecksum((const uint8_t *)&mcfg, sizeof(master_t)); masterConfig.chk = calculateChecksum((const uint8_t *)&masterConfig, sizeof(master_t));
// write it // write it
FLASH_Unlock(); FLASH_Unlock();
@ -139,7 +140,7 @@ void writeEEPROM(void)
status = FLASH_ErasePage(FLASH_WRITE_ADDR); status = FLASH_ErasePage(FLASH_WRITE_ADDR);
for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) { for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, *(uint32_t *) ((char *)&mcfg + wordOffset)); status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, *(uint32_t *) ((char *)&masterConfig + wordOffset));
} }
if (status == FLASH_COMPLETE) { if (status == FLASH_COMPLETE) {
break; break;
@ -175,193 +176,193 @@ static void resetConf(void)
int8_t servoRates[8] = { 30, 30, 100, 100, 100, 100, 100, 100 }; int8_t servoRates[8] = { 30, 30, 100, 100, 100, 100, 100, 100 };
// Clear all configuration // Clear all configuration
memset(&mcfg, 0, sizeof(master_t)); memset(&masterConfig, 0, sizeof(master_t));
memset(&cfg, 0, sizeof(config_t)); memset(&currentProfile, 0, sizeof(profile_t));
mcfg.version = EEPROM_CONF_VERSION; masterConfig.version = EEPROM_CONF_VERSION;
mcfg.mixerConfiguration = MULTITYPE_QUADX; masterConfig.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll(); featureClearAll();
featureSet(FEATURE_VBAT); featureSet(FEATURE_VBAT);
// global settings // global settings
mcfg.current_profile = 0; // default profile masterConfig.current_profile = 0; // default profile
mcfg.gyro_cmpf_factor = 600; // default MWC masterConfig.gyro_cmpf_factor = 600; // default MWC
mcfg.gyro_cmpfm_factor = 250; // default MWC masterConfig.gyro_cmpfm_factor = 250; // default MWC
mcfg.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
mcfg.accZero[0] = 0; masterConfig.accZero[0] = 0;
mcfg.accZero[1] = 0; masterConfig.accZero[1] = 0;
mcfg.accZero[2] = 0; masterConfig.accZero[2] = 0;
mcfg.gyro_align = ALIGN_DEFAULT; masterConfig.gyro_align = ALIGN_DEFAULT;
mcfg.acc_align = ALIGN_DEFAULT; masterConfig.acc_align = ALIGN_DEFAULT;
mcfg.mag_align = ALIGN_DEFAULT; masterConfig.mag_align = ALIGN_DEFAULT;
mcfg.boardAlignment.rollDegrees = 0; masterConfig.boardAlignment.rollDegrees = 0;
mcfg.boardAlignment.pitchDegrees = 0; masterConfig.boardAlignment.pitchDegrees = 0;
mcfg.boardAlignment.yawDegrees = 0; masterConfig.boardAlignment.yawDegrees = 0;
mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
mcfg.max_angle_inclination = 500; // 50 degrees masterConfig.max_angle_inclination = 500; // 50 degrees
mcfg.yaw_control_direction = 1; masterConfig.yaw_control_direction = 1;
mcfg.moron_threshold = 32; masterConfig.moron_threshold = 32;
mcfg.batteryConfig.vbatscale = 110; masterConfig.batteryConfig.vbatscale = 110;
mcfg.batteryConfig.vbatmaxcellvoltage = 43; masterConfig.batteryConfig.vbatmaxcellvoltage = 43;
mcfg.batteryConfig.vbatmincellvoltage = 33; masterConfig.batteryConfig.vbatmincellvoltage = 33;
mcfg.power_adc_channel = 0; masterConfig.power_adc_channel = 0;
mcfg.telemetry_provider = TELEMETRY_PROVIDER_FRSKY; masterConfig.telemetry_provider = TELEMETRY_PROVIDER_FRSKY;
mcfg.telemetry_port = TELEMETRY_PORT_UART; masterConfig.telemetry_port = TELEMETRY_PORT_UART;
mcfg.telemetry_switch = 0; masterConfig.telemetry_switch = 0;
mcfg.rxConfig.serialrx_type = 0; masterConfig.rxConfig.serialrx_type = 0;
mcfg.rxConfig.midrc = 1500; masterConfig.rxConfig.midrc = 1500;
mcfg.rxConfig.mincheck = 1100; masterConfig.rxConfig.mincheck = 1100;
mcfg.rxConfig.maxcheck = 1900; masterConfig.rxConfig.maxcheck = 1900;
mcfg.retarded_arm = 0; // disable arm/disarm on roll left/right masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
mcfg.flaps_speed = 0; masterConfig.flaps_speed = 0;
mcfg.fixedwing_althold_dir = 1; masterConfig.fixedwing_althold_dir = 1;
// Motor/ESC/Servo // Motor/ESC/Servo
mcfg.minthrottle = 1150; masterConfig.minthrottle = 1150;
mcfg.maxthrottle = 1850; masterConfig.maxthrottle = 1850;
mcfg.mincommand = 1000; masterConfig.mincommand = 1000;
mcfg.deadband3d_low = 1406; masterConfig.deadband3d_low = 1406;
mcfg.deadband3d_high = 1514; masterConfig.deadband3d_high = 1514;
mcfg.neutral3d = 1460; masterConfig.neutral3d = 1460;
mcfg.deadband3d_throttle = 50; masterConfig.deadband3d_throttle = 50;
mcfg.motor_pwm_rate = 400; masterConfig.motor_pwm_rate = 400;
mcfg.servo_pwm_rate = 50; masterConfig.servo_pwm_rate = 50;
// gps/nav stuff // gps/nav stuff
mcfg.gps_type = GPS_NMEA; masterConfig.gps_type = GPS_NMEA;
mcfg.gps_baudrate = GPS_BAUD_115200; masterConfig.gps_baudrate = GPS_BAUD_115200;
// serial (USART1) baudrate // serial (USART1) baudrate
mcfg.serialConfig.port1_baudrate = 115200; masterConfig.serialConfig.port1_baudrate = 115200;
mcfg.serialConfig.softserial_baudrate = 9600; masterConfig.serialConfig.softserial_baudrate = 9600;
mcfg.serialConfig.softserial_1_inverted = 0; masterConfig.serialConfig.softserial_1_inverted = 0;
mcfg.serialConfig.softserial_2_inverted = 0; masterConfig.serialConfig.softserial_2_inverted = 0;
mcfg.serialConfig.reboot_character = 'R'; masterConfig.serialConfig.reboot_character = 'R';
mcfg.looptime = 3500; masterConfig.looptime = 3500;
mcfg.emfAvoidance = 0; masterConfig.emfAvoidance = 0;
mcfg.rssi_aux_channel = 0; masterConfig.rssi_aux_channel = 0;
cfg.pidController = 0; currentProfile.pidController = 0;
cfg.P8[ROLL] = 40; currentProfile.P8[ROLL] = 40;
cfg.I8[ROLL] = 30; currentProfile.I8[ROLL] = 30;
cfg.D8[ROLL] = 23; currentProfile.D8[ROLL] = 23;
cfg.P8[PITCH] = 40; currentProfile.P8[PITCH] = 40;
cfg.I8[PITCH] = 30; currentProfile.I8[PITCH] = 30;
cfg.D8[PITCH] = 23; currentProfile.D8[PITCH] = 23;
cfg.P8[YAW] = 85; currentProfile.P8[YAW] = 85;
cfg.I8[YAW] = 45; currentProfile.I8[YAW] = 45;
cfg.D8[YAW] = 0; currentProfile.D8[YAW] = 0;
cfg.P8[PIDALT] = 50; currentProfile.P8[PIDALT] = 50;
cfg.I8[PIDALT] = 0; currentProfile.I8[PIDALT] = 0;
cfg.D8[PIDALT] = 0; currentProfile.D8[PIDALT] = 0;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100; currentProfile.P8[PIDPOS] = 11; // POSHOLD_P * 100;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100; currentProfile.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0; currentProfile.D8[PIDPOS] = 0;
cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10; currentProfile.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100; currentProfile.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000; currentProfile.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
cfg.P8[PIDNAVR] = 14; // NAV_P * 10; currentProfile.P8[PIDNAVR] = 14; // NAV_P * 10;
cfg.I8[PIDNAVR] = 20; // NAV_I * 100; currentProfile.I8[PIDNAVR] = 20; // NAV_I * 100;
cfg.D8[PIDNAVR] = 80; // NAV_D * 1000; currentProfile.D8[PIDNAVR] = 80; // NAV_D * 1000;
cfg.P8[PIDLEVEL] = 90; currentProfile.P8[PIDLEVEL] = 90;
cfg.I8[PIDLEVEL] = 10; currentProfile.I8[PIDLEVEL] = 10;
cfg.D8[PIDLEVEL] = 100; currentProfile.D8[PIDLEVEL] = 100;
cfg.P8[PIDMAG] = 40; currentProfile.P8[PIDMAG] = 40;
cfg.P8[PIDVEL] = 120; currentProfile.P8[PIDVEL] = 120;
cfg.I8[PIDVEL] = 45; currentProfile.I8[PIDVEL] = 45;
cfg.D8[PIDVEL] = 1; currentProfile.D8[PIDVEL] = 1;
cfg.controlRateConfig.rcRate8 = 90; currentProfile.controlRateConfig.rcRate8 = 90;
cfg.controlRateConfig.rcExpo8 = 65; currentProfile.controlRateConfig.rcExpo8 = 65;
cfg.controlRateConfig.rollPitchRate = 0; currentProfile.controlRateConfig.rollPitchRate = 0;
cfg.controlRateConfig.yawRate = 0; currentProfile.controlRateConfig.yawRate = 0;
cfg.dynThrPID = 0; currentProfile.dynThrPID = 0;
cfg.tpaBreakPoint = 1500; currentProfile.tpaBreakPoint = 1500;
cfg.controlRateConfig.thrMid8 = 50; currentProfile.controlRateConfig.thrMid8 = 50;
cfg.controlRateConfig.thrExpo8 = 0; currentProfile.controlRateConfig.thrExpo8 = 0;
// for (i = 0; i < CHECKBOXITEMS; i++) // for (i = 0; i < CHECKBOXITEMS; i++)
// cfg.activate[i] = 0; // cfg.activate[i] = 0;
cfg.angleTrim[0] = 0; currentProfile.angleTrim[0] = 0;
cfg.angleTrim[1] = 0; currentProfile.angleTrim[1] = 0;
cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. currentProfile.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.acc_lpf_factor = 4; currentProfile.acc_lpf_factor = 4;
cfg.accz_deadband = 40; currentProfile.accz_deadband = 40;
cfg.accxy_deadband = 40; currentProfile.accxy_deadband = 40;
cfg.baro_tab_size = 21; currentProfile.baro_tab_size = 21;
cfg.baro_noise_lpf = 0.6f; currentProfile.baro_noise_lpf = 0.6f;
cfg.baro_cf_vel = 0.985f; currentProfile.baro_cf_vel = 0.985f;
cfg.baro_cf_alt = 0.965f; currentProfile.baro_cf_alt = 0.965f;
cfg.acc_unarmedcal = 1; currentProfile.acc_unarmedcal = 1;
// Radio // Radio
parseRcChannels("AETR1234", &mcfg.rxConfig); parseRcChannels("AETR1234", &masterConfig.rxConfig);
cfg.deadband = 0; currentProfile.deadband = 0;
cfg.yawdeadband = 0; currentProfile.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 40; currentProfile.alt_hold_throttle_neutral = 40;
cfg.alt_hold_fast_change = 1; currentProfile.alt_hold_fast_change = 1;
cfg.throttle_correction_value = 0; // could 10 with althold or 40 for fpv currentProfile.throttle_correction_value = 0; // could 10 with althold or 40 for fpv
cfg.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv currentProfile.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
// Failsafe Variables // Failsafe Variables
cfg.failsafeConfig.failsafe_delay = 10; // 1sec currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec
cfg.failsafeConfig.failsafe_off_delay = 200; // 20sec currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec
cfg.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people. currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
cfg.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe currentProfile.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
// servos // servos
for (i = 0; i < 8; i++) { for (i = 0; i < 8; i++) {
cfg.servoConf[i].min = DEFAULT_SERVO_MIN; currentProfile.servoConf[i].min = DEFAULT_SERVO_MIN;
cfg.servoConf[i].max = DEFAULT_SERVO_MAX; currentProfile.servoConf[i].max = DEFAULT_SERVO_MAX;
cfg.servoConf[i].middle = DEFAULT_SERVO_MIDDLE; currentProfile.servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
cfg.servoConf[i].rate = servoRates[i]; currentProfile.servoConf[i].rate = servoRates[i];
cfg.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; currentProfile.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
} }
cfg.yaw_direction = 1; currentProfile.yaw_direction = 1;
cfg.tri_unarmed_servo = 1; currentProfile.tri_unarmed_servo = 1;
// gimbal // gimbal
cfg.gimbal_flags = GIMBAL_NORMAL; currentProfile.gimbal_flags = GIMBAL_NORMAL;
// gps/nav stuff // gps/nav stuff
cfg.gps_wp_radius = 200; currentProfile.gps_wp_radius = 200;
cfg.gps_lpf = 20; currentProfile.gps_lpf = 20;
cfg.nav_slew_rate = 30; currentProfile.nav_slew_rate = 30;
cfg.nav_controls_heading = 1; currentProfile.nav_controls_heading = 1;
cfg.nav_speed_min = 100; currentProfile.nav_speed_min = 100;
cfg.nav_speed_max = 300; currentProfile.nav_speed_max = 300;
cfg.ap_mode = 40; currentProfile.ap_mode = 40;
// custom mixer. clear by defaults. // custom mixer. clear by defaults.
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
mcfg.customMixer[i].throttle = 0.0f; masterConfig.customMixer[i].throttle = 0.0f;
// copy default config into all 3 profiles // copy default config into all 3 profiles
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
memcpy(&mcfg.profile[i], &cfg, sizeof(config_t)); memcpy(&masterConfig.profile[i], &currentProfile, sizeof(profile_t));
} }
bool feature(uint32_t mask) bool feature(uint32_t mask)
{ {
return mcfg.enabledFeatures & mask; return masterConfig.enabledFeatures & mask;
} }
void featureSet(uint32_t mask) void featureSet(uint32_t mask)
{ {
mcfg.enabledFeatures |= mask; masterConfig.enabledFeatures |= mask;
} }
void featureClear(uint32_t mask) void featureClear(uint32_t mask)
{ {
mcfg.enabledFeatures &= ~(mask); masterConfig.enabledFeatures &= ~(mask);
} }
void featureClearAll() void featureClearAll()
{ {
mcfg.enabledFeatures = 0; masterConfig.enabledFeatures = 0;
} }
uint32_t featureMask(void) uint32_t featureMask(void)
{ {
return mcfg.enabledFeatures; return masterConfig.enabledFeatures;
} }

63
src/config_storage.h → src/config_master.h
View file

@ -1,62 +1,5 @@
#pragma once #pragma once
typedef struct config_t {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
controlRateConfig_t controlRateConfig;
uint8_t dynThrPID;
uint16_t tpaBreakPoint; // Breakpoint where TPA is activated
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
int16_t angleTrim[ANGLE_INDEX_COUNT]; // accelerometer trim
// sensor-related stuff
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations
uint8_t accxy_deadband; // set the acc deadband for xy-Axis
uint8_t baro_tab_size; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
float baro_cf_alt; // apply CF to use ACC for height estimation
uint8_t acc_unarmedcal; // turn automatic acc compensation on/off
uint16_t activate[CHECKBOX_ITEM_COUNT]; // activate switches
// Radio/ESC-related configuration
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
// Servo-related stuff
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
// Failsafe related configuration
failsafeConfig_t failsafeConfig;
// mixer-related configuration
int8_t yaw_direction;
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
// gimbal-related configuration
uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff
// gps-related stuff
uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz)
uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes
uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it
uint16_t nav_speed_min; // cm/sec
uint16_t nav_speed_max; // cm/sec
uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS
} config_t;
// System-wide // System-wide
typedef struct master_t { typedef struct master_t {
uint8_t version; uint8_t version;
@ -116,12 +59,12 @@ typedef struct master_t {
uint8_t telemetry_provider; // See TelemetryProvider enum. uint8_t telemetry_provider; // See TelemetryProvider enum.
uint8_t telemetry_port; // See TelemetryPort enum. uint8_t telemetry_port; // See TelemetryPort enum.
uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
config_t profile[3]; // 3 separate profiles profile_t profile[3]; // 3 separate profiles
uint8_t current_profile; // currently loaded profile uint8_t current_profile; // currently loaded profile
uint8_t magic_ef; // magic number, should be 0xEF uint8_t magic_ef; // magic number, should be 0xEF
uint8_t chk; // XOR checksum uint8_t chk; // XOR checksum
} master_t; } master_t;
extern master_t mcfg; extern master_t masterConfig;
extern config_t cfg; extern profile_t currentProfile;

57
src/config_profile.h Normal file
View file

@ -0,0 +1,57 @@
#pragma once
typedef struct profile_s {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
controlRateConfig_t controlRateConfig;
uint8_t dynThrPID;
uint16_t tpaBreakPoint; // Breakpoint where TPA is activated
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
int16_t angleTrim[ANGLE_INDEX_COUNT]; // accelerometer trim
// sensor-related stuff
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations
uint8_t accxy_deadband; // set the acc deadband for xy-Axis
uint8_t baro_tab_size; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
float baro_cf_alt; // apply CF to use ACC for height estimation
uint8_t acc_unarmedcal; // turn automatic acc compensation on/off
uint16_t activate[CHECKBOX_ITEM_COUNT]; // activate switches
// Radio/ESC-related configuration
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
// Servo-related stuff
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
// Failsafe related configuration
failsafeConfig_t failsafeConfig;
// mixer-related configuration
int8_t yaw_direction;
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
// gimbal-related configuration
uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff
// gps-related stuff
uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz)
uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes
uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it
uint16_t nav_speed_min; // cm/sec
uint16_t nav_speed_max; // cm/sec
uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS
} profile_t;

48
src/flight_imu.c
View file

@ -28,7 +28,8 @@
#include "failsafe.h" #include "failsafe.h"
#include "runtime_config.h" #include "runtime_config.h"
#include "config.h" #include "config.h"
#include "config_storage.h" #include "config_profile.h"
#include "config_master.h"
int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
@ -75,7 +76,7 @@ void imuInit(void)
{ {
smallAngle = lrintf(acc_1G * cosf(RAD * 25.0f)); smallAngle = lrintf(acc_1G * cosf(RAD * 25.0f));
accVelScale = 9.80665f / acc_1G / 10000.0f; accVelScale = 9.80665f / acc_1G / 10000.0f;
throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle); throttleAngleScale = (1800.0f / M_PI) * (900.0f / currentProfile.throttle_correction_angle);
#ifdef MAG #ifdef MAG
// if mag sensor is enabled, use it // if mag sensor is enabled, use it
@ -99,7 +100,7 @@ void computeIMU(void)
accADC[Z] = 0; accADC[Z] = 0;
} }
if (mcfg.mixerConfiguration == MULTITYPE_TRI) { if (masterConfig.mixerConfiguration == MULTITYPE_TRI) {
gyroData[GI_YAW] = (gyroYawSmooth * 2 + gyroADC[GI_YAW]) / 3; gyroData[GI_YAW] = (gyroYawSmooth * 2 + gyroADC[GI_YAW]) / 3;
gyroYawSmooth = gyroData[GI_YAW]; gyroYawSmooth = gyroData[GI_YAW];
gyroData[GI_ROLL] = gyroADC[GI_ROLL]; gyroData[GI_ROLL] = gyroADC[GI_ROLL];
@ -124,9 +125,6 @@ void computeIMU(void)
// //
// ************************************************** // **************************************************
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
typedef struct fp_vector { typedef struct fp_vector {
float X; float X;
float Y; float Y;
@ -241,7 +239,7 @@ void acc_calc(uint32_t deltaT)
rotateAnglesV(&accel_ned.V, &rpy); rotateAnglesV(&accel_ned.V, &rpy);
if (cfg.acc_unarmedcal == 1) { if (currentProfile.acc_unarmedcal == 1) {
if (!f.ARMED) { if (!f.ARMED) {
accZoffset -= accZoffset / 64; accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z; accZoffset += accel_ned.V.Z;
@ -253,9 +251,9 @@ void acc_calc(uint32_t deltaT)
accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence // apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband); accSum[X] += applyDeadband(lrintf(accel_ned.V.X), currentProfile.accxy_deadband);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband); accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), currentProfile.accxy_deadband);
accSum[Z] += applyDeadband(lrintf(accz_smooth), cfg.accz_deadband); accSum[Z] += applyDeadband(lrintf(accz_smooth), currentProfile.accz_deadband);
// sum up Values for later integration to get velocity and distance // sum up Values for later integration to get velocity and distance
accTimeSum += deltaT; accTimeSum += deltaT;
@ -308,8 +306,8 @@ static void getEstimatedAttitude(void)
// Initialization // Initialization
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale; deltaGyroAngle[axis] = gyroADC[axis] * scale;
if (cfg.acc_lpf_factor > 0) { if (currentProfile.acc_lpf_factor > 0) {
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor); accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / currentProfile.acc_lpf_factor)) + accADC[axis] * (1.0f / currentProfile.acc_lpf_factor);
accSmooth[axis] = accLPF[axis]; accSmooth[axis] = accLPF[axis];
} else { } else {
accSmooth[axis] = accADC[axis]; accSmooth[axis] = accADC[axis];
@ -329,14 +327,20 @@ static void getEstimatedAttitude(void)
// Apply complimentary filter (Gyro drift correction) // Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation. // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro // To do that, we just skip filter, as EstV already rotated by Gyro
float invGyroComplimentaryFilterFactor = (1.0f / ((float)masterConfig.gyro_cmpf_factor + 1.0f));
if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) { if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
for (axis = 0; axis < 3; axis++) for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR; EstG.A[axis] = (EstG.A[axis] * (float)masterConfig.gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor;
} }
// FIXME what does the _M_ mean?
float invGyroComplimentaryFilter_M_Factor = (1.0f / ((float)masterConfig.gyro_cmpfm_factor + 1.0f));
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
for (axis = 0; axis < 3; axis++) for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR; EstM.A[axis] = (EstM.A[axis] * (float)masterConfig.gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
} }
f.SMALL_ANGLE = (EstG.A[Z] > smallAngle); f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
@ -354,7 +358,7 @@ static void getEstimatedAttitude(void)
acc_calc(deltaT); // rotate acc vector into earth frame acc_calc(deltaT); // rotate acc vector into earth frame
if (cfg.throttle_correction_value) { if (currentProfile.throttle_correction_value) {
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z); float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
@ -364,7 +368,7 @@ static void getEstimatedAttitude(void)
int angle = lrintf(acosf(cosZ) * throttleAngleScale); int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900) if (angle > 900)
angle = 900; angle = 900;
throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ; throttleAngleCorrection = lrintf(currentProfile.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ;
} }
} }
@ -410,7 +414,7 @@ int getEstimatedAltitude(void)
// Integrator - Altitude in cm // Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * cfg.baro_cf_alt + (float)BaroAlt * (1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) accAlt = accAlt * currentProfile.baro_cf_alt + (float)BaroAlt * (1.0f - currentProfile.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
EstAlt = accAlt; EstAlt = accAlt;
vel += vel_acc; vel += vel_acc;
@ -430,7 +434,7 @@ int getEstimatedAltitude(void)
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel); vel = vel * currentProfile.baro_cf_vel + baroVel * (1 - currentProfile.baro_cf_vel);
vel_tmp = lrintf(vel); vel_tmp = lrintf(vel);
// set vario // set vario
@ -439,20 +443,20 @@ int getEstimatedAltitude(void)
// Altitude P-Controller // Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500); error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s setVel = constrain((currentProfile.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
// Velocity PID-Controller // Velocity PID-Controller
// P // P
error = setVel - vel_tmp; error = setVel - vel_tmp;
BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300); BaroPID = constrain((currentProfile.P8[PIDVEL] * error / 32), -300, +300);
// I // I
errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8; errorAltitudeI += (currentProfile.I8[PIDVEL] * error) / 8;
errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200)); errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
BaroPID += errorAltitudeI / 1024; // I in range +/-200 BaroPID += errorAltitudeI / 1024; // I in range +/-200
// D // D
BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); BaroPID -= constrain(currentProfile.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
accZ_old = accZ_tmp; accZ_old = accZ_tmp;
return 1; return 1;

102
src/flight_mixer.c
View file

@ -159,14 +159,14 @@ const mixer_t mixers[] = {
int16_t determineServoMiddleOrForwardFromChannel(int nr) int16_t determineServoMiddleOrForwardFromChannel(int nr)
{ {
uint8_t channelToForwardFrom = cfg.servoConf[nr].forwardFromChannel; uint8_t channelToForwardFrom = currentProfile.servoConf[nr].forwardFromChannel;
if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom << MAX_SUPPORTED_RC_CHANNEL_COUNT) { if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom << MAX_SUPPORTED_RC_CHANNEL_COUNT) {
return rcData[channelToForwardFrom]; return rcData[channelToForwardFrom];
} }
if (nr < MAX_SERVOS) { if (nr < MAX_SERVOS) {
return cfg.servoConf[nr].middle; return currentProfile.servoConf[nr].middle;
} }
return DEFAULT_SERVO_MIDDLE; return DEFAULT_SERVO_MIDDLE;
@ -180,7 +180,7 @@ int servoDirection(int nr, int lr)
// rate[1] = roll_direction // rate[1] = roll_direction
// rate[0] = pitch_direction // rate[0] = pitch_direction
// servo.rate is also used as gimbal gain multiplier (yeah) // servo.rate is also used as gimbal gain multiplier (yeah)
if (cfg.servoConf[nr].rate & lr) if (currentProfile.servoConf[nr].rate & lr)
return -1; return -1;
else else
return 1; return 1;
@ -191,26 +191,26 @@ void mixerInit(void)
int i; int i;
// enable servos for mixes that require them. note, this shifts motor counts. // enable servos for mixes that require them. note, this shifts motor counts.
useServo = mixers[mcfg.mixerConfiguration].useServo; useServo = mixers[masterConfig.mixerConfiguration].useServo;
// if we want camstab/trig, that also enables servos, even if mixer doesn't // if we want camstab/trig, that also enables servos, even if mixer doesn't
if (feature(FEATURE_SERVO_TILT)) if (feature(FEATURE_SERVO_TILT))
useServo = 1; useServo = 1;
if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) { if (masterConfig.mixerConfiguration == MULTITYPE_CUSTOM) {
// load custom mixer into currentMixer // load custom mixer into currentMixer
for (i = 0; i < MAX_MOTORS; i++) { for (i = 0; i < MAX_MOTORS; i++) {
// check if done // check if done
if (mcfg.customMixer[i].throttle == 0.0f) if (masterConfig.customMixer[i].throttle == 0.0f)
break; break;
currentMixer[i] = mcfg.customMixer[i]; currentMixer[i] = masterConfig.customMixer[i];
numberMotor++; numberMotor++;
} }
} else { } else {
numberMotor = mixers[mcfg.mixerConfiguration].numberMotor; numberMotor = mixers[masterConfig.mixerConfiguration].numberMotor;
// copy motor-based mixers // copy motor-based mixers
if (mixers[mcfg.mixerConfiguration].motor) { if (mixers[masterConfig.mixerConfiguration].motor) {
for (i = 0; i < numberMotor; i++) for (i = 0; i < numberMotor; i++)
currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i]; currentMixer[i] = mixers[masterConfig.mixerConfiguration].motor[i];
} }
} }
@ -226,8 +226,8 @@ void mixerInit(void)
} }
// set flag that we're on something with wings // set flag that we're on something with wings
if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING ||
mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE)
f.FIXED_WING = 1; f.FIXED_WING = 1;
else else
f.FIXED_WING = 0; f.FIXED_WING = 0;
@ -240,7 +240,7 @@ void mixerResetMotors(void)
int i; int i;
// set disarmed motor values // set disarmed motor values
for (i = 0; i < MAX_MOTORS; i++) for (i = 0; i < MAX_MOTORS; i++)
motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand; motor_disarmed[i] = feature(FEATURE_3D) ? masterConfig.neutral3d : masterConfig.mincommand;
} }
void mixerLoadMix(int index) void mixerLoadMix(int index)
@ -251,12 +251,12 @@ void mixerLoadMix(int index)
index++; index++;
// clear existing // clear existing
for (i = 0; i < MAX_MOTORS; i++) for (i = 0; i < MAX_MOTORS; i++)
mcfg.customMixer[i].throttle = 0.0f; masterConfig.customMixer[i].throttle = 0.0f;
// do we have anything here to begin with? // do we have anything here to begin with?
if (mixers[index].motor != NULL) { if (mixers[index].motor != NULL) {
for (i = 0; i < mixers[index].numberMotor; i++) for (i = 0; i < mixers[index].numberMotor; i++)
mcfg.customMixer[i] = mixers[index].motor[i]; masterConfig.customMixer[i] = mixers[index].motor[i];
} }
} }
@ -271,14 +271,14 @@ void writeServos(void)
if (!useServo) if (!useServo)
return; return;
switch (mcfg.mixerConfiguration) { switch (masterConfig.mixerConfiguration) {
case MULTITYPE_BI: case MULTITYPE_BI:
pwmWriteServo(0, servo[4]); pwmWriteServo(0, servo[4]);
pwmWriteServo(1, servo[5]); pwmWriteServo(1, servo[5]);
break; break;
case MULTITYPE_TRI: case MULTITYPE_TRI:
if (cfg.tri_unarmed_servo) { if (currentProfile.tri_unarmed_servo) {
// if unarmed flag set, we always move servo // if unarmed flag set, we always move servo
pwmWriteServo(0, servo[5]); pwmWriteServo(0, servo[5]);
} else { } else {
@ -345,33 +345,33 @@ static void airplaneMixer(void)
int i; int i;
if (!f.ARMED) if (!f.ARMED)
servo[7] = mcfg.mincommand; // Kill throttle when disarmed servo[7] = masterConfig.mincommand; // Kill throttle when disarmed
else else
servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); servo[7] = constrain(rcCommand[THROTTLE], masterConfig.minthrottle, masterConfig.maxthrottle);
motor[0] = servo[7]; motor[0] = servo[7];
#if 0 #if 0
if (cfg.flaperons) { if (currentProfile.flaperons) {
} }
#endif #endif
if (mcfg.flaps_speed) { if (masterConfig.flaps_speed) {
// configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control // configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control
// use servo min, servo max and servo rate for proper endpoints adjust // use servo min, servo max and servo rate for proper endpoints adjust
static int16_t slow_LFlaps; static int16_t slow_LFlaps;
int16_t lFlap = determineServoMiddleOrForwardFromChannel(2); int16_t lFlap = determineServoMiddleOrForwardFromChannel(2);
lFlap = constrain(lFlap, cfg.servoConf[2].min, cfg.servoConf[2].max); lFlap = constrain(lFlap, currentProfile.servoConf[2].min, currentProfile.servoConf[2].max);
lFlap = mcfg.rxConfig.midrc - lFlap; // shouldn't this be servoConf[2].middle? lFlap = masterConfig.rxConfig.midrc - lFlap; // shouldn't this be servoConf[2].middle?
if (slow_LFlaps < lFlap) if (slow_LFlaps < lFlap)
slow_LFlaps += mcfg.flaps_speed; slow_LFlaps += masterConfig.flaps_speed;
else if (slow_LFlaps > lFlap) else if (slow_LFlaps > lFlap)
slow_LFlaps -= mcfg.flaps_speed; slow_LFlaps -= masterConfig.flaps_speed;
servo[2] = ((int32_t)cfg.servoConf[2].rate * slow_LFlaps) / 100L; servo[2] = ((int32_t)currentProfile.servoConf[2].rate * slow_LFlaps) / 100L;
servo[2] += mcfg.rxConfig.midrc; servo[2] += masterConfig.rxConfig.midrc;
} }
if (f.PASSTHRU_MODE) { // Direct passthru from RX if (f.PASSTHRU_MODE) { // Direct passthru from RX
@ -387,7 +387,7 @@ static void airplaneMixer(void)
servo[6] = axisPID[PITCH]; // Elevator servo[6] = axisPID[PITCH]; // Elevator
} }
for (i = 3; i < 7; i++) { for (i = 3; i < 7; i++) {
servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100L; // servo rates servo[i] = ((int32_t)currentProfile.servoConf[i].rate * servo[i]) / 100L; // servo rates
servo[i] += determineServoMiddleOrForwardFromChannel(i); servo[i] += determineServoMiddleOrForwardFromChannel(i);
} }
} }
@ -405,10 +405,10 @@ void mixTable(void)
// motors for non-servo mixes // motors for non-servo mixes
if (numberMotor > 1) if (numberMotor > 1)
for (i = 0; i < numberMotor; i++) for (i = 0; i < numberMotor; i++)
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw; motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -currentProfile.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
// airplane / servo mixes // airplane / servo mixes
switch (mcfg.mixerConfiguration) { switch (masterConfig.mixerConfiguration) {
case MULTITYPE_BI: case MULTITYPE_BI:
servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(4); // LEFT servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(4); // LEFT
servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(5); // RIGHT servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(5); // RIGHT
@ -419,8 +419,8 @@ void mixTable(void)
break; break;
case MULTITYPE_GIMBAL: case MULTITYPE_GIMBAL:
servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + determineServoMiddleOrForwardFromChannel(0); servo[0] = (((int32_t)currentProfile.servoConf[0].rate * angle[PITCH]) / 50) + determineServoMiddleOrForwardFromChannel(0);
servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + determineServoMiddleOrForwardFromChannel(1); servo[1] = (((int32_t)currentProfile.servoConf[1].rate * angle[ROLL]) / 50) + determineServoMiddleOrForwardFromChannel(1);
break; break;
case MULTITYPE_AIRPLANE: case MULTITYPE_AIRPLANE:
@ -429,9 +429,9 @@ void mixTable(void)
case MULTITYPE_FLYING_WING: case MULTITYPE_FLYING_WING:
if (!f.ARMED) if (!f.ARMED)
servo[7] = mcfg.mincommand; servo[7] = masterConfig.mincommand;
else else
servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); servo[7] = constrain(rcCommand[THROTTLE], masterConfig.minthrottle, masterConfig.maxthrottle);
motor[0] = servo[7]; motor[0] = servo[7];
if (f.PASSTHRU_MODE) { if (f.PASSTHRU_MODE) {
// do not use sensors for correction, simple 2 channel mixing // do not use sensors for correction, simple 2 channel mixing
@ -469,28 +469,28 @@ void mixTable(void)
servo[1] = determineServoMiddleOrForwardFromChannel(1); servo[1] = determineServoMiddleOrForwardFromChannel(1);
if (rcOptions[BOXCAMSTAB]) { if (rcOptions[BOXCAMSTAB]) {
if (cfg.gimbal_flags & GIMBAL_MIXTILT) { if (currentProfile.gimbal_flags & GIMBAL_MIXTILT) {
servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; servo[0] -= (-(int32_t)currentProfile.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)currentProfile.servoConf[1].rate * angle[ROLL] / 50;
servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; servo[1] += (-(int32_t)currentProfile.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)currentProfile.servoConf[1].rate * angle[ROLL] / 50;
} else { } else {
servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50; servo[0] += (int32_t)currentProfile.servoConf[0].rate * angle[PITCH] / 50;
servo[1] += (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; servo[1] += (int32_t)currentProfile.servoConf[1].rate * angle[ROLL] / 50;
} }
} }
} }
// constrain servos // constrain servos
for (i = 0; i < MAX_SERVOS; i++) for (i = 0; i < MAX_SERVOS; i++)
servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values servo[i] = constrain(servo[i], currentProfile.servoConf[i].min, currentProfile.servoConf[i].max); // limit the values
// forward AUX1-4 to servo outputs (not constrained) // forward AUX1-4 to servo outputs (not constrained)
if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { if (currentProfile.gimbal_flags & GIMBAL_FORWARDAUX) {
int offset = 0; int offset = 0;
// offset servos based off number already used in mixer types // offset servos based off number already used in mixer types
// airplane and servo_tilt together can't be used // airplane and servo_tilt together can't be used
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) if (masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE || masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING)
offset = 4; offset = 4;
else if (mixers[mcfg.mixerConfiguration].useServo) else if (mixers[masterConfig.mixerConfiguration].useServo)
offset = 2; offset = 2;
for (i = 0; i < 4; i++) for (i = 0; i < 4; i++)
pwmWriteServo(i + offset, rcData[AUX1 + i]); pwmWriteServo(i + offset, rcData[AUX1 + i]);
@ -501,21 +501,21 @@ void mixTable(void)
if (motor[i] > maxMotor) if (motor[i] > maxMotor)
maxMotor = motor[i]; maxMotor = motor[i];
for (i = 0; i < numberMotor; i++) { for (i = 0; i < numberMotor; i++) {
if (maxMotor > mcfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. if (maxMotor > masterConfig.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - mcfg.maxthrottle; motor[i] -= maxMotor - masterConfig.maxthrottle;
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
if ((rcData[THROTTLE]) > 1500) { if ((rcData[THROTTLE]) > 1500) {
motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle); motor[i] = constrain(motor[i], masterConfig.deadband3d_high, masterConfig.maxthrottle);
} else { } else {
motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low); motor[i] = constrain(motor[i], masterConfig.mincommand, masterConfig.deadband3d_low);
} }
} else { } else {
motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); motor[i] = constrain(motor[i], masterConfig.minthrottle, masterConfig.maxthrottle);
if ((rcData[THROTTLE]) < mcfg.rxConfig.mincheck) { if ((rcData[THROTTLE]) < masterConfig.rxConfig.mincheck) {
if (!feature(FEATURE_MOTOR_STOP)) if (!feature(FEATURE_MOTOR_STOP))
motor[i] = mcfg.minthrottle; motor[i] = masterConfig.minthrottle;
else else
motor[i] = mcfg.mincommand; motor[i] = masterConfig.mincommand;
} }
} }
if (!f.ARMED) { if (!f.ARMED) {

44
src/gps_common.c
View file

@ -93,7 +93,7 @@ void gpsInit(uint8_t baudrateIndex)
gpsData.lastMessage = millis(); gpsData.lastMessage = millis();
gpsData.errors = 0; gpsData.errors = 0;
// only RX is needed for NMEA-style GPS // only RX is needed for NMEA-style GPS
if (mcfg.gps_type == GPS_NMEA) if (masterConfig.gps_type == GPS_NMEA)
mode = MODE_RX; mode = MODE_RX;
gpsSetPIDs(); gpsSetPIDs();
@ -105,7 +105,7 @@ void gpsInit(uint8_t baudrateIndex)
void gpsInitHardware(void) void gpsInitHardware(void)
{ {
switch (mcfg.gps_type) { switch (masterConfig.gps_type) {
case GPS_NMEA: case GPS_NMEA:
// nothing to do, just set baud rate and try receiving some stuff and see if it parses // nothing to do, just set baud rate and try receiving some stuff and see if it parses
serialSetBaudRate(serialPorts.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate); serialSetBaudRate(serialPorts.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
@ -203,7 +203,7 @@ void gpsThread(void)
static bool gpsNewFrame(uint8_t c) static bool gpsNewFrame(uint8_t c)
{ {
switch (mcfg.gps_type) { switch (masterConfig.gps_type) {
case GPS_NMEA: // NMEA case GPS_NMEA: // NMEA
case GPS_MTK_NMEA: // MTK in NMEA mode case GPS_MTK_NMEA: // MTK in NMEA mode
return gpsNewFrameNMEA(c); return gpsNewFrameNMEA(c);
@ -312,11 +312,11 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
pid->derivative = (input - pid->last_input) / *dt; pid->derivative = (input - pid->last_input) / *dt;
// Low pass filter cut frequency for derivative calculation // Low pass filter cut frequency for derivative calculation
// Set to "1 / ( 2 * PI * gps_lpf )" // Set to "1 / ( 2 * PI * gps_lpf )
#define PID_FILTER (1.0f / (2.0f * M_PI * (float)cfg.gps_lpf)) float pidFilter = (1.0f / (2.0f * M_PI * (float)currentProfile.gps_lpf));
// discrete low pass filter, cuts out the // discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy // high frequency noise that can drive the controller crazy
pid->derivative = pid->last_derivative + (*dt / (PID_FILTER + *dt)) * (pid->derivative - pid->last_derivative); pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);
// update state // update state
pid->last_input = input; pid->last_input = input;
pid->last_derivative = pid->derivative; pid->last_derivative = pid->derivative;
@ -471,13 +471,13 @@ static void gpsNewData(uint16_t c)
break; break;
case NAV_MODE_WP: case NAV_MODE_WP:
speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV); // slow navigation speed = GPS_calc_desired_speed(currentProfile.nav_speed_max, NAV_SLOW_NAV); // slow navigation
// use error as the desired rate towards the target // use error as the desired rate towards the target
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100 // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_nav_rate(speed); GPS_calc_nav_rate(speed);
// Tail control // Tail control
if (cfg.nav_controls_heading) { if (currentProfile.nav_controls_heading) {
if (NAV_TAIL_FIRST) { if (NAV_TAIL_FIRST) {
magHold = wrap_18000(nav_bearing - 18000) / 100; magHold = wrap_18000(nav_bearing - 18000) / 100;
} else { } else {
@ -485,7 +485,7 @@ static void gpsNewData(uint16_t c)
} }
} }
// Are we there yet ?(within x meters of the destination) // Are we there yet ?(within x meters of the destination)
if ((wp_distance <= cfg.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode if ((wp_distance <= currentProfile.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode
nav_mode = NAV_MODE_POSHOLD; nav_mode = NAV_MODE_POSHOLD;
if (NAV_SET_TAKEOFF_HEADING) { if (NAV_SET_TAKEOFF_HEADING) {
magHold = nav_takeoff_bearing; magHold = nav_takeoff_bearing;
@ -528,18 +528,18 @@ void GPS_reset_nav(void)
// Get the relevant P I D values and set the PID controllers // Get the relevant P I D values and set the PID controllers
void gpsSetPIDs(void) void gpsSetPIDs(void)
{ {
posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f; posholdPID_PARAM.kP = (float)currentProfile.P8[PIDPOS] / 100.0f;
posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f; posholdPID_PARAM.kI = (float)currentProfile.I8[PIDPOS] / 100.0f;
posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
poshold_ratePID_PARAM.kP = (float)cfg.P8[PIDPOSR] / 10.0f; poshold_ratePID_PARAM.kP = (float)currentProfile.P8[PIDPOSR] / 10.0f;
poshold_ratePID_PARAM.kI = (float)cfg.I8[PIDPOSR] / 100.0f; poshold_ratePID_PARAM.kI = (float)currentProfile.I8[PIDPOSR] / 100.0f;
poshold_ratePID_PARAM.kD = (float)cfg.D8[PIDPOSR] / 1000.0f; poshold_ratePID_PARAM.kD = (float)currentProfile.D8[PIDPOSR] / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
navPID_PARAM.kP = (float)cfg.P8[PIDNAVR] / 10.0f; navPID_PARAM.kP = (float)currentProfile.P8[PIDNAVR] / 10.0f;
navPID_PARAM.kI = (float)cfg.I8[PIDNAVR] / 100.0f; navPID_PARAM.kI = (float)currentProfile.I8[PIDNAVR] / 100.0f;
navPID_PARAM.kD = (float)cfg.D8[PIDNAVR] / 1000.0f; navPID_PARAM.kD = (float)currentProfile.D8[PIDNAVR] / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
} }
@ -597,7 +597,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
nav_bearing = target_bearing; nav_bearing = target_bearing;
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
original_target_bearing = target_bearing; original_target_bearing = target_bearing;
waypoint_speed_gov = cfg.nav_speed_min; waypoint_speed_gov = currentProfile.nav_speed_min;
} }
//////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////
@ -773,7 +773,7 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
max_speed = min(max_speed, wp_distance / 2); max_speed = min(max_speed, wp_distance / 2);
} else { } else {
max_speed = min(max_speed, wp_distance); max_speed = min(max_speed, wp_distance);
max_speed = max(max_speed, cfg.nav_speed_min); // go at least 100cm/s max_speed = max(max_speed, currentProfile.nav_speed_min); // go at least 100cm/s
} }
// limit the ramp up of the speed // limit the ramp up of the speed
@ -1273,9 +1273,9 @@ void updateGpsState(void)
{ {
float sin_yaw_y = sinf(heading * 0.0174532925f); float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f); float cos_yaw_x = cosf(heading * 0.0174532925f);
if (cfg.nav_slew_rate) { if (currentProfile.nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8 nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -currentProfile.nav_slew_rate, currentProfile.nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.nav_slew_rate); nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -currentProfile.nav_slew_rate, currentProfile.nav_slew_rate);
GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10; GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10; GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else { } else {

45
src/main.c
View file

@ -13,7 +13,6 @@
#include "telemetry_common.h" #include "telemetry_common.h"
#include "boardalignment.h" #include "boardalignment.h"
#include "config.h" #include "config.h"
#include "config_storage.h"
#include "build_config.h" #include "build_config.h"
@ -37,29 +36,29 @@ int main(void)
serialPort_t* loopbackPort1 = NULL; serialPort_t* loopbackPort1 = NULL;
serialPort_t* loopbackPort2 = NULL; serialPort_t* loopbackPort2 = NULL;
#endif #endif
systemInit(mcfg.emfAvoidance); systemInit(masterConfig.emfAvoidance);
initPrintfSupport(); initPrintfSupport();
ensureEEPROMContainsValidData(); ensureEEPROMContainsValidData();
readEEPROM(); readEEPROM();
// configure power ADC // configure power ADC
if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) if (masterConfig.power_adc_channel > 0 && (masterConfig.power_adc_channel == 1 || masterConfig.power_adc_channel == 9))
adc_params.powerAdcChannel = mcfg.power_adc_channel; adc_params.powerAdcChannel = masterConfig.power_adc_channel;
else { else {
adc_params.powerAdcChannel = 0; adc_params.powerAdcChannel = 0;
mcfg.power_adc_channel = 0; masterConfig.power_adc_channel = 0;
} }
adcInit(&adc_params); adcInit(&adc_params);
initBoardAlignment(&mcfg.boardAlignment); initBoardAlignment(&masterConfig.boardAlignment);
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET); sensorsSet(SENSORS_SET);
mixerInit(); mixerInit();
// when using airplane/wing mixer, servo/motor outputs are remapped // when using airplane/wing mixer, servo/motor outputs are remapped
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) if (masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE || masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true; pwm_params.airplane = true;
else else
pwm_params.airplane = false; pwm_params.airplane = false;
@ -68,18 +67,18 @@ int main(void)
pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
pwm_params.useServos = isMixerUsingServos(); pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.extraServos = currentProfile.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate; pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D)) if (feature(FEATURE_3D))
pwm_params.idlePulse = mcfg.neutral3d; pwm_params.idlePulse = masterConfig.neutral3d;
if (pwm_params.motorPwmRate > 500) if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = mcfg.rxConfig.midrc; pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
pwm_params.failsafeThreshold = cfg.failsafeConfig.failsafe_detect_threshold; pwm_params.failsafeThreshold = currentProfile.failsafeConfig.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) { switch (masterConfig.power_adc_channel) {
case 1: case 1:
pwm_params.adcChannel = PWM2; pwm_params.adcChannel = PWM2;
break; break;
@ -91,14 +90,14 @@ int main(void)
break; break;
} }
failsafe = failsafeInit(&mcfg.rxConfig); failsafe = failsafeInit(&masterConfig.rxConfig);
buzzerInit(failsafe); buzzerInit(failsafe);
pwmInit(&pwm_params, failsafe); pwmInit(&pwm_params, failsafe);
rxInit(&mcfg.rxConfig, failsafe); rxInit(&masterConfig.rxConfig, failsafe);
if (feature(FEATURE_GPS) && !feature(FEATURE_SERIALRX)) { if (feature(FEATURE_GPS) && !feature(FEATURE_SERIALRX)) {
gpsInit(mcfg.gps_baudrate); gpsInit(masterConfig.gps_baudrate);
} }
#ifdef SONAR #ifdef SONAR
@ -128,16 +127,16 @@ int main(void)
// Check battery type/voltage // Check battery type/voltage
if (feature(FEATURE_VBAT)) if (feature(FEATURE_VBAT))
batteryInit(&mcfg.batteryConfig); batteryInit(&masterConfig.batteryConfig);
serialInit(&mcfg.serialConfig); serialInit(&masterConfig.serialConfig);
#ifndef FY90Q #ifndef FY90Q
if (feature(FEATURE_SOFTSERIAL)) { if (feature(FEATURE_SOFTSERIAL)) {
//mcfg.softserial_baudrate = 19200; // Uncomment to override config value //mcfg.softserial_baudrate = 19200; // Uncomment to override config value
setupSoftSerialPrimary(mcfg.serialConfig.softserial_baudrate, mcfg.serialConfig.softserial_1_inverted); setupSoftSerialPrimary(masterConfig.serialConfig.softserial_baudrate, masterConfig.serialConfig.softserial_1_inverted);
setupSoftSerialSecondary(mcfg.serialConfig.softserial_2_inverted); setupSoftSerialSecondary(masterConfig.serialConfig.softserial_2_inverted);
#ifdef SOFTSERIAL_LOOPBACK #ifdef SOFTSERIAL_LOOPBACK
loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]); loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]);
@ -154,7 +153,7 @@ int main(void)
initTelemetry(&serialPorts); initTelemetry(&serialPorts);
previousTime = micros(); previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = CALIBRATING_ACC_CYCLES; calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES; calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
@ -192,6 +191,6 @@ int main(void)
void HardFault_Handler(void) void HardFault_Handler(void)
{ {
// fall out of the sky // fall out of the sky
writeAllMotors(mcfg.mincommand); writeAllMotors(masterConfig.mincommand);
while (1); while (1);
} }

134
src/mw.c
View file

@ -79,22 +79,22 @@ void annexCode(void)
static uint8_t vbatTimer = 0; static uint8_t vbatTimer = 0;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < cfg.tpaBreakPoint) { if (rcData[THROTTLE] < currentProfile.tpaBreakPoint) {
prop2 = 100; prop2 = 100;
} else { } else {
if (rcData[THROTTLE] < 2000) { if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpaBreakPoint) / (2000 - cfg.tpaBreakPoint); prop2 = 100 - (uint16_t)currentProfile.dynThrPID * (rcData[THROTTLE] - currentProfile.tpaBreakPoint) / (2000 - currentProfile.tpaBreakPoint);
} else { } else {
prop2 = 100 - cfg.dynThrPID; prop2 = 100 - currentProfile.dynThrPID;
} }
} }
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - mcfg.rxConfig.midrc), 500); tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis != 2) { // ROLL & PITCH if (axis != 2) { // ROLL & PITCH
if (cfg.deadband) { if (currentProfile.deadband) {
if (tmp > cfg.deadband) { if (tmp > currentProfile.deadband) {
tmp -= cfg.deadband; tmp -= currentProfile.deadband;
} else { } else {
tmp = 0; tmp = 0;
} }
@ -102,28 +102,28 @@ void annexCode(void)
tmp2 = tmp / 100; tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)cfg.controlRateConfig.rollPitchRate * tmp / 500; prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100; prop1 = (uint16_t)prop1 * prop2 / 100;
} else { // YAW } else { // YAW
if (cfg.yawdeadband) { if (currentProfile.yawdeadband) {
if (tmp > cfg.yawdeadband) { if (tmp > currentProfile.yawdeadband) {
tmp -= cfg.yawdeadband; tmp -= currentProfile.yawdeadband;
} else { } else {
tmp = 0; tmp = 0;
} }
} }
rcCommand[axis] = tmp * -mcfg.yaw_control_direction; rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)cfg.controlRateConfig.yawRate * abs(tmp) / 500; prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500;
} }
dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100; dynP8[axis] = (uint16_t)currentProfile.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile.D8[axis] * prop1 / 100;
if (rcData[axis] < mcfg.rxConfig.midrc) if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis]; rcCommand[axis] = -rcCommand[axis];
} }
tmp = constrain(rcData[THROTTLE], mcfg.rxConfig.mincheck, PWM_RANGE_MAX); tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - mcfg.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - mcfg.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100; tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
@ -237,15 +237,15 @@ static void pidMultiWii(void)
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// 50 degrees max inclination // 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis]; errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis];
PTermACC = errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = errorAngle * currentProfile.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5); PTermACC = constrain(PTermACC, -currentProfile.D8[PIDLEVEL] * 5, +currentProfile.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = (errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; ITermACC = (errorAngleI[axis] * currentProfile.I8[PIDLEVEL]) >> 12;
} }
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis]; error = (int32_t)rcCommand[axis] * 10 * 8 / currentProfile.P8[axis];
error -= gyroData[axis]; error -= gyroData[axis];
PTermGYRO = rcCommand[axis]; PTermGYRO = rcCommand[axis];
@ -253,7 +253,7 @@ static void pidMultiWii(void)
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640) if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0; errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; ITermGYRO = (errorGyroI[axis] / 125 * currentProfile.I8[axis]) >> 6;
} }
if (f.HORIZON_MODE && axis < 2) { if (f.HORIZON_MODE && axis < 2) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
@ -296,19 +296,19 @@ static void pidRewrite(void)
// -----Get the desired angle rate depending on flight mode // -----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// calculate error and limit the angle to max configured inclination // calculate error and limit the angle to max configured inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis]; // 16 bits is ok here
} }
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t)(cfg.controlRateConfig.yawRate + 27) * rcCommand[2]) >> 5); AngleRateTmp = (((int32_t)(currentProfile.controlRateConfig.yawRate + 27) * rcCommand[2]) >> 5);
} else { } else {
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t) (cfg.controlRateConfig.rollPitchRate + 27) * rcCommand[axis]) >> 4; AngleRateTmp = ((int32_t) (currentProfile.controlRateConfig.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) { if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel // mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * cfg.I8[PIDLEVEL]) >> 8; AngleRateTmp += (errorAngle * currentProfile.I8[PIDLEVEL]) >> 8;
} }
} else { // it's the ANGLE mode - control is angle based, so control loop is needed } else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * cfg.P8[PIDLEVEL]) >> 4; AngleRateTmp = (errorAngle * currentProfile.P8[PIDLEVEL]) >> 4;
} }
} }
@ -319,13 +319,13 @@ static void pidRewrite(void)
RateError = AngleRateTmp - gyroData[axis]; RateError = AngleRateTmp - gyroData[axis];
// -----calculate P component // -----calculate P component
PTerm = (RateError * cfg.P8[axis]) >> 7; PTerm = (RateError * currentProfile.P8[axis]) >> 7;
// -----calculate I component // -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced. // there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time) // Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048. // is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * cfg.I8[axis]; errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * currentProfile.I8[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings // I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -343,7 +343,7 @@ static void pidRewrite(void)
deltaSum = delta1[axis] + delta2[axis] + delta; deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis]; delta2[axis] = delta1[axis];
delta1[axis] = delta; delta1[axis] = delta;
DTerm = (deltaSum * cfg.D8[axis]) >> 8; DTerm = (deltaSum * currentProfile.D8[axis]) >> 8;
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm; axisPID[axis] = PTerm + ITerm + DTerm;
@ -381,7 +381,7 @@ void loop(void)
// calculate rc stuff from serial-based receivers (spek/sbus) // calculate rc stuff from serial-based receivers (spek/sbus)
if (feature(FEATURE_SERIALRX)) { if (feature(FEATURE_SERIALRX)) {
switch (mcfg.rxConfig.serialrx_type) { switch (masterConfig.rxConfig.serialrx_type) {
case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048: case SERIALRX_SPEKTRUM2048:
rcReady = spektrumFrameComplete(); rcReady = spektrumFrameComplete();
@ -398,7 +398,7 @@ void loop(void)
if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven
rcReady = false; rcReady = false;
rcTime = currentTime + 20000; rcTime = currentTime + 20000;
computeRC(&mcfg.rxConfig, &rxRuntimeConfig); computeRC(&masterConfig.rxConfig, &rxRuntimeConfig);
// in 3D mode, we need to be able to disarm by switch at any time // in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
@ -408,8 +408,8 @@ void loop(void)
// Read value of AUX channel as rssi // Read value of AUX channel as rssi
// 0 is disable, 1-4 is AUX{1..4} // 0 is disable, 1-4 is AUX{1..4}
if (mcfg.rssi_aux_channel > 0) { if (masterConfig.rssi_aux_channel > 0) {
const int16_t rssiChannelData = rcData[AUX1 + mcfg.rssi_aux_channel - 1]; const int16_t rssiChannelData = rcData[AUX1 + masterConfig.rssi_aux_channel - 1];
// Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023]; // Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023];
rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f); rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f);
} }
@ -422,9 +422,9 @@ void loop(void)
// checking sticks positions // checking sticks positions
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++) {
stTmp >>= 2; stTmp >>= 2;
if (rcData[i] > mcfg.rxConfig.mincheck) if (rcData[i] > masterConfig.rxConfig.mincheck)
stTmp |= 0x80; // check for MIN stTmp |= 0x80; // check for MIN
if (rcData[i] < mcfg.rxConfig.maxcheck) if (rcData[i] < masterConfig.rxConfig.maxcheck)
stTmp |= 0x40; // check for MAX stTmp |= 0x40; // check for MAX
} }
if (stTmp == rcSticks) { if (stTmp == rcSticks) {
@ -435,9 +435,9 @@ void loop(void)
rcSticks = stTmp; rcSticks = stTmp;
// perform actions // perform actions
if (feature(FEATURE_3D) && (rcData[THROTTLE] > (mcfg.rxConfig.midrc - mcfg.deadband3d_throttle) && rcData[THROTTLE] < (mcfg.rxConfig.midrc + mcfg.deadband3d_throttle))) if (feature(FEATURE_3D) && (rcData[THROTTLE] > (masterConfig.rxConfig.midrc - masterConfig.deadband3d_throttle) && rcData[THROTTLE] < (masterConfig.rxConfig.midrc + masterConfig.deadband3d_throttle)))
isThrottleLow = true; isThrottleLow = true;
else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < mcfg.rxConfig.mincheck)) else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < masterConfig.rxConfig.mincheck))
isThrottleLow = true; isThrottleLow = true;
if (isThrottleLow) { if (isThrottleLow) {
errorGyroI[ROLL] = 0; errorGyroI[ROLL] = 0;
@ -445,7 +445,7 @@ void loop(void)
errorGyroI[YAW] = 0; errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0; errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0; errorAngleI[PITCH] = 0;
if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX if (currentProfile.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
if (rcOptions[BOXARM] && f.OK_TO_ARM) if (rcOptions[BOXARM] && f.OK_TO_ARM)
mwArm(); mwArm();
else if (f.ARMED) else if (f.ARMED)
@ -456,10 +456,10 @@ void loop(void)
if (rcDelayCommand == 20) { if (rcDelayCommand == 20) {
if (f.ARMED) { // actions during armed if (f.ARMED) { // actions during armed
// Disarm on throttle down + yaw // Disarm on throttle down + yaw
if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE)) if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
mwDisarm(); mwDisarm();
// Disarm on roll (only when retarded_arm is enabled) // Disarm on roll (only when retarded_arm is enabled)
if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
mwDisarm(); mwDisarm();
} else { // actions during not armed } else { // actions during not armed
i = 0; i = 0;
@ -495,7 +495,7 @@ void loop(void)
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
i = 3; i = 3;
if (i) { if (i) {
mcfg.current_profile = i - 1; masterConfig.current_profile = i - 1;
writeEEPROM(); writeEEPROM();
readEEPROM(); readEEPROM();
blinkLedAndSoundBeeper(2, 40, i); blinkLedAndSoundBeeper(2, 40, i);
@ -503,10 +503,10 @@ void loop(void)
} }
// Arm via YAW // Arm via YAW
if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE))
mwArm(); mwArm();
// Arm via ROLL // Arm via ROLL
else if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) else if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI))
mwArm(); mwArm();
// Calibrating Acc // Calibrating Acc
else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
@ -517,20 +517,20 @@ void loop(void)
i = 0; i = 0;
// Acc Trim // Acc Trim
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
cfg.angleTrim[PITCH] += 2; currentProfile.angleTrim[PITCH] += 2;
i = 1; i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
cfg.angleTrim[PITCH] -= 2; currentProfile.angleTrim[PITCH] -= 2;
i = 1; i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
cfg.angleTrim[ROLL] += 2; currentProfile.angleTrim[ROLL] += 2;
i = 1; i = 1;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
cfg.angleTrim[ROLL] -= 2; currentProfile.angleTrim[ROLL] -= 2;
i = 1; i = 1;
} }
if (i) { if (i) {
copyCurrentProfileToProfileSlot(mcfg.current_profile); copyCurrentProfileToProfileSlot(masterConfig.current_profile);
writeEEPROM(); writeEEPROM();
readEEPROMAndNotify(); readEEPROMAndNotify();
rcDelayCommand = 0; // allow autorepetition rcDelayCommand = 0; // allow autorepetition
@ -539,7 +539,7 @@ void loop(void)
} }
if (feature(FEATURE_INFLIGHT_ACC_CAL)) { if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.rxConfig.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50; InflightcalibratingA = 50;
AccInflightCalibrationArmed = false; AccInflightCalibrationArmed = false;
} }
@ -557,7 +557,7 @@ void loop(void)
for (i = 0; i < 4; i++) for (i = 0; i < 4; i++)
auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2); auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) for (i = 0; i < CHECKBOX_ITEM_COUNT; i++)
rcOptions[i] = (auxState & cfg.activate[i]) > 0; rcOptions[i] = (auxState & currentProfile.activate[i]) > 0;
if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode // bumpless transfer to Level mode
@ -652,7 +652,7 @@ void loop(void)
} }
} else { } else {
f.GPS_HOME_MODE = 0; f.GPS_HOME_MODE = 0;
if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < cfg.ap_mode && abs(rcCommand[PITCH]) < cfg.ap_mode) { if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < currentProfile.ap_mode && abs(rcCommand[PITCH]) < currentProfile.ap_mode) {
if (!f.GPS_HOLD_MODE) { if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1; f.GPS_HOLD_MODE = 1;
GPSNavReset = 0; GPSNavReset = 0;
@ -683,7 +683,7 @@ void loop(void)
f.PASSTHRU_MODE = 0; f.PASSTHRU_MODE = 0;
} }
if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) { if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) {
f.HEADFREE_MODE = 0; f.HEADFREE_MODE = 0;
} }
} else { // not in rc loop } else { // not in rc loop
@ -730,8 +730,8 @@ void loop(void)
} }
currentTime = micros(); currentTime = micros();
if (mcfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + mcfg.looptime; loopTime = currentTime + masterConfig.looptime;
computeIMU(); computeIMU();
annexCode(); annexCode();
@ -748,9 +748,9 @@ void loop(void)
dif += 360; dif += 360;
if (dif >= +180) if (dif >= +180)
dif -= 360; dif -= 360;
dif *= -mcfg.yaw_control_direction; dif *= -masterConfig.yaw_control_direction;
if (f.SMALL_ANGLE) if (f.SMALL_ANGLE)
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg rcCommand[YAW] -= dif * currentProfile.P8[PIDMAG] / 30; // 18 deg
} else } else
magHold = heading; magHold = heading;
} }
@ -763,12 +763,12 @@ void loop(void)
static int16_t AltHoldCorr = 0; static int16_t AltHoldCorr = 0;
if (!f.FIXED_WING) { if (!f.FIXED_WING) {
// multirotor alt hold // multirotor alt hold
if (cfg.alt_hold_fast_change) { if (currentProfile.alt_hold_fast_change) {
// rapid alt changes // rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_throttle_neutral) {
errorAltitudeI = 0; errorAltitudeI = 0;
isAltHoldChanged = 1; isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral; rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_throttle_neutral : currentProfile.alt_hold_throttle_neutral;
} else { } else {
if (isAltHoldChanged) { if (isAltHoldChanged) {
AltHold = EstAlt; AltHold = EstAlt;
@ -778,7 +778,7 @@ void loop(void)
} }
} else { } else {
// slow alt changes for apfags // slow alt changes for apfags
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_throttle_neutral) {
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms) // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold; AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
AltHold += AltHoldCorr / 2000; AltHold += AltHoldCorr / 2000;
@ -790,19 +790,19 @@ void loop(void)
isAltHoldChanged = 0; isAltHoldChanged = 0;
} }
rcCommand[THROTTLE] = initialThrottleHold + BaroPID; rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle); rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], masterConfig.minthrottle + 150, masterConfig.maxthrottle);
} }
} else { } else {
// handle fixedwing-related althold. UNTESTED! and probably wrong // handle fixedwing-related althold. UNTESTED! and probably wrong
// most likely need to check changes on pitch channel and 'reset' althold similar to // most likely need to check changes on pitch channel and 'reset' althold similar to
// how throttle does it on multirotor // how throttle does it on multirotor
rcCommand[PITCH] += BaroPID * mcfg.fixedwing_althold_dir; rcCommand[PITCH] += BaroPID * masterConfig.fixedwing_althold_dir;
} }
} }
} }
#endif #endif
if (cfg.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) { if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {
rcCommand[THROTTLE] += throttleAngleCorrection; rcCommand[THROTTLE] += throttleAngleCorrection;
} }

3
src/mw.h
View file

@ -35,7 +35,8 @@ enum {
#include "serial_common.h" #include "serial_common.h"
#include "rx_common.h" #include "rx_common.h"
#include "config.h" #include "config.h"
#include "config_storage.h" #include "config_profile.h"
#include "config_master.h"
extern int16_t axisPID[3]; extern int16_t axisPID[3];
extern int16_t rcCommand[4]; extern int16_t rcCommand[4];

54
src/sensors_acceleration.c
View file

@ -32,16 +32,16 @@ void ACC_Common(void)
a[axis] += accADC[axis]; a[axis] += accADC[axis];
// Clear global variables for next reading // Clear global variables for next reading
accADC[axis] = 0; accADC[axis] = 0;
mcfg.accZero[axis] = 0; masterConfig.accZero[axis] = 0;
} }
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) { if (calibratingA == 1) {
mcfg.accZero[GI_ROLL] = (a[GI_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; masterConfig.accZero[GI_ROLL] = (a[GI_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
mcfg.accZero[GI_PITCH] = (a[GI_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; masterConfig.accZero[GI_PITCH] = (a[GI_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
mcfg.accZero[GI_YAW] = (a[GI_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G; masterConfig.accZero[GI_YAW] = (a[GI_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
cfg.angleTrim[AI_ROLL] = 0; currentProfile.angleTrim[AI_ROLL] = 0;
cfg.angleTrim[AI_PITCH] = 0; currentProfile.angleTrim[AI_PITCH] = 0;
copyCurrentProfileToProfileSlot(mcfg.current_profile); copyCurrentProfileToProfileSlot(masterConfig.current_profile);
writeEEPROM(); // write accZero in EEPROM writeEEPROM(); // write accZero in EEPROM
readEEPROMAndNotify(); readEEPROMAndNotify();
} }
@ -54,11 +54,11 @@ void ACC_Common(void)
static int16_t angleTrim_saved[2] = { 0, 0 }; static int16_t angleTrim_saved[2] = { 0, 0 };
// Saving old zeropoints before measurement // Saving old zeropoints before measurement
if (InflightcalibratingA == 50) { if (InflightcalibratingA == 50) {
accZero_saved[GI_ROLL] = mcfg.accZero[GI_ROLL]; accZero_saved[GI_ROLL] = masterConfig.accZero[GI_ROLL];
accZero_saved[GI_PITCH] = mcfg.accZero[GI_PITCH]; accZero_saved[GI_PITCH] = masterConfig.accZero[GI_PITCH];
accZero_saved[GI_YAW] = mcfg.accZero[GI_YAW]; accZero_saved[GI_YAW] = masterConfig.accZero[GI_YAW];
angleTrim_saved[AI_ROLL] = cfg.angleTrim[AI_ROLL]; angleTrim_saved[AI_ROLL] = currentProfile.angleTrim[AI_ROLL];
angleTrim_saved[AI_PITCH] = cfg.angleTrim[AI_PITCH]; angleTrim_saved[AI_PITCH] = currentProfile.angleTrim[AI_PITCH];
} }
if (InflightcalibratingA > 0) { if (InflightcalibratingA > 0) {
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
@ -69,7 +69,7 @@ void ACC_Common(void)
b[axis] += accADC[axis]; b[axis] += accADC[axis];
// Clear global variables for next reading // Clear global variables for next reading
accADC[axis] = 0; accADC[axis] = 0;
mcfg.accZero[axis] = 0; masterConfig.accZero[axis] = 0;
} }
// all values are measured // all values are measured
if (InflightcalibratingA == 1) { if (InflightcalibratingA == 1) {
@ -77,31 +77,31 @@ void ACC_Common(void)
AccInflightCalibrationMeasurementDone = true; AccInflightCalibrationMeasurementDone = true;
toggleBeep = 2; // buzzer for indicatiing the end of calibration toggleBeep = 2; // buzzer for indicatiing the end of calibration
// recover saved values to maintain current flight behavior until new values are transferred // recover saved values to maintain current flight behavior until new values are transferred
mcfg.accZero[GI_ROLL] = accZero_saved[GI_ROLL]; masterConfig.accZero[GI_ROLL] = accZero_saved[GI_ROLL];
mcfg.accZero[GI_PITCH] = accZero_saved[GI_PITCH]; masterConfig.accZero[GI_PITCH] = accZero_saved[GI_PITCH];
mcfg.accZero[GI_YAW] = accZero_saved[GI_YAW]; masterConfig.accZero[GI_YAW] = accZero_saved[GI_YAW];
cfg.angleTrim[AI_ROLL] = angleTrim_saved[AI_ROLL]; currentProfile.angleTrim[AI_ROLL] = angleTrim_saved[AI_ROLL];
cfg.angleTrim[AI_PITCH] = angleTrim_saved[AI_PITCH]; currentProfile.angleTrim[AI_PITCH] = angleTrim_saved[AI_PITCH];
} }
InflightcalibratingA--; InflightcalibratingA--;
} }
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = false; AccInflightCalibrationSavetoEEProm = false;
mcfg.accZero[GI_ROLL] = b[GI_ROLL] / 50; masterConfig.accZero[GI_ROLL] = b[GI_ROLL] / 50;
mcfg.accZero[GI_PITCH] = b[GI_PITCH] / 50; masterConfig.accZero[GI_PITCH] = b[GI_PITCH] / 50;
mcfg.accZero[GI_YAW] = b[GI_YAW] / 50 - acc_1G; // for nunchuk 200=1G masterConfig.accZero[GI_YAW] = b[GI_YAW] / 50 - acc_1G; // for nunchuk 200=1G
cfg.angleTrim[AI_ROLL] = 0; currentProfile.angleTrim[AI_ROLL] = 0;
cfg.angleTrim[AI_PITCH] = 0; currentProfile.angleTrim[AI_PITCH] = 0;
copyCurrentProfileToProfileSlot(mcfg.current_profile); copyCurrentProfileToProfileSlot(masterConfig.current_profile);
writeEEPROM(); // write accZero in EEPROM writeEEPROM(); // write accZero in EEPROM
readEEPROMAndNotify(); readEEPROMAndNotify();
} }
} }
accADC[GI_ROLL] -= mcfg.accZero[GI_ROLL]; accADC[GI_ROLL] -= masterConfig.accZero[GI_ROLL];
accADC[GI_PITCH] -= mcfg.accZero[GI_PITCH]; accADC[GI_PITCH] -= masterConfig.accZero[GI_PITCH];
accADC[GI_YAW] -= mcfg.accZero[GI_YAW]; accADC[GI_YAW] -= masterConfig.accZero[GI_YAW];
} }
void ACC_getADC(void) void ACC_getADC(void)

8
src/sensors_barometer.c
View file

@ -16,7 +16,7 @@ void Baro_Common(void)
int indexplus1; int indexplus1;
indexplus1 = (baroHistIdx + 1); indexplus1 = (baroHistIdx + 1);
if (indexplus1 == cfg.baro_tab_size) if (indexplus1 == currentProfile.baro_tab_size)
indexplus1 = 0; indexplus1 = 0;
baroHistTab[baroHistIdx] = baroPressure; baroHistTab[baroHistIdx] = baroPressure;
baroPressureSum += baroHistTab[baroHistIdx]; baroPressureSum += baroHistTab[baroHistIdx];
@ -58,9 +58,9 @@ int32_t Baro_calculateAltitude(void)
// calculates height from ground via baro readings // calculates height from ground via baro readings
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (currentProfile.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp -= baroGroundAltitude; BaroAlt_tmp -= baroGroundAltitude;
BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise BaroAlt = lrintf((float)BaroAlt * currentProfile.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - currentProfile.baro_noise_lpf)); // additional LPF to reduce baro noise
return BaroAlt; return BaroAlt;
} }
@ -68,7 +68,7 @@ int32_t Baro_calculateAltitude(void)
void performBaroCalibrationCycle(void) void performBaroCalibrationCycle(void)
{ {
baroGroundPressure -= baroGroundPressure / 8; baroGroundPressure -= baroGroundPressure / 8;
baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1); baroGroundPressure += baroPressureSum / (currentProfile.baro_tab_size - 1);
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
calibratingB--; calibratingB--;

12
src/sensors_compass.c
View file

@ -35,7 +35,7 @@ int Mag_getADC(void)
if (f.CALIBRATE_MAG) { if (f.CALIBRATE_MAG) {
tCal = t; tCal = t;
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
mcfg.magZero[axis] = 0; masterConfig.magZero[axis] = 0;
magZeroTempMin[axis] = magADC[axis]; magZeroTempMin[axis] = magADC[axis];
magZeroTempMax[axis] = magADC[axis]; magZeroTempMax[axis] = magADC[axis];
} }
@ -43,9 +43,9 @@ int Mag_getADC(void)
} }
if (magInit) { // we apply offset only once mag calibration is done if (magInit) { // we apply offset only once mag calibration is done
magADC[X] -= mcfg.magZero[X]; magADC[X] -= masterConfig.magZero[X];
magADC[Y] -= mcfg.magZero[Y]; magADC[Y] -= masterConfig.magZero[Y];
magADC[Z] -= mcfg.magZero[Z]; magADC[Z] -= masterConfig.magZero[Z];
} }
if (tCal != 0) { if (tCal != 0) {
@ -60,9 +60,9 @@ int Mag_getADC(void)
} else { } else {
tCal = 0; tCal = 0;
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets masterConfig.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
} }
copyCurrentProfileToProfileSlot(mcfg.current_profile); copyCurrentProfileToProfileSlot(masterConfig.current_profile);
writeEEPROM(); writeEEPROM();
readEEPROMAndNotify(); readEEPROMAndNotify();
} }

2
src/sensors_gyro.c
View file

@ -33,7 +33,7 @@ void GYRO_Common(void)
if (calibratingG == 1) { if (calibratingG == 1) {
float dev = devStandardDeviation(&var[axis]); float dev = devStandardDeviation(&var[axis]);
// check deviation and startover if idiot was moving the model // check deviation and startover if idiot was moving the model
if (mcfg.moron_threshold && dev > mcfg.moron_threshold) { if (masterConfig.moron_threshold && dev > masterConfig.moron_threshold) {
calibratingG = CALIBRATING_GYRO_CYCLES; calibratingG = CALIBRATING_GYRO_CYCLES;
devClear(&var[0]); devClear(&var[0]);
devClear(&var[1]); devClear(&var[1]);

38
src/sensors_initialisation.c
View file

@ -38,12 +38,12 @@ void sensorsAutodetect(void)
memset(&gyro, sizeof(gyro), 0); memset(&gyro, sizeof(gyro), 0);
// Autodetect gyro hardware. We have MPU3050 or MPU6050. // Autodetect gyro hardware. We have MPU3050 or MPU6050.
if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf)) { if (mpu6050Detect(&acc, &gyro, masterConfig.gyro_lpf)) {
haveMpu6k = true; haveMpu6k = true;
gyroAlign = CW0_DEG; // default NAZE alignment gyroAlign = CW0_DEG; // default NAZE alignment
} else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) { } else if (l3g4200dDetect(&gyro, masterConfig.gyro_lpf)) {
gyroAlign = CW0_DEG; gyroAlign = CW0_DEG;
} else if (mpu3050Detect(&gyro, mcfg.gyro_lpf)) { } else if (mpu3050Detect(&gyro, masterConfig.gyro_lpf)) {
gyroAlign = CW0_DEG; gyroAlign = CW0_DEG;
} else { } else {
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error. // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
@ -52,7 +52,7 @@ void sensorsAutodetect(void)
// Accelerometer. Fuck it. Let user break shit. // Accelerometer. Fuck it. Let user break shit.
retry: retry:
switch (mcfg.acc_hardware) { switch (masterConfig.acc_hardware) {
case ACC_NONE: // disable ACC case ACC_NONE: // disable ACC
sensorsClear(SENSOR_ACC); sensorsClear(SENSOR_ACC);
break; break;
@ -64,15 +64,15 @@ retry:
accHardware = ACC_ADXL345; accHardware = ACC_ADXL345;
accAlign = CW270_DEG; // default NAZE alignment accAlign = CW270_DEG; // default NAZE alignment
} }
if (mcfg.acc_hardware == ACC_ADXL345) if (masterConfig.acc_hardware == ACC_ADXL345)
break; break;
; // fallthrough ; // fallthrough
case ACC_MPU6050: // MPU6050 case ACC_MPU6050: // MPU6050
if (haveMpu6k) { if (haveMpu6k) {
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf); // yes, i'm rerunning it again. re-fill acc struct mpu6050Detect(&acc, &gyro, masterConfig.gyro_lpf); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050; accHardware = ACC_MPU6050;
accAlign = CW0_DEG; // default NAZE alignment accAlign = CW0_DEG; // default NAZE alignment
if (mcfg.acc_hardware == ACC_MPU6050) if (masterConfig.acc_hardware == ACC_MPU6050)
break; break;
} }
; // fallthrough ; // fallthrough
@ -81,7 +81,7 @@ retry:
if (mma8452Detect(&acc)) { if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452; accHardware = ACC_MMA8452;
accAlign = CW90_DEG; // default NAZE alignment accAlign = CW90_DEG; // default NAZE alignment
if (mcfg.acc_hardware == ACC_MMA8452) if (masterConfig.acc_hardware == ACC_MMA8452)
break; break;
} }
; // fallthrough ; // fallthrough
@ -89,7 +89,7 @@ retry:
if (bma280Detect(&acc)) { if (bma280Detect(&acc)) {
accHardware = ACC_BMA280; accHardware = ACC_BMA280;
accAlign = CW0_DEG; // accAlign = CW0_DEG; //
if (mcfg.acc_hardware == ACC_BMA280) if (masterConfig.acc_hardware == ACC_BMA280)
break; break;
} }
#endif #endif
@ -97,9 +97,9 @@ retry:
// Found anything? Check if user fucked up or ACC is really missing. // Found anything? Check if user fucked up or ACC is really missing.
if (accHardware == ACC_DEFAULT) { if (accHardware == ACC_DEFAULT) {
if (mcfg.acc_hardware > ACC_DEFAULT) { if (masterConfig.acc_hardware > ACC_DEFAULT) {
// Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present. // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
mcfg.acc_hardware = ACC_DEFAULT; masterConfig.acc_hardware = ACC_DEFAULT;
goto retry; goto retry;
} else { } else {
// We're really screwed // We're really screwed
@ -118,14 +118,14 @@ retry:
} }
#endif #endif
if (mcfg.gyro_align != ALIGN_DEFAULT) { if (masterConfig.gyro_align != ALIGN_DEFAULT) {
gyroAlign = mcfg.gyro_align; gyroAlign = masterConfig.gyro_align;
} }
if (mcfg.acc_align != ALIGN_DEFAULT) { if (masterConfig.acc_align != ALIGN_DEFAULT) {
accAlign = mcfg.acc_align; accAlign = masterConfig.acc_align;
} }
if (mcfg.mag_align != ALIGN_DEFAULT) { if (masterConfig.mag_align != ALIGN_DEFAULT) {
magAlign = mcfg.mag_align; magAlign = masterConfig.mag_align;
} }
@ -145,8 +145,8 @@ retry:
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
// calculate magnetic declination // calculate magnetic declination
deg = cfg.mag_declination / 100; deg = currentProfile.mag_declination / 100;
min = cfg.mag_declination % 100; min = currentProfile.mag_declination % 100;
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
} else { } else {

282
src/serial_cli.c
View file

@ -122,117 +122,117 @@ typedef struct {
} clivalue_t; } clivalue_t;
const clivalue_t valueTable[] = { const clivalue_t valueTable[] = {
{ "looptime", VAR_UINT16, &mcfg.looptime, 0, 9000 }, { "looptime", VAR_UINT16, &masterConfig.looptime, 0, 9000 },
{ "emf_avoidance", VAR_UINT8, &mcfg.emfAvoidance, 0, 1 }, { "emf_avoidance", VAR_UINT8, &masterConfig.emfAvoidance, 0, 1 },
{ "midrc", VAR_UINT16, &mcfg.rxConfig.midrc, 1200, 1700 }, { "midrc", VAR_UINT16, &masterConfig.rxConfig.midrc, 1200, 1700 },
{ "minthrottle", VAR_UINT16, &mcfg.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "minthrottle", VAR_UINT16, &masterConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
{ "maxthrottle", VAR_UINT16, &mcfg.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "maxthrottle", VAR_UINT16, &masterConfig.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
{ "mincommand", VAR_UINT16, &mcfg.mincommand, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "mincommand", VAR_UINT16, &masterConfig.mincommand, PWM_RANGE_ZERO, PWM_RANGE_MAX },
{ "mincheck", VAR_UINT16, &mcfg.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "mincheck", VAR_UINT16, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
{ "maxcheck", VAR_UINT16, &mcfg.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "maxcheck", VAR_UINT16, &masterConfig.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
{ "deadband3d_low", VAR_UINT16, &mcfg.deadband3d_low, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "deadband3d_low", VAR_UINT16, &masterConfig.deadband3d_low, PWM_RANGE_ZERO, PWM_RANGE_MAX },
{ "deadband3d_high", VAR_UINT16, &mcfg.deadband3d_high, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "deadband3d_high", VAR_UINT16, &masterConfig.deadband3d_high, PWM_RANGE_ZERO, PWM_RANGE_MAX },
{ "neutral3d", VAR_UINT16, &mcfg.neutral3d, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "neutral3d", VAR_UINT16, &masterConfig.neutral3d, PWM_RANGE_ZERO, PWM_RANGE_MAX },
{ "deadband3d_throttle", VAR_UINT16, &mcfg.deadband3d_throttle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "deadband3d_throttle", VAR_UINT16, &masterConfig.deadband3d_throttle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
{ "motor_pwm_rate", VAR_UINT16, &mcfg.motor_pwm_rate, 50, 32000 }, { "motor_pwm_rate", VAR_UINT16, &masterConfig.motor_pwm_rate, 50, 32000 },
{ "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 }, { "servo_pwm_rate", VAR_UINT16, &masterConfig.servo_pwm_rate, 50, 498 },
{ "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 }, { "retarded_arm", VAR_UINT8, &masterConfig.retarded_arm, 0, 1 },
{ "flaps_speed", VAR_UINT8, &mcfg.flaps_speed, 0, 100 }, { "flaps_speed", VAR_UINT8, &masterConfig.flaps_speed, 0, 100 },
{ "fixedwing_althold_dir", VAR_INT8, &mcfg.fixedwing_althold_dir, -1, 1 }, { "fixedwing_althold_dir", VAR_INT8, &masterConfig.fixedwing_althold_dir, -1, 1 },
{ "reboot_character", VAR_UINT8, &mcfg.serialConfig.reboot_character, 48, 126 }, { "reboot_character", VAR_UINT8, &masterConfig.serialConfig.reboot_character, 48, 126 },
{ "serial_baudrate", VAR_UINT32, &mcfg.serialConfig.port1_baudrate, 1200, 115200 }, { "serial_baudrate", VAR_UINT32, &masterConfig.serialConfig.port1_baudrate, 1200, 115200 },
{ "softserial_baudrate", VAR_UINT32, &mcfg.serialConfig.softserial_baudrate, 1200, 19200 }, { "softserial_baudrate", VAR_UINT32, &masterConfig.serialConfig.softserial_baudrate, 1200, 19200 },
{ "softserial_1_inverted", VAR_UINT8, &mcfg.serialConfig.softserial_1_inverted, 0, 1 }, { "softserial_1_inverted", VAR_UINT8, &masterConfig.serialConfig.softserial_1_inverted, 0, 1 },
{ "softserial_2_inverted", VAR_UINT8, &mcfg.serialConfig.softserial_2_inverted, 0, 1 }, { "softserial_2_inverted", VAR_UINT8, &masterConfig.serialConfig.softserial_2_inverted, 0, 1 },
{ "gps_type", VAR_UINT8, &mcfg.gps_type, 0, GPS_HARDWARE_MAX }, { "gps_type", VAR_UINT8, &masterConfig.gps_type, 0, GPS_HARDWARE_MAX },
{ "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, 0, GPS_BAUD_MAX }, { "gps_baudrate", VAR_INT8, &masterConfig.gps_baudrate, 0, GPS_BAUD_MAX },
{ "serialrx_type", VAR_UINT8, &mcfg.rxConfig.serialrx_type, 0, 3 }, { "serialrx_type", VAR_UINT8, &masterConfig.rxConfig.serialrx_type, 0, 3 },
{ "telemetry_provider", VAR_UINT8, &mcfg.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX }, { "telemetry_provider", VAR_UINT8, &masterConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX },
{ "telemetry_port", VAR_UINT8, &mcfg.telemetry_port, 0, TELEMETRY_PORT_MAX }, { "telemetry_port", VAR_UINT8, &masterConfig.telemetry_port, 0, TELEMETRY_PORT_MAX },
{ "telemetry_switch", VAR_UINT8, &mcfg.telemetry_switch, 0, 1 }, { "telemetry_switch", VAR_UINT8, &masterConfig.telemetry_switch, 0, 1 },
{ "vbatscale", VAR_UINT8, &mcfg.batteryConfig.vbatscale, 10, 200 }, { "vbatscale", VAR_UINT8, &masterConfig.batteryConfig.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &mcfg.batteryConfig.vbatmaxcellvoltage, 10, 50 }, { "vbatmaxcellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 },
{ "vbatmincellvoltage", VAR_UINT8, &mcfg.batteryConfig.vbatmincellvoltage, 10, 50 }, { "vbatmincellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 },
{ "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 }, { "power_adc_channel", VAR_UINT8, &masterConfig.power_adc_channel, 0, 9 },
{ "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 }, { "align_gyro", VAR_UINT8, &masterConfig.gyro_align, 0, 8 },
{ "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 }, { "align_acc", VAR_UINT8, &masterConfig.acc_align, 0, 8 },
{ "align_mag", VAR_UINT8, &mcfg.mag_align, 0, 8 }, { "align_mag", VAR_UINT8, &masterConfig.mag_align, 0, 8 },
{ "align_board_roll", VAR_INT16, &mcfg.boardAlignment.rollDegrees, -180, 360 }, { "align_board_roll", VAR_INT16, &masterConfig.boardAlignment.rollDegrees, -180, 360 },
{ "align_board_pitch", VAR_INT16, &mcfg.boardAlignment.pitchDegrees, -180, 360 }, { "align_board_pitch", VAR_INT16, &masterConfig.boardAlignment.pitchDegrees, -180, 360 },
{ "align_board_yaw", VAR_INT16, &mcfg.boardAlignment.yawDegrees, -180, 360 }, { "align_board_yaw", VAR_INT16, &masterConfig.boardAlignment.yawDegrees, -180, 360 },
{ "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 }, { "yaw_control_direction", VAR_INT8, &masterConfig.yaw_control_direction, -1, 1 },
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 }, { "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 },
{ "max_angle_inclination", VAR_UINT16, &mcfg.max_angle_inclination, 100, 900 }, { "max_angle_inclination", VAR_UINT16, &masterConfig.max_angle_inclination, 100, 900 },
{ "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 }, { "moron_threshold", VAR_UINT8, &masterConfig.moron_threshold, 0, 128 },
{ "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 }, { "gyro_lpf", VAR_UINT16, &masterConfig.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 }, { "gyro_cmpf_factor", VAR_UINT16, &masterConfig.gyro_cmpf_factor, 100, 1000 },
{ "gyro_cmpfm_factor", VAR_UINT16, &mcfg.gyro_cmpfm_factor, 100, 1000 }, { "gyro_cmpfm_factor", VAR_UINT16, &masterConfig.gyro_cmpfm_factor, 100, 1000 },
{ "pid_controller", VAR_UINT8, &cfg.pidController, 0, 1 }, { "pid_controller", VAR_UINT8, &currentProfile.pidController, 0, 1 },
{ "deadband", VAR_UINT8, &cfg.deadband, 0, 32 }, { "deadband", VAR_UINT8, &currentProfile.deadband, 0, 32 },
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 }, { "yawdeadband", VAR_UINT8, &currentProfile.yawdeadband, 0, 100 },
{ "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 }, { "alt_hold_throttle_neutral", VAR_UINT8, &currentProfile.alt_hold_throttle_neutral, 1, 250 },
{ "alt_hold_fast_change", VAR_UINT8, &cfg.alt_hold_fast_change, 0, 1 }, { "alt_hold_fast_change", VAR_UINT8, &currentProfile.alt_hold_fast_change, 0, 1 },
{ "throttle_correction_value", VAR_UINT8, &cfg.throttle_correction_value, 0, 150 }, { "throttle_correction_value", VAR_UINT8, &currentProfile.throttle_correction_value, 0, 150 },
{ "throttle_correction_angle", VAR_UINT16, &cfg.throttle_correction_angle, 1, 900 }, { "throttle_correction_angle", VAR_UINT16, &currentProfile.throttle_correction_angle, 1, 900 },
{ "rc_rate", VAR_UINT8, &cfg.controlRateConfig.rcRate8, 0, 250 }, { "rc_rate", VAR_UINT8, &currentProfile.controlRateConfig.rcRate8, 0, 250 },
{ "rc_expo", VAR_UINT8, &cfg.controlRateConfig.rcExpo8, 0, 100 }, { "rc_expo", VAR_UINT8, &currentProfile.controlRateConfig.rcExpo8, 0, 100 },
{ "thr_mid", VAR_UINT8, &cfg.controlRateConfig.thrMid8, 0, 100 }, { "thr_mid", VAR_UINT8, &currentProfile.controlRateConfig.thrMid8, 0, 100 },
{ "thr_expo", VAR_UINT8, &cfg.controlRateConfig.thrExpo8, 0, 100 }, { "thr_expo", VAR_UINT8, &currentProfile.controlRateConfig.thrExpo8, 0, 100 },
{ "roll_pitch_rate", VAR_UINT8, &cfg.controlRateConfig.rollPitchRate, 0, 100 }, { "roll_pitch_rate", VAR_UINT8, &currentProfile.controlRateConfig.rollPitchRate, 0, 100 },
{ "yawrate", VAR_UINT8, &cfg.controlRateConfig.yawRate, 0, 100 }, { "yawrate", VAR_UINT8, &currentProfile.controlRateConfig.yawRate, 0, 100 },
{ "tparate", VAR_UINT8, &cfg.dynThrPID, 0, 100}, { "tparate", VAR_UINT8, &currentProfile.dynThrPID, 0, 100},
{ "tpa_breakpoint", VAR_UINT16, &cfg.tpaBreakPoint, PWM_RANGE_MIN, PWM_RANGE_MAX}, { "tpa_breakpoint", VAR_UINT16, &currentProfile.tpaBreakPoint, PWM_RANGE_MIN, PWM_RANGE_MAX},
{ "failsafe_delay", VAR_UINT8, &cfg.failsafeConfig.failsafe_delay, 0, 200 }, { "failsafe_delay", VAR_UINT8, &currentProfile.failsafeConfig.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8, &cfg.failsafeConfig.failsafe_off_delay, 0, 200 }, { "failsafe_off_delay", VAR_UINT8, &currentProfile.failsafeConfig.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX }, { "failsafe_throttle", VAR_UINT16, &currentProfile.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
{ "failsafe_detect_threshold", VAR_UINT16, &cfg.failsafeConfig.failsafe_detect_threshold, 100, PWM_RANGE_MAX }, { "failsafe_detect_threshold", VAR_UINT16, &currentProfile.failsafeConfig.failsafe_detect_threshold, 100, PWM_RANGE_MAX },
{ "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 }, { "rssi_aux_channel", VAR_INT8, &masterConfig.rssi_aux_channel, 0, 4 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 }, { "yaw_direction", VAR_INT8, &currentProfile.yaw_direction, -1, 1 },
{ "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 }, { "tri_unarmed_servo", VAR_INT8, &currentProfile.tri_unarmed_servo, 0, 1 },
{ "gimbal_flags", VAR_UINT8, &cfg.gimbal_flags, 0, 255}, { "gimbal_flags", VAR_UINT8, &currentProfile.gimbal_flags, 0, 255},
{ "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 }, { "acc_lpf_factor", VAR_UINT8, &currentProfile.acc_lpf_factor, 0, 250 },
{ "accxy_deadband", VAR_UINT8, &cfg.accxy_deadband, 0, 100 }, { "accxy_deadband", VAR_UINT8, &currentProfile.accxy_deadband, 0, 100 },
{ "accz_deadband", VAR_UINT8, &cfg.accz_deadband, 0, 100 }, { "accz_deadband", VAR_UINT8, &currentProfile.accz_deadband, 0, 100 },
{ "acc_unarmedcal", VAR_UINT8, &cfg.acc_unarmedcal, 0, 1 }, { "acc_unarmedcal", VAR_UINT8, &currentProfile.acc_unarmedcal, 0, 1 },
{ "acc_trim_pitch", VAR_INT16, &cfg.angleTrim[PITCH], -300, 300 }, { "acc_trim_pitch", VAR_INT16, &currentProfile.angleTrim[PITCH], -300, 300 },
{ "acc_trim_roll", VAR_INT16, &cfg.angleTrim[ROLL], -300, 300 }, { "acc_trim_roll", VAR_INT16, &currentProfile.angleTrim[ROLL], -300, 300 },
{ "baro_tab_size", VAR_UINT8, &cfg.baro_tab_size, 0, BARO_TAB_SIZE_MAX }, { "baro_tab_size", VAR_UINT8, &currentProfile.baro_tab_size, 0, BARO_TAB_SIZE_MAX },
{ "baro_noise_lpf", VAR_FLOAT, &cfg.baro_noise_lpf, 0, 1 }, { "baro_noise_lpf", VAR_FLOAT, &currentProfile.baro_noise_lpf, 0, 1 },
{ "baro_cf_vel", VAR_FLOAT, &cfg.baro_cf_vel, 0, 1 }, { "baro_cf_vel", VAR_FLOAT, &currentProfile.baro_cf_vel, 0, 1 },
{ "baro_cf_alt", VAR_FLOAT, &cfg.baro_cf_alt, 0, 1 }, { "baro_cf_alt", VAR_FLOAT, &currentProfile.baro_cf_alt, 0, 1 },
{ "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 }, { "mag_declination", VAR_INT16, &currentProfile.mag_declination, -18000, 18000 },
{ "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 }, { "gps_pos_p", VAR_UINT8, &currentProfile.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 }, { "gps_pos_i", VAR_UINT8, &currentProfile.I8[PIDPOS], 0, 200 },
{ "gps_pos_d", VAR_UINT8, &cfg.D8[PIDPOS], 0, 200 }, { "gps_pos_d", VAR_UINT8, &currentProfile.D8[PIDPOS], 0, 200 },
{ "gps_posr_p", VAR_UINT8, &cfg.P8[PIDPOSR], 0, 200 }, { "gps_posr_p", VAR_UINT8, &currentProfile.P8[PIDPOSR], 0, 200 },
{ "gps_posr_i", VAR_UINT8, &cfg.I8[PIDPOSR], 0, 200 }, { "gps_posr_i", VAR_UINT8, &currentProfile.I8[PIDPOSR], 0, 200 },
{ "gps_posr_d", VAR_UINT8, &cfg.D8[PIDPOSR], 0, 200 }, { "gps_posr_d", VAR_UINT8, &currentProfile.D8[PIDPOSR], 0, 200 },
{ "gps_nav_p", VAR_UINT8, &cfg.P8[PIDNAVR], 0, 200 }, { "gps_nav_p", VAR_UINT8, &currentProfile.P8[PIDNAVR], 0, 200 },
{ "gps_nav_i", VAR_UINT8, &cfg.I8[PIDNAVR], 0, 200 }, { "gps_nav_i", VAR_UINT8, &currentProfile.I8[PIDNAVR], 0, 200 },
{ "gps_nav_d", VAR_UINT8, &cfg.D8[PIDNAVR], 0, 200 }, { "gps_nav_d", VAR_UINT8, &currentProfile.D8[PIDNAVR], 0, 200 },
{ "gps_wp_radius", VAR_UINT16, &cfg.gps_wp_radius, 0, 2000 }, { "gps_wp_radius", VAR_UINT16, &currentProfile.gps_wp_radius, 0, 2000 },
{ "nav_controls_heading", VAR_UINT8, &cfg.nav_controls_heading, 0, 1 }, { "nav_controls_heading", VAR_UINT8, &currentProfile.nav_controls_heading, 0, 1 },
{ "nav_speed_min", VAR_UINT16, &cfg.nav_speed_min, 10, 2000 }, { "nav_speed_min", VAR_UINT16, &currentProfile.nav_speed_min, 10, 2000 },
{ "nav_speed_max", VAR_UINT16, &cfg.nav_speed_max, 10, 2000 }, { "nav_speed_max", VAR_UINT16, &currentProfile.nav_speed_max, 10, 2000 },
{ "nav_slew_rate", VAR_UINT8, &cfg.nav_slew_rate, 0, 100 }, { "nav_slew_rate", VAR_UINT8, &currentProfile.nav_slew_rate, 0, 100 },
{ "p_pitch", VAR_UINT8, &cfg.P8[PITCH], 0, 200 }, { "p_pitch", VAR_UINT8, &currentProfile.P8[PITCH], 0, 200 },
{ "i_pitch", VAR_UINT8, &cfg.I8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8, &currentProfile.I8[PITCH], 0, 200 },
{ "d_pitch", VAR_UINT8, &cfg.D8[PITCH], 0, 200 }, { "d_pitch", VAR_UINT8, &currentProfile.D8[PITCH], 0, 200 },
{ "p_roll", VAR_UINT8, &cfg.P8[ROLL], 0, 200 }, { "p_roll", VAR_UINT8, &currentProfile.P8[ROLL], 0, 200 },
{ "i_roll", VAR_UINT8, &cfg.I8[ROLL], 0, 200 }, { "i_roll", VAR_UINT8, &currentProfile.I8[ROLL], 0, 200 },
{ "d_roll", VAR_UINT8, &cfg.D8[ROLL], 0, 200 }, { "d_roll", VAR_UINT8, &currentProfile.D8[ROLL], 0, 200 },
{ "p_yaw", VAR_UINT8, &cfg.P8[YAW], 0, 200 }, { "p_yaw", VAR_UINT8, &currentProfile.P8[YAW], 0, 200 },
{ "i_yaw", VAR_UINT8, &cfg.I8[YAW], 0, 200 }, { "i_yaw", VAR_UINT8, &currentProfile.I8[YAW], 0, 200 },
{ "d_yaw", VAR_UINT8, &cfg.D8[YAW], 0, 200 }, { "d_yaw", VAR_UINT8, &currentProfile.D8[YAW], 0, 200 },
{ "p_alt", VAR_UINT8, &cfg.P8[PIDALT], 0, 200 }, { "p_alt", VAR_UINT8, &currentProfile.P8[PIDALT], 0, 200 },
{ "i_alt", VAR_UINT8, &cfg.I8[PIDALT], 0, 200 }, { "i_alt", VAR_UINT8, &currentProfile.I8[PIDALT], 0, 200 },
{ "d_alt", VAR_UINT8, &cfg.D8[PIDALT], 0, 200 }, { "d_alt", VAR_UINT8, &currentProfile.D8[PIDALT], 0, 200 },
{ "p_level", VAR_UINT8, &cfg.P8[PIDLEVEL], 0, 200 }, { "p_level", VAR_UINT8, &currentProfile.P8[PIDLEVEL], 0, 200 },
{ "i_level", VAR_UINT8, &cfg.I8[PIDLEVEL], 0, 200 }, { "i_level", VAR_UINT8, &currentProfile.I8[PIDLEVEL], 0, 200 },
{ "d_level", VAR_UINT8, &cfg.D8[PIDLEVEL], 0, 200 }, { "d_level", VAR_UINT8, &currentProfile.D8[PIDLEVEL], 0, 200 },
{ "p_vel", VAR_UINT8, &cfg.P8[PIDVEL], 0, 200 }, { "p_vel", VAR_UINT8, &currentProfile.P8[PIDVEL], 0, 200 },
{ "i_vel", VAR_UINT8, &cfg.I8[PIDVEL], 0, 200 }, { "i_vel", VAR_UINT8, &currentProfile.I8[PIDVEL], 0, 200 },
{ "d_vel", VAR_UINT8, &cfg.D8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8, &currentProfile.D8[PIDVEL], 0, 200 },
}; };
#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
@ -447,14 +447,14 @@ static void cliAux(char *cmdline)
if (len == 0) { if (len == 0) {
// print out aux channel settings // print out aux channel settings
for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) for (i = 0; i < CHECKBOX_ITEM_COUNT; i++)
printf("aux %u %u\r\n", i, cfg.activate[i]); printf("aux %u %u\r\n", i, currentProfile.activate[i]);
} else { } else {
ptr = cmdline; ptr = cmdline;
i = atoi(ptr); i = atoi(ptr);
if (i < CHECKBOX_ITEM_COUNT) { if (i < CHECKBOX_ITEM_COUNT) {
ptr = strchr(cmdline, ' '); ptr = strchr(cmdline, ' ');
val = atoi(ptr); val = atoi(ptr);
cfg.activate[i] = val; currentProfile.activate[i] = val;
} else { } else {
printf("Invalid Feature index: must be < %u\r\n", CHECKBOX_ITEM_COUNT); printf("Invalid Feature index: must be < %u\r\n", CHECKBOX_ITEM_COUNT);
} }
@ -475,20 +475,20 @@ static void cliCMix(char *cmdline)
if (len == 0) { if (len == 0) {
cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n"); cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
for (i = 0; i < MAX_MOTORS; i++) { for (i = 0; i < MAX_MOTORS; i++) {
if (mcfg.customMixer[i].throttle == 0.0f) if (masterConfig.customMixer[i].throttle == 0.0f)
break; break;
num_motors++; num_motors++;
printf("#%d:\t", i + 1); printf("#%d:\t", i + 1);
printf("%s\t", ftoa(mcfg.customMixer[i].throttle, buf)); printf("%s\t", ftoa(masterConfig.customMixer[i].throttle, buf));
printf("%s\t", ftoa(mcfg.customMixer[i].roll, buf)); printf("%s\t", ftoa(masterConfig.customMixer[i].roll, buf));
printf("%s\t", ftoa(mcfg.customMixer[i].pitch, buf)); printf("%s\t", ftoa(masterConfig.customMixer[i].pitch, buf));
printf("%s\r\n", ftoa(mcfg.customMixer[i].yaw, buf)); printf("%s\r\n", ftoa(masterConfig.customMixer[i].yaw, buf));
} }
mixsum[0] = mixsum[1] = mixsum[2] = 0.0f; mixsum[0] = mixsum[1] = mixsum[2] = 0.0f;
for (i = 0; i < num_motors; i++) { for (i = 0; i < num_motors; i++) {
mixsum[0] += mcfg.customMixer[i].roll; mixsum[0] += masterConfig.customMixer[i].roll;
mixsum[1] += mcfg.customMixer[i].pitch; mixsum[1] += masterConfig.customMixer[i].pitch;
mixsum[2] += mcfg.customMixer[i].yaw; mixsum[2] += masterConfig.customMixer[i].yaw;
} }
cliPrint("Sanity check:\t"); cliPrint("Sanity check:\t");
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
@ -498,7 +498,7 @@ static void cliCMix(char *cmdline)
} else if (strncasecmp(cmdline, "reset", 5) == 0) { } else if (strncasecmp(cmdline, "reset", 5) == 0) {
// erase custom mixer // erase custom mixer
for (i = 0; i < MAX_MOTORS; i++) for (i = 0; i < MAX_MOTORS; i++)
mcfg.customMixer[i].throttle = 0.0f; masterConfig.customMixer[i].throttle = 0.0f;
} else if (strncasecmp(cmdline, "load", 4) == 0) { } else if (strncasecmp(cmdline, "load", 4) == 0) {
ptr = strchr(cmdline, ' '); ptr = strchr(cmdline, ' ');
if (ptr) { if (ptr) {
@ -522,22 +522,22 @@ static void cliCMix(char *cmdline)
if (--i < MAX_MOTORS) { if (--i < MAX_MOTORS) {
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
mcfg.customMixer[i].throttle = _atof(++ptr); masterConfig.customMixer[i].throttle = _atof(++ptr);
check++; check++;
} }
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
mcfg.customMixer[i].roll = _atof(++ptr); masterConfig.customMixer[i].roll = _atof(++ptr);
check++; check++;
} }
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
mcfg.customMixer[i].pitch = _atof(++ptr); masterConfig.customMixer[i].pitch = _atof(++ptr);
check++; check++;
} }
ptr = strchr(ptr, ' '); ptr = strchr(ptr, ' ');
if (ptr) { if (ptr) {
mcfg.customMixer[i].yaw = _atof(++ptr); masterConfig.customMixer[i].yaw = _atof(++ptr);
check++; check++;
} }
if (check != 4) { if (check != 4) {
@ -565,17 +565,17 @@ static void cliDump(char *cmdline)
cliAux(""); cliAux("");
// print out current motor mix // print out current motor mix
printf("mixer %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]); printf("mixer %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]);
// print custom mix if exists // print custom mix if exists
if (mcfg.customMixer[0].throttle != 0.0f) { if (masterConfig.customMixer[0].throttle != 0.0f) {
for (i = 0; i < MAX_MOTORS; i++) { for (i = 0; i < MAX_MOTORS; i++) {
if (mcfg.customMixer[i].throttle == 0.0f) if (masterConfig.customMixer[i].throttle == 0.0f)
break; break;
thr = mcfg.customMixer[i].throttle; thr = masterConfig.customMixer[i].throttle;
roll = mcfg.customMixer[i].roll; roll = masterConfig.customMixer[i].roll;
pitch = mcfg.customMixer[i].pitch; pitch = masterConfig.customMixer[i].pitch;
yaw = mcfg.customMixer[i].yaw; yaw = masterConfig.customMixer[i].yaw;
printf("cmix %d", i + 1); printf("cmix %d", i + 1);
if (thr < 0) if (thr < 0)
printf(" "); printf(" ");
@ -609,7 +609,7 @@ static void cliDump(char *cmdline)
// print RC MAPPING // print RC MAPPING
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
buf[mcfg.rxConfig.rcmap[i]] = rcChannelLetters[i]; buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i];
buf[i] = '\0'; buf[i] = '\0';
printf("map %s\r\n", buf); printf("map %s\r\n", buf);
@ -725,11 +725,11 @@ static void cliMap(char *cmdline)
cliPrint("Must be any order of AETR1234\r\n"); cliPrint("Must be any order of AETR1234\r\n");
return; return;
} }
parseRcChannels(cmdline, &mcfg.rxConfig); parseRcChannels(cmdline, &masterConfig.rxConfig);
} }
cliPrint("Current assignment: "); cliPrint("Current assignment: ");
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
out[mcfg.rxConfig.rcmap[i]] = rcChannelLetters[i]; out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i];
out[i] = '\0'; out[i] = '\0';
printf("%s\r\n", out); printf("%s\r\n", out);
} }
@ -742,7 +742,7 @@ static void cliMixer(char *cmdline)
len = strlen(cmdline); len = strlen(cmdline);
if (len == 0) { if (len == 0) {
printf("Current mixer: %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]); printf("Current mixer: %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]);
return; return;
} else if (strncasecmp(cmdline, "list", len) == 0) { } else if (strncasecmp(cmdline, "list", len) == 0) {
cliPrint("Available mixers: "); cliPrint("Available mixers: ");
@ -761,7 +761,7 @@ static void cliMixer(char *cmdline)
break; break;
} }
if (strncasecmp(cmdline, mixerNames[i], len) == 0) { if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
mcfg.mixerConfiguration = i + 1; masterConfig.mixerConfiguration = i + 1;
printf("Mixer set to %s\r\n", mixerNames[i]); printf("Mixer set to %s\r\n", mixerNames[i]);
break; break;
} }
@ -821,12 +821,12 @@ static void cliProfile(char *cmdline)
len = strlen(cmdline); len = strlen(cmdline);
if (len == 0) { if (len == 0) {
printf("Current profile: %d\r\n", mcfg.current_profile); printf("Current profile: %d\r\n", masterConfig.current_profile);
return; return;
} else { } else {
i = atoi(cmdline); i = atoi(cmdline);
if (i >= 0 && i <= 2) { if (i >= 0 && i <= 2) {
mcfg.current_profile = i; masterConfig.current_profile = i;
writeEEPROM(); writeEEPROM();
readEEPROM(); readEEPROM();
cliProfile(""); cliProfile("");
@ -843,7 +843,7 @@ static void cliReboot(void) {
static void cliSave(char *cmdline) static void cliSave(char *cmdline)
{ {
cliPrint("Saving..."); cliPrint("Saving...");
copyCurrentProfileToProfileSlot(mcfg.current_profile); copyCurrentProfileToProfileSlot(masterConfig.current_profile);
writeEEPROM(); writeEEPROM();
cliReboot(); cliReboot();
} }

124
src/serial_msp.c
View file

@ -270,13 +270,13 @@ void mspInit(void)
availableBoxes[idx++] = BOXGPSHOME; availableBoxes[idx++] = BOXGPSHOME;
availableBoxes[idx++] = BOXGPSHOLD; availableBoxes[idx++] = BOXGPSHOLD;
} }
if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE)
availableBoxes[idx++] = BOXPASSTHRU; availableBoxes[idx++] = BOXPASSTHRU;
availableBoxes[idx++] = BOXBEEPERON; availableBoxes[idx++] = BOXBEEPERON;
if (feature(FEATURE_INFLIGHT_ACC_CAL)) if (feature(FEATURE_INFLIGHT_ACC_CAL))
availableBoxes[idx++] = BOXCALIB; availableBoxes[idx++] = BOXCALIB;
availableBoxes[idx++] = BOXOSD; availableBoxes[idx++] = BOXOSD;
if (feature(FEATURE_TELEMETRY && mcfg.telemetry_switch)) if (feature(FEATURE_TELEMETRY && masterConfig.telemetry_switch))
availableBoxes[idx++] = BOXTELEMETRY; availableBoxes[idx++] = BOXTELEMETRY;
numberBoxItems = idx; numberBoxItems = idx;
} }
@ -294,8 +294,8 @@ static void evaluateCommand(void)
headSerialReply(0); headSerialReply(0);
break; break;
case MSP_SET_ACC_TRIM: case MSP_SET_ACC_TRIM:
cfg.angleTrim[PITCH] = read16(); currentProfile.angleTrim[PITCH] = read16();
cfg.angleTrim[ROLL] = read16(); currentProfile.angleTrim[ROLL] = read16();
headSerialReply(0); headSerialReply(0);
break; break;
case MSP_SET_RAW_GPS: case MSP_SET_RAW_GPS:
@ -310,39 +310,39 @@ static void evaluateCommand(void)
break; break;
case MSP_SET_PID: case MSP_SET_PID:
for (i = 0; i < PID_ITEM_COUNT; i++) { for (i = 0; i < PID_ITEM_COUNT; i++) {
cfg.P8[i] = read8(); currentProfile.P8[i] = read8();
cfg.I8[i] = read8(); currentProfile.I8[i] = read8();
cfg.D8[i] = read8(); currentProfile.D8[i] = read8();
} }
headSerialReply(0); headSerialReply(0);
break; break;
case MSP_SET_BOX: case MSP_SET_BOX:
for (i = 0; i < numberBoxItems; i++) for (i = 0; i < numberBoxItems; i++)
cfg.activate[availableBoxes[i]] = read16(); currentProfile.activate[availableBoxes[i]] = read16();
headSerialReply(0); headSerialReply(0);
break; break;
case MSP_SET_RC_TUNING: case MSP_SET_RC_TUNING:
cfg.controlRateConfig.rcRate8 = read8(); currentProfile.controlRateConfig.rcRate8 = read8();
cfg.controlRateConfig.rcExpo8 = read8(); currentProfile.controlRateConfig.rcExpo8 = read8();
cfg.controlRateConfig.rollPitchRate = read8(); currentProfile.controlRateConfig.rollPitchRate = read8();
cfg.controlRateConfig.yawRate = read8(); currentProfile.controlRateConfig.yawRate = read8();
cfg.dynThrPID = read8(); currentProfile.dynThrPID = read8();
cfg.controlRateConfig.thrMid8 = read8(); currentProfile.controlRateConfig.thrMid8 = read8();
cfg.controlRateConfig.thrExpo8 = read8(); currentProfile.controlRateConfig.thrExpo8 = read8();
headSerialReply(0); headSerialReply(0);
break; break;
case MSP_SET_MISC: case MSP_SET_MISC:
read16(); // powerfailmeter read16(); // powerfailmeter
mcfg.minthrottle = read16(); masterConfig.minthrottle = read16();
mcfg.maxthrottle = read16(); masterConfig.maxthrottle = read16();
mcfg.mincommand = read16(); masterConfig.mincommand = read16();
cfg.failsafeConfig.failsafe_throttle = read16(); currentProfile.failsafeConfig.failsafe_throttle = read16();
read16(); read16();
read32(); read32();
cfg.mag_declination = read16() * 10; currentProfile.mag_declination = read16() * 10;
mcfg.batteryConfig.vbatscale = read8(); // actual vbatscale as intended masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended
mcfg.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI
mcfg.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI
read8(); // vbatlevel_crit (unused) read8(); // vbatlevel_crit (unused)
headSerialReply(0); headSerialReply(0);
break; break;
@ -353,9 +353,9 @@ static void evaluateCommand(void)
break; break;
case MSP_SELECT_SETTING: case MSP_SELECT_SETTING:
if (!f.ARMED) { if (!f.ARMED) {
mcfg.current_profile = read8(); masterConfig.current_profile = read8();
if (mcfg.current_profile > 2) { if (masterConfig.current_profile > 2) {
mcfg.current_profile = 0; masterConfig.current_profile = 0;
} }
writeEEPROM(); writeEEPROM();
readEEPROM(); readEEPROM();
@ -369,9 +369,9 @@ static void evaluateCommand(void)
case MSP_IDENT: case MSP_IDENT:
headSerialReply(7); headSerialReply(7);
serialize8(VERSION); // multiwii version serialize8(VERSION); // multiwii version
serialize8(mcfg.mixerConfiguration); // type of multicopter serialize8(masterConfig.mixerConfiguration); // type of multicopter
serialize8(MSP_VERSION); // MultiWii Serial Protocol Version serialize8(MSP_VERSION); // MultiWii Serial Protocol Version
serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0) | CAP_CHANNEL_FORWARDING); // "capability" serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (masterConfig.flaps_speed ? CAP_FLAPS : 0) | CAP_CHANNEL_FORWARDING); // "capability"
break; break;
case MSP_STATUS: case MSP_STATUS:
headSerialReply(11); headSerialReply(11);
@ -402,7 +402,7 @@ static void evaluateCommand(void)
junk |= 1 << i; junk |= 1 << i;
} }
serialize32(junk); serialize32(junk);
serialize8(mcfg.current_profile); serialize8(masterConfig.current_profile);
break; break;
case MSP_RAW_IMU: case MSP_RAW_IMU:
headSerialReply(18); headSerialReply(18);
@ -425,38 +425,38 @@ static void evaluateCommand(void)
case MSP_SERVO_CONF: case MSP_SERVO_CONF:
headSerialReply(56); headSerialReply(56);
for (i = 0; i < MAX_SERVOS; i++) { for (i = 0; i < MAX_SERVOS; i++) {
serialize16(cfg.servoConf[i].min); serialize16(currentProfile.servoConf[i].min);
serialize16(cfg.servoConf[i].max); serialize16(currentProfile.servoConf[i].max);
serialize16(cfg.servoConf[i].middle); serialize16(currentProfile.servoConf[i].middle);
serialize8(cfg.servoConf[i].rate); serialize8(currentProfile.servoConf[i].rate);
} }
break; break;
case MSP_SET_SERVO_CONF: case MSP_SET_SERVO_CONF:
headSerialReply(0); headSerialReply(0);
for (i = 0; i < MAX_SERVOS; i++) { for (i = 0; i < MAX_SERVOS; i++) {
cfg.servoConf[i].min = read16(); currentProfile.servoConf[i].min = read16();
cfg.servoConf[i].max = read16(); currentProfile.servoConf[i].max = read16();
// provide temporary support for old clients that try and send a channel index instead of a servo middle // provide temporary support for old clients that try and send a channel index instead of a servo middle
uint16_t potentialServoMiddleOrChannelToForward = read16(); uint16_t potentialServoMiddleOrChannelToForward = read16();
if (potentialServoMiddleOrChannelToForward < MAX_SERVOS) { if (potentialServoMiddleOrChannelToForward < MAX_SERVOS) {
cfg.servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward; currentProfile.servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward;
} }
if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) { if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) {
cfg.servoConf[i].middle = potentialServoMiddleOrChannelToForward; currentProfile.servoConf[i].middle = potentialServoMiddleOrChannelToForward;
} }
cfg.servoConf[i].rate = read8(); currentProfile.servoConf[i].rate = read8();
} }
break; break;
case MSP_CHANNEL_FORWARDING: case MSP_CHANNEL_FORWARDING:
headSerialReply(8); headSerialReply(8);
for (i = 0; i < MAX_SERVOS; i++) { for (i = 0; i < MAX_SERVOS; i++) {
serialize8(cfg.servoConf[i].forwardFromChannel); serialize8(currentProfile.servoConf[i].forwardFromChannel);
} }
break; break;
case MSP_SET_CHANNEL_FORWARDING: case MSP_SET_CHANNEL_FORWARDING:
headSerialReply(0); headSerialReply(0);
for (i = 0; i < MAX_SERVOS; i++) { for (i = 0; i < MAX_SERVOS; i++) {
cfg.servoConf[i].forwardFromChannel = read8(); currentProfile.servoConf[i].forwardFromChannel = read8();
} }
break; break;
case MSP_MOTOR: case MSP_MOTOR:
@ -503,20 +503,20 @@ static void evaluateCommand(void)
break; break;
case MSP_RC_TUNING: case MSP_RC_TUNING:
headSerialReply(7); headSerialReply(7);
serialize8(cfg.controlRateConfig.rcRate8); serialize8(currentProfile.controlRateConfig.rcRate8);
serialize8(cfg.controlRateConfig.rcExpo8); serialize8(currentProfile.controlRateConfig.rcExpo8);
serialize8(cfg.controlRateConfig.rollPitchRate); serialize8(currentProfile.controlRateConfig.rollPitchRate);
serialize8(cfg.controlRateConfig.yawRate); serialize8(currentProfile.controlRateConfig.yawRate);
serialize8(cfg.dynThrPID); serialize8(currentProfile.dynThrPID);
serialize8(cfg.controlRateConfig.thrMid8); serialize8(currentProfile.controlRateConfig.thrMid8);
serialize8(cfg.controlRateConfig.thrExpo8); serialize8(currentProfile.controlRateConfig.thrExpo8);
break; break;
case MSP_PID: case MSP_PID:
headSerialReply(3 * PID_ITEM_COUNT); headSerialReply(3 * PID_ITEM_COUNT);
for (i = 0; i < PID_ITEM_COUNT; i++) { for (i = 0; i < PID_ITEM_COUNT; i++) {
serialize8(cfg.P8[i]); serialize8(currentProfile.P8[i]);
serialize8(cfg.I8[i]); serialize8(currentProfile.I8[i]);
serialize8(cfg.D8[i]); serialize8(currentProfile.D8[i]);
} }
break; break;
case MSP_PIDNAMES: case MSP_PIDNAMES:
@ -526,7 +526,7 @@ static void evaluateCommand(void)
case MSP_BOX: case MSP_BOX:
headSerialReply(2 * numberBoxItems); headSerialReply(2 * numberBoxItems);
for (i = 0; i < numberBoxItems; i++) for (i = 0; i < numberBoxItems; i++)
serialize16(cfg.activate[availableBoxes[i]]); serialize16(currentProfile.activate[availableBoxes[i]]);
break; break;
case MSP_BOXNAMES: case MSP_BOXNAMES:
// headSerialReply(sizeof(boxnames) - 1); // headSerialReply(sizeof(boxnames) - 1);
@ -540,16 +540,16 @@ static void evaluateCommand(void)
case MSP_MISC: case MSP_MISC:
headSerialReply(2 * 6 + 4 + 2 + 4); headSerialReply(2 * 6 + 4 + 2 + 4);
serialize16(0); // intPowerTrigger1 (aka useless trash) serialize16(0); // intPowerTrigger1 (aka useless trash)
serialize16(mcfg.minthrottle); serialize16(masterConfig.minthrottle);
serialize16(mcfg.maxthrottle); serialize16(masterConfig.maxthrottle);
serialize16(mcfg.mincommand); serialize16(masterConfig.mincommand);
serialize16(cfg.failsafeConfig.failsafe_throttle); serialize16(currentProfile.failsafeConfig.failsafe_throttle);
serialize16(0); // plog useless shit serialize16(0); // plog useless shit
serialize32(0); // plog useless shit serialize32(0); // plog useless shit
serialize16(cfg.mag_declination / 10); // TODO check this shit serialize16(currentProfile.mag_declination / 10); // TODO check this shit
serialize8(mcfg.batteryConfig.vbatscale); serialize8(masterConfig.batteryConfig.vbatscale);
serialize8(mcfg.batteryConfig.vbatmincellvoltage); serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
serialize8(mcfg.batteryConfig.vbatmaxcellvoltage); serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
serialize8(0); serialize8(0);
break; break;
case MSP_MOTOR_PINS: case MSP_MOTOR_PINS:
@ -621,7 +621,7 @@ static void evaluateCommand(void)
if (f.ARMED) { if (f.ARMED) {
headSerialError(0); headSerialError(0);
} else { } else {
copyCurrentProfileToProfileSlot(mcfg.current_profile); copyCurrentProfileToProfileSlot(masterConfig.current_profile);
writeEEPROM(); writeEEPROM();
readEEPROM(); readEEPROM();
headSerialReply(0); headSerialReply(0);
@ -638,8 +638,8 @@ static void evaluateCommand(void)
// Additional commands that are not compatible with MultiWii // Additional commands that are not compatible with MultiWii
case MSP_ACC_TRIM: case MSP_ACC_TRIM:
headSerialReply(4); headSerialReply(4);
serialize16(cfg.angleTrim[PITCH]); serialize16(currentProfile.angleTrim[PITCH]);
serialize16(cfg.angleTrim[ROLL]); serialize16(currentProfile.angleTrim[ROLL]);
break; break;
case MSP_UID: case MSP_UID:
headSerialReply(12); headSerialReply(12);

20
src/telemetry_common.c
View file

@ -13,12 +13,12 @@ static bool isTelemetryConfigurationValid = false; // flag used to avoid repeate
bool isTelemetryProviderFrSky(void) bool isTelemetryProviderFrSky(void)
{ {
return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY; return masterConfig.telemetry_provider == TELEMETRY_PROVIDER_FRSKY;
} }
bool isTelemetryProviderHoTT(void) bool isTelemetryProviderHoTT(void)
{ {
return mcfg.telemetry_provider == TELEMETRY_PROVIDER_HOTT; return masterConfig.telemetry_provider == TELEMETRY_PROVIDER_HOTT;
} }
bool canUseTelemetryWithCurrentConfiguration(void) bool canUseTelemetryWithCurrentConfiguration(void)
@ -28,14 +28,14 @@ bool canUseTelemetryWithCurrentConfiguration(void)
} }
if (!feature(FEATURE_SOFTSERIAL)) { if (!feature(FEATURE_SOFTSERIAL)) {
if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) { if (masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) {
// softserial feature must be enabled to use telemetry on softserial ports // softserial feature must be enabled to use telemetry on softserial ports
return false; return false;
} }
} }
if (isTelemetryProviderHoTT()) { if (isTelemetryProviderHoTT()) {
if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) {
// HoTT requires a serial port that supports RX/TX mode swapping // HoTT requires a serial port that supports RX/TX mode swapping
return false; return false;
} }
@ -48,20 +48,20 @@ void initTelemetry(serialPorts_t *serialPorts)
{ {
// Force telemetry to uart when softserial disabled // Force telemetry to uart when softserial disabled
if (!feature(FEATURE_SOFTSERIAL)) if (!feature(FEATURE_SOFTSERIAL))
mcfg.telemetry_port = TELEMETRY_PORT_UART; masterConfig.telemetry_port = TELEMETRY_PORT_UART;
#ifdef FY90Q #ifdef FY90Q
// FY90Q does not support softserial // FY90Q does not support softserial
mcfg.telemetry_port = TELEMETRY_PORT_UART; masterConfig.telemetry_port = TELEMETRY_PORT_UART;
serialPorts->telemport = serialPorts->mainport; serialPorts->telemport = serialPorts->mainport;
#endif #endif
isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration(); isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration();
#ifndef FY90Q #ifndef FY90Q
if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1) if (masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1)
serialPorts->telemport = &(softSerialPorts[0].port); serialPorts->telemport = &(softSerialPorts[0].port);
else if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) else if (masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2)
serialPorts->telemport = &(softSerialPorts[1].port); serialPorts->telemport = &(softSerialPorts[1].port);
else else
serialPorts->telemport = serialPorts->mainport; serialPorts->telemport = serialPorts->mainport;
@ -76,8 +76,8 @@ bool determineNewTelemetryEnabledState(void)
{ {
bool enabled = true; bool enabled = true;
if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) {
if (!mcfg.telemetry_switch) if (!masterConfig.telemetry_switch)
enabled = f.ARMED; enabled = f.ARMED;
else else
enabled = rcOptions[BOXTELEMETRY]; enabled = rcOptions[BOXTELEMETRY];

4
src/telemetry_frsky.c
View file

@ -224,14 +224,14 @@ static void sendHeading(void)
void freeFrSkyTelemetryPort(void) void freeFrSkyTelemetryPort(void)
{ {
if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) {
resetMainSerialPort(); resetMainSerialPort();
} }
} }
void configureFrSkyTelemetryPort(void) void configureFrSkyTelemetryPort(void)
{ {
if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) {
openMainSerialPort(9600); openMainSerialPort(9600);
} }
} }