rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Instead of copying a profile from the master config into memory again, 

just use it in-place.  This saves ~308bytes of memory.

Prior to this there were 4 profiles in ram all the time, the 3 main
profiles and a copy of one of them.

This commit was aided by a side effect of the work done to clean up the
output of the cli dump command since it is now easy to conditionally
apply the changes to the memory addressed used to read/write cli
variables.  See 8c3a869251.
Conflicts:

	src/main/io/serial_msp.c
This commit is contained in:
Dominic Clifton 2014-08-22 21:53:23 +01:00
parent bc34fc3744
commit f51efaa7c0
10 changed files with 262 additions and 262 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
src/main/config/config.c
View File

@ -84,7 +84,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
static uint32_t flashWriteAddress = (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG));
master_t masterConfig; // master config struct with data independent from profiles
profile_t currentProfile; // profile config struct
profile_t *currentProfile; // profile config struct
static const uint8_t EEPROM_CONF_VERSION = 75;
@ -290,66 +290,66 @@ static void resetConf(void)
masterConfig.looptime = 3500;
masterConfig.emf_avoidance = 0;
currentProfile.pidController = 0;
resetPidProfile(&currentProfile.pidProfile);
currentProfile->pidController = 0;
resetPidProfile(&currentProfile->pidProfile);
currentProfile.controlRateConfig.rcRate8 = 90;
currentProfile.controlRateConfig.rcExpo8 = 65;
currentProfile.controlRateConfig.rollPitchRate = 0;
currentProfile.controlRateConfig.yawRate = 0;
currentProfile.dynThrPID = 0;
currentProfile.tpa_breakpoint = 1500;
currentProfile.controlRateConfig.thrMid8 = 50;
currentProfile.controlRateConfig.thrExpo8 = 0;
currentProfile->controlRateConfig.rcRate8 = 90;
currentProfile->controlRateConfig.rcExpo8 = 65;
currentProfile->controlRateConfig.rollPitchRate = 0;
currentProfile->controlRateConfig.yawRate = 0;
currentProfile->dynThrPID = 0;
currentProfile->tpa_breakpoint = 1500;
currentProfile->controlRateConfig.thrMid8 = 50;
currentProfile->controlRateConfig.thrExpo8 = 0;
// for (i = 0; i < CHECKBOXITEMS; i++)
// cfg.activate[i] = 0;
resetRollAndPitchTrims(&currentProfile.accelerometerTrims);
resetRollAndPitchTrims(&currentProfile->accelerometerTrims);
currentProfile.mag_declination = 0;
currentProfile.acc_lpf_factor = 4;
currentProfile.accz_lpf_cutoff = 5.0f;
currentProfile.accDeadband.xy = 40;
currentProfile.accDeadband.z = 40;
currentProfile->mag_declination = 0;
currentProfile->acc_lpf_factor = 4;
currentProfile->accz_lpf_cutoff = 5.0f;
currentProfile->accDeadband.xy = 40;
currentProfile->accDeadband.z = 40;
resetBarometerConfig(&currentProfile.barometerConfig);
resetBarometerConfig(&currentProfile->barometerConfig);
currentProfile.acc_unarmedcal = 1;
currentProfile->acc_unarmedcal = 1;
// Radio
parseRcChannels("AETR1234", &masterConfig.rxConfig);
currentProfile.deadband = 0;
currentProfile.yaw_deadband = 0;
currentProfile.alt_hold_deadband = 40;
currentProfile.alt_hold_fast_change = 1;
currentProfile.throttle_correction_value = 0; // could 10 with althold or 40 for fpv
currentProfile.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
currentProfile->deadband = 0;
currentProfile->yaw_deadband = 0;
currentProfile->alt_hold_deadband = 40;
currentProfile->alt_hold_fast_change = 1;
currentProfile->throttle_correction_value = 0; // could 10 with althold or 40 for fpv
currentProfile->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
// Failsafe Variables
currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec
currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec
currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
currentProfile.failsafeConfig.failsafe_min_usec = 985; // any of first 4 channels below this value will trigger failsafe
currentProfile.failsafeConfig.failsafe_max_usec = 2115; // any of first 4 channels above this value will trigger failsafe
currentProfile->failsafeConfig.failsafe_delay = 10; // 1sec
currentProfile->failsafeConfig.failsafe_off_delay = 200; // 20sec
currentProfile->failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
currentProfile->failsafeConfig.failsafe_min_usec = 985; // any of first 4 channels below this value will trigger failsafe
currentProfile->failsafeConfig.failsafe_max_usec = 2115; // any of first 4 channels above this value will trigger failsafe
// servos
for (i = 0; i < 8; i++) {
currentProfile.servoConf[i].min = DEFAULT_SERVO_MIN;
currentProfile.servoConf[i].max = DEFAULT_SERVO_MAX;
currentProfile.servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
currentProfile.servoConf[i].rate = servoRates[i];
currentProfile.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN;
currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX;
currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
currentProfile->servoConf[i].rate = servoRates[i];
currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
}
currentProfile.mixerConfig.yaw_direction = 1;
currentProfile.mixerConfig.tri_unarmed_servo = 1;
currentProfile->mixerConfig.yaw_direction = 1;
currentProfile->mixerConfig.tri_unarmed_servo = 1;
// gimbal
currentProfile.gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
#ifdef GPS
resetGpsProfile(&currentProfile.gpsProfile);
resetGpsProfile(&currentProfile->gpsProfile);
#endif
// custom mixer. clear by defaults.
@ -397,43 +397,43 @@ void activateConfig(void)
{
static imuRuntimeConfig_t imuRuntimeConfig;
generatePitchCurve(&currentProfile.controlRateConfig);
generateThrottleCurve(&currentProfile.controlRateConfig, &masterConfig.escAndServoConfig);
generatePitchCurve(&currentProfile->controlRateConfig);
generateThrottleCurve(&currentProfile->controlRateConfig, &masterConfig.escAndServoConfig);
useGyroConfig(&masterConfig.gyroConfig);
#ifdef TELEMETRY
useTelemetryConfig(&masterConfig.telemetryConfig);
#endif
setPIDController(currentProfile.pidController);
setPIDController(currentProfile->pidController);
#ifdef GPS
gpsUseProfile(&currentProfile.gpsProfile);
gpsUsePIDs(&currentProfile.pidProfile);
gpsUseProfile(&currentProfile->gpsProfile);
gpsUsePIDs(&currentProfile->pidProfile);
#endif
useFailsafeConfig(&currentProfile.failsafeConfig);
useFailsafeConfig(&currentProfile->failsafeConfig);
setAccelerationTrims(&masterConfig.accZero);
mixerUseConfigs(
currentProfile.servoConf,
currentProfile->servoConf,
&masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig,
&currentProfile.mixerConfig,
&currentProfile->mixerConfig,
&masterConfig.airplaneConfig,
&masterConfig.rxConfig,
&currentProfile.gimbalConfig
&currentProfile->gimbalConfig
);
imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;
imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
imuRuntimeConfig.acc_lpf_factor = currentProfile.acc_lpf_factor;
imuRuntimeConfig.acc_unarmedcal = currentProfile.acc_unarmedcal;;
imuRuntimeConfig.acc_lpf_factor = currentProfile->acc_lpf_factor;
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
imuRuntimeConfig.small_angle = masterConfig.small_angle;
configureImu(&imuRuntimeConfig, &currentProfile.pidProfile, &currentProfile.barometerConfig, &currentProfile.accDeadband);
configureImu(&imuRuntimeConfig, &currentProfile->pidProfile, &currentProfile->barometerConfig, &currentProfile->accDeadband);
calculateThrottleAngleScale(currentProfile.throttle_correction_angle);
calculateAccZLowPassFilterRCTimeConstant(currentProfile.accz_lpf_cutoff);
calculateThrottleAngleScale(currentProfile->throttle_correction_angle);
calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff);
#ifdef BARO
useBarometerConfig(&currentProfile.barometerConfig);
useBarometerConfig(&currentProfile->barometerConfig);
#endif
}
@ -529,7 +529,8 @@ void readEEPROM(void)
// Copy current profile
if (masterConfig.current_profile_index > 2) // sanity check
masterConfig.current_profile_index = 0;
memcpy(&currentProfile, &masterConfig.profile[masterConfig.current_profile_index], sizeof(profile_t));
currentProfile = &masterConfig.profile[masterConfig.current_profile_index];
validateAndFixConfig();
activateConfig();
@ -542,13 +543,6 @@ void readEEPROMAndNotify(void)
blinkLedAndSoundBeeper(15, 20, 1);
}
void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex)
{
// copy current in-memory profile to stored configuration
memcpy(&masterConfig.profile[profileSlotIndex], &currentProfile, sizeof(profile_t));
}
void writeEEPROM(void)
{
// Generate compile time error if the config does not fit in the reserved area of flash.
@ -632,9 +626,8 @@ void resetEEPROM(void)
writeEEPROM();
}
void saveAndReloadCurrentProfileToCurrentProfileSlot(void)
void saveConfigAndNotify(void)
{
copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
writeEEPROM();
readEEPROMAndNotify();
}

2
src/main/config/config.h
View File

@ -51,7 +51,7 @@ void readEEPROM(void);
void readEEPROMAndNotify(void);
void writeEEPROM();
void ensureEEPROMContainsValidData(void);
void saveAndReloadCurrentProfileToCurrentProfileSlot(void);
void saveConfigAndNotify(void);
void changeProfile(uint8_t profileIndex);
bool canSoftwareSerialBeUsed(void);

2
src/main/config/config_master.h
View File

@ -81,4 +81,4 @@ typedef struct master_t {
} master_t;
extern master_t masterConfig;
extern profile_t currentProfile;
extern profile_t *currentProfile;

8
src/main/flight/altitudehold.c
View File

@ -65,12 +65,12 @@ static void multirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
// multirotor alt hold
if (currentProfile.alt_hold_fast_change) {
if (currentProfile->alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile->alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_deadband : currentProfile.alt_hold_deadband;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile->alt_hold_deadband : currentProfile->alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
@ -80,7 +80,7 @@ static void multirotorAltHold(void)
}
} else {
// slow alt changes for apfags
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile->alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;

192
src/main/io/serial_cli.c
View File

@ -228,20 +228,20 @@ const clivalue_t valueTable[] = {
{ "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.sbasMode, 0, SBAS_MODE_MAX },
{ "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.I8[PIDPOS], 0, 200 },
{ "gps_pos_d", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.D8[PIDPOS], 0, 200 },
{ "gps_posr_p", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.P8[PIDPOSR], 0, 200 },
{ "gps_posr_i", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.I8[PIDPOSR], 0, 200 },
{ "gps_posr_d", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.D8[PIDPOSR], 0, 200 },
{ "gps_nav_p", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.P8[PIDNAVR], 0, 200 },
{ "gps_nav_i", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.I8[PIDNAVR], 0, 200 },
{ "gps_nav_d", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.D8[PIDNAVR], 0, 200 },
{ "gps_wp_radius", VAR_UINT16 | PROFILE_VALUE, &currentProfile.gpsProfile.gps_wp_radius, 0, 2000 },
{ "nav_controls_heading", VAR_UINT8 | PROFILE_VALUE, &currentProfile.gpsProfile.nav_controls_heading, 0, 1 },
{ "nav_speed_min", VAR_UINT16 | PROFILE_VALUE, &currentProfile.gpsProfile.nav_speed_min, 10, 2000 },
{ "nav_speed_max", VAR_UINT16 | PROFILE_VALUE, &currentProfile.gpsProfile.nav_speed_max, 10, 2000 },
{ "nav_slew_rate", VAR_UINT8 | PROFILE_VALUE, &currentProfile.gpsProfile.nav_slew_rate, 0, 100 },
{ "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDPOS], 0, 200 },
{ "gps_pos_d", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDPOS], 0, 200 },
{ "gps_posr_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOSR], 0, 200 },
{ "gps_posr_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDPOSR], 0, 200 },
{ "gps_posr_d", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDPOSR], 0, 200 },
{ "gps_nav_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDNAVR], 0, 200 },
{ "gps_nav_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDNAVR], 0, 200 },
{ "gps_nav_d", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDNAVR], 0, 200 },
{ "gps_wp_radius", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.gps_wp_radius, 0, 2000 },
{ "nav_controls_heading", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.nav_controls_heading, 0, 1 },
{ "nav_speed_min", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.nav_speed_min, 10, 2000 },
{ "nav_speed_max", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.nav_speed_max, 10, 2000 },
{ "nav_slew_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.nav_slew_rate, 0, 100 },
#endif
{ "serialrx_provider", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.serialrx_provider, 0, SERIALRX_PROVIDER_MAX },
@ -273,88 +273,88 @@ const clivalue_t valueTable[] = {
{ "gyro_cmpf_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpf_factor, 100, 1000 },
{ "gyro_cmpfm_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpfm_factor, 100, 1000 },
{ "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &currentProfile.alt_hold_deadband, 1, 250 },
{ "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE, &currentProfile.alt_hold_fast_change, 0, 1 },
{ "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].alt_hold_deadband, 1, 250 },
{ "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].alt_hold_fast_change, 0, 1 },
{ "throttle_correction_value", VAR_UINT8 | PROFILE_VALUE, &currentProfile.throttle_correction_value, 0, 150 },
{ "throttle_correction_angle", VAR_UINT16 | PROFILE_VALUE, &currentProfile.throttle_correction_angle, 1, 900 },
{ "throttle_correction_value", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].throttle_correction_value, 0, 150 },
{ "throttle_correction_angle", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].throttle_correction_angle, 1, 900 },
{ "deadband", VAR_UINT8 | PROFILE_VALUE, &currentProfile.deadband, 0, 32 },
{ "yaw_deadband", VAR_UINT8 | PROFILE_VALUE, &currentProfile.yaw_deadband, 0, 100 },
{ "deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].deadband, 0, 32 },
{ "yaw_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].yaw_deadband, 0, 100 },
{ "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 },
{ "yaw_direction", VAR_INT8 | PROFILE_VALUE, &currentProfile.mixerConfig.yaw_direction, -1, 1 },
{ "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &currentProfile.mixerConfig.tri_unarmed_servo, 0, 1 },
{ "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 },
{ "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 },
{ "rc_rate", VAR_UINT8 | PROFILE_VALUE, &currentProfile.controlRateConfig.rcRate8, 0, 250 },
{ "rc_expo", VAR_UINT8 | PROFILE_VALUE, &currentProfile.controlRateConfig.rcExpo8, 0, 100 },
{ "thr_mid", VAR_UINT8 | PROFILE_VALUE, &currentProfile.controlRateConfig.thrMid8, 0, 100 },
{ "thr_expo", VAR_UINT8 | PROFILE_VALUE, &currentProfile.controlRateConfig.thrExpo8, 0, 100 },
{ "roll_pitch_rate", VAR_UINT8 | PROFILE_VALUE, &currentProfile.controlRateConfig.rollPitchRate, 0, 100 },
{ "yaw_rate", VAR_UINT8 | PROFILE_VALUE, &currentProfile.controlRateConfig.yawRate, 0, 100 },
{ "tpa_rate", VAR_UINT8 | PROFILE_VALUE, &currentProfile.dynThrPID, 0, 100},
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_VALUE, &currentProfile.tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX},
{ "rc_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.rcRate8, 0, 250 },
{ "rc_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.rcExpo8, 0, 100 },
{ "thr_mid", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.thrMid8, 0, 100 },
{ "thr_expo", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.thrExpo8, 0, 100 },
{ "roll_pitch_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.rollPitchRate, 0, 100 },
{ "yaw_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateConfig.yawRate, 0, 100 },
{ "tpa_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].dynThrPID, 0, 100},
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX},
{ "failsafe_delay", VAR_UINT8 | PROFILE_VALUE, &currentProfile.failsafeConfig.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8 | PROFILE_VALUE, &currentProfile.failsafeConfig.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16 | PROFILE_VALUE, &currentProfile.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
{ "failsafe_min_usec", VAR_UINT16 | PROFILE_VALUE, &currentProfile.failsafeConfig.failsafe_min_usec, 100, PWM_RANGE_MAX },
{ "failsafe_max_usec", VAR_UINT16 | PROFILE_VALUE, &currentProfile.failsafeConfig.failsafe_max_usec, 100, PWM_RANGE_MAX + (PWM_RANGE_MAX - PWM_RANGE_MIN) },
{ "failsafe_delay", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
{ "failsafe_min_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_min_usec, 100, PWM_RANGE_MAX },
{ "failsafe_max_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_max_usec, 100, PWM_RANGE_MAX + (PWM_RANGE_MAX - PWM_RANGE_MIN) },
{ "gimbal_flags", VAR_UINT8 | PROFILE_VALUE, &currentProfile.gimbalConfig.gimbal_flags, 0, 255},
{ "gimbal_flags", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, 5 },
{ "acc_lpf_factor", VAR_UINT8 | PROFILE_VALUE, &currentProfile.acc_lpf_factor, 0, 250 },
{ "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &currentProfile.accDeadband.xy, 0, 100 },
{ "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &currentProfile.accDeadband.z, 0, 100 },
{ "accz_lpf_cutoff", VAR_FLOAT | PROFILE_VALUE, &currentProfile.accz_lpf_cutoff, 1, 20 },
{ "acc_unarmedcal", VAR_UINT8 | PROFILE_VALUE, &currentProfile.acc_unarmedcal, 0, 1 },
{ "acc_trim_pitch", VAR_INT16 | PROFILE_VALUE, &currentProfile.accelerometerTrims.values.pitch, -300, 300 },
{ "acc_trim_roll", VAR_INT16 | PROFILE_VALUE, &currentProfile.accelerometerTrims.values.roll, -300, 300 },
{ "acc_lpf_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 },
{ "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, 0, 100 },
{ "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, 0, 100 },
{ "accz_lpf_cutoff", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].accz_lpf_cutoff, 1, 20 },
{ "acc_unarmedcal", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_unarmedcal, 0, 1 },
{ "acc_trim_pitch", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].accelerometerTrims.values.pitch, -300, 300 },
{ "acc_trim_roll", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].accelerometerTrims.values.roll, -300, 300 },
{ "baro_tab_size", VAR_UINT8 | PROFILE_VALUE, &currentProfile.barometerConfig.baro_sample_count, 0, BARO_SAMPLE_COUNT_MAX },
{ "baro_noise_lpf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.barometerConfig.baro_noise_lpf, 0, 1 },
{ "baro_cf_vel", VAR_FLOAT | PROFILE_VALUE, &currentProfile.barometerConfig.baro_cf_vel, 0, 1 },
{ "baro_cf_alt", VAR_FLOAT | PROFILE_VALUE, &currentProfile.barometerConfig.baro_cf_alt, 0, 1 },
{ "baro_tab_size", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_sample_count, 0, BARO_SAMPLE_COUNT_MAX },
{ "baro_noise_lpf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_noise_lpf, 0, 1 },
{ "baro_cf_vel", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_vel, 0, 1 },
{ "baro_cf_alt", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_alt, 0, 1 },
{ "mag_declination", VAR_INT16 | PROFILE_VALUE, &currentProfile.mag_declination, -18000, 18000 },
{ "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidController, 0, 2 },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 2 },
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.P8[PITCH], 0, 200 },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.I8[PITCH], 0, 200 },
{ "d_pitch", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.D8[PITCH], 0, 200 },
{ "p_roll", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.P8[ROLL], 0, 200 },
{ "i_roll", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.I8[ROLL], 0, 200 },
{ "d_roll", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.D8[ROLL], 0, 200 },
{ "p_yaw", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.P8[YAW], 0, 200 },
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.I8[YAW], 0, 200 },
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.D8[YAW], 0, 200 },
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 },
{ "d_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PITCH], 0, 200 },
{ "p_roll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[ROLL], 0, 200 },
{ "i_roll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[ROLL], 0, 200 },
{ "d_roll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[ROLL], 0, 200 },
{ "p_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[YAW], 0, 200 },
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[YAW], 0, 200 },
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[YAW], 0, 200 },
{ "p_pitchf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.P_f[PITCH], 0, 100 },
{ "i_pitchf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.I_f[PITCH], 0, 100 },
{ "d_pitchf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.D_f[PITCH], 0, 100 },
{ "p_rollf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.P_f[ROLL], 0, 100 },
{ "i_rollf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.I_f[ROLL], 0, 100 },
{ "d_rollf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.D_f[ROLL], 0, 100 },
{ "p_yawf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.P_f[YAW], 0, 100 },
{ "i_yawf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.I_f[YAW], 0, 100 },
{ "d_yawf", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.D_f[YAW], 0, 100 },
{ "p_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[PITCH], 0, 100 },
{ "i_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[PITCH], 0, 100 },
{ "d_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[PITCH], 0, 100 },
{ "p_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[ROLL], 0, 100 },
{ "i_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[ROLL], 0, 100 },
{ "d_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[ROLL], 0, 100 },
{ "p_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[YAW], 0, 100 },
{ "i_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[YAW], 0, 100 },
{ "d_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[YAW], 0, 100 },
{ "level_horizon", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.H_level, 0, 10 },
{ "level_angle", VAR_FLOAT | PROFILE_VALUE, &currentProfile.pidProfile.A_level, 0, 10 },
{ "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, 0, 10 },
{ "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, 0, 10 },
{ "p_alt", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.P8[PIDALT], 0, 200 },
{ "i_alt", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.I8[PIDALT], 0, 200 },
{ "d_alt", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.D8[PIDALT], 0, 200 },
{ "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], 0, 200 },
{ "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], 0, 200 },
{ "d_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDALT], 0, 200 },
{ "p_level", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.P8[PIDLEVEL], 0, 200 },
{ "i_level", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.I8[PIDLEVEL], 0, 200 },
{ "d_level", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.D8[PIDLEVEL], 0, 200 },
{ "p_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDLEVEL], 0, 200 },
{ "i_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDLEVEL], 0, 200 },
{ "d_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], 0, 200 },
{ "p_vel", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.P8[PIDVEL], 0, 200 },
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.I8[PIDVEL], 0, 200 },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &currentProfile.pidProfile.D8[PIDVEL], 0, 200 },
{ "p_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDVEL], 0, 200 },
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
};
#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
@ -390,14 +390,14 @@ static void cliAux(char *cmdline)
if (len == 0) {
// print out aux channel settings
for (i = 0; i < CHECKBOX_ITEM_COUNT; i++)
printf("aux %u %u\r\n", i, currentProfile.activate[i]);
printf("aux %u %u\r\n", i, currentProfile->activate[i]);
} else {
ptr = cmdline;
i = atoi(ptr);
if (i < CHECKBOX_ITEM_COUNT) {
ptr = strchr(cmdline, ' ');
val = atoi(ptr);
currentProfile.activate[i] = val;
currentProfile->activate[i] = val;
} else {
printf("Invalid Feature index: must be < %u\r\n", CHECKBOX_ITEM_COUNT);
}
@ -874,7 +874,7 @@ static void cliSave(char *cmdline)
UNUSED(cmdline);
cliPrint("Saving");
copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
//copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
writeEEPROM();
cliReboot();
}
@ -904,29 +904,34 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full)
int32_t value = 0;
char buf[8];
void *ptr = var->ptr;
if (var->type & PROFILE_VALUE) {
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
}
switch (var->type & VALUE_TYPE_MASK) {
case VAR_UINT8:
value = *(uint8_t *)var->ptr;
value = *(uint8_t *)ptr;
break;
case VAR_INT8:
value = *(int8_t *)var->ptr;
value = *(int8_t *)ptr;
break;
case VAR_UINT16:
value = *(uint16_t *)var->ptr;
value = *(uint16_t *)ptr;
break;
case VAR_INT16:
value = *(int16_t *)var->ptr;
value = *(int16_t *)ptr;
break;
case VAR_UINT32:
value = *(uint32_t *)var->ptr;
value = *(uint32_t *)ptr;
break;
case VAR_FLOAT:
printf("%s", ftoa(*(float *)var->ptr, buf));
printf("%s", ftoa(*(float *)ptr, buf));
if (full) {
printf(" %s", ftoa((float)var->min, buf));
printf(" %s", ftoa((float)var->max, buf));
@ -940,23 +945,28 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full)
static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
{
void *ptr = var->ptr;
if (var->type & PROFILE_VALUE) {
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
}
switch (var->type & VALUE_TYPE_MASK) {
case VAR_UINT8:
case VAR_INT8:
*(char *)var->ptr = (char)value.int_value;
*(char *)ptr = (char)value.int_value;
break;
case VAR_UINT16:
case VAR_INT16:
*(short *)var->ptr = (short)value.int_value;
*(short *)ptr = (short)value.int_value;
break;
case VAR_UINT32:
*(int *)var->ptr = (int)value.int_value;
*(int *)ptr = (int)value.int_value;
break;
case VAR_FLOAT:
*(float *)var->ptr = (float)value.float_value;
*(float *)ptr = (float)value.float_value;
break;
}
}

129
src/main/io/serial_msp.c
View File

@ -137,13 +137,11 @@ extern int16_t debug[4]; // FIXME dependency on mw.c
#define ACTIVATE_MASK 0xFFF // see
typedef struct box_e {
struct box_t {
const uint8_t boxIndex; // this is from boxnames enum
const char *boxName; // GUI-readable box name
const uint8_t permanentId; //
} box_t;
static const box_t const boxes[] = {
} boxes[] = {
{ BOXARM, "ARM;", 0 },
{ BOXANGLE, "ANGLE;", 1 },
{ BOXHORIZON, "HORIZON;", 2 },
@ -418,8 +416,8 @@ static void evaluateCommand(void)
rxMspFrameRecieve();
break;
case MSP_SET_ACC_TRIM:
currentProfile.accelerometerTrims.values.pitch = read16();
currentProfile.accelerometerTrims.values.roll = read16();
currentProfile->accelerometerTrims.values.pitch = read16();
currentProfile->accelerometerTrims.values.roll = read16();
headSerialReply(0);
break;
#ifdef GPS
@ -435,47 +433,47 @@ static void evaluateCommand(void)
break;
#endif
case MSP_SET_PID:
if (currentProfile.pidController == 2) {
if (currentProfile->pidController == 2) {
for (i = 0; i < 3; i++) {
currentProfile.pidProfile.P_f[i] = (float)read8() / 10.0f;
currentProfile.pidProfile.I_f[i] = (float)read8() / 100.0f;
currentProfile.pidProfile.D_f[i] = (float)read8() / 1000.0f;
currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f;
currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f;
currentProfile->pidProfile.D_f[i] = (float)read8() / 1000.0f;
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
currentProfile.pidProfile.A_level = (float)read8() / 10.0f;
currentProfile.pidProfile.H_level = (float)read8() / 10.0f;
currentProfile->pidProfile.A_level = (float)read8() / 10.0f;
currentProfile->pidProfile.H_level = (float)read8() / 10.0f;
read8();
} else {
currentProfile.pidProfile.P8[i] = read8();
currentProfile.pidProfile.I8[i] = read8();
currentProfile.pidProfile.D8[i] = read8();
currentProfile->pidProfile.P8[i] = read8();
currentProfile->pidProfile.I8[i] = read8();
currentProfile->pidProfile.D8[i] = read8();
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
currentProfile.pidProfile.P8[i] = read8();
currentProfile.pidProfile.I8[i] = read8();
currentProfile.pidProfile.D8[i] = read8();
currentProfile->pidProfile.P8[i] = read8();
currentProfile->pidProfile.I8[i] = read8();
currentProfile->pidProfile.D8[i] = read8();
}
}
headSerialReply(0);
break;
case MSP_SET_BOX:
for (i = 0; i < numberBoxItems; i++)
currentProfile.activate[availableBoxes[i]] = read16() & ACTIVATE_MASK;
currentProfile->activate[availableBoxes[i]] = read16() & ACTIVATE_MASK;
for (i = 0; i < numberBoxItems; i++)
currentProfile.activate[availableBoxes[i]] |= (read16() & ACTIVATE_MASK) << 16;
currentProfile->activate[availableBoxes[i]] |= (read16() & ACTIVATE_MASK) << 16;
headSerialReply(0);
break;
case MSP_SET_RC_TUNING:
currentProfile.controlRateConfig.rcRate8 = read8();
currentProfile.controlRateConfig.rcExpo8 = read8();
currentProfile.controlRateConfig.rollPitchRate = read8();
currentProfile.controlRateConfig.yawRate = read8();
currentProfile.dynThrPID = read8();
currentProfile.controlRateConfig.thrMid8 = read8();
currentProfile.controlRateConfig.thrExpo8 = read8();
currentProfile->controlRateConfig.rcRate8 = read8();
currentProfile->controlRateConfig.rcExpo8 = read8();
currentProfile->controlRateConfig.rollPitchRate = read8();
currentProfile->controlRateConfig.yawRate = read8();
currentProfile->dynThrPID = read8();
currentProfile->controlRateConfig.thrMid8 = read8();
currentProfile->controlRateConfig.thrExpo8 = read8();
headSerialReply(0);
break;
case MSP_SET_MISC:
@ -483,10 +481,10 @@ static void evaluateCommand(void)
masterConfig.escAndServoConfig.minthrottle = read16();
masterConfig.escAndServoConfig.maxthrottle = read16();
masterConfig.escAndServoConfig.mincommand = read16();
currentProfile.failsafeConfig.failsafe_throttle = read16();
currentProfile->failsafeConfig.failsafe_throttle = read16();
read16();
read32();
currentProfile.mag_declination = read16() * 10;
currentProfile->mag_declination = read16() * 10;
masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended
masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI
masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI
@ -578,38 +576,38 @@ static void evaluateCommand(void)
case MSP_SERVO_CONF:
headSerialReply(56);
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
serialize16(currentProfile.servoConf[i].min);
serialize16(currentProfile.servoConf[i].max);
serialize16(currentProfile.servoConf[i].middle);
serialize8(currentProfile.servoConf[i].rate);
serialize16(currentProfile->servoConf[i].min);
serialize16(currentProfile->servoConf[i].max);
serialize16(currentProfile->servoConf[i].middle);
serialize8(currentProfile->servoConf[i].rate);
}
break;
case MSP_SET_SERVO_CONF:
headSerialReply(0);
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
currentProfile.servoConf[i].min = read16();
currentProfile.servoConf[i].max = read16();
currentProfile->servoConf[i].min = read16();
currentProfile->servoConf[i].max = read16();
// provide temporary support for old clients that try and send a channel index instead of a servo middle
uint16_t potentialServoMiddleOrChannelToForward = read16();
if (potentialServoMiddleOrChannelToForward < MAX_SUPPORTED_SERVOS) {
currentProfile.servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward;
currentProfile->servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward;
}
if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) {
currentProfile.servoConf[i].middle = potentialServoMiddleOrChannelToForward;
currentProfile->servoConf[i].middle = potentialServoMiddleOrChannelToForward;
}
currentProfile.servoConf[i].rate = read8();
currentProfile->servoConf[i].rate = read8();
}
break;
case MSP_CHANNEL_FORWARDING:
headSerialReply(8);
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
serialize8(currentProfile.servoConf[i].forwardFromChannel);
serialize8(currentProfile->servoConf[i].forwardFromChannel);
}
break;
case MSP_SET_CHANNEL_FORWARDING:
headSerialReply(0);
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
currentProfile.servoConf[i].forwardFromChannel = read8();
currentProfile->servoConf[i].forwardFromChannel = read8();
}
break;
case MSP_MOTOR:
@ -661,38 +659,38 @@ static void evaluateCommand(void)
break;
case MSP_RC_TUNING:
headSerialReply(7);
serialize8(currentProfile.controlRateConfig.rcRate8);
serialize8(currentProfile.controlRateConfig.rcExpo8);
serialize8(currentProfile.controlRateConfig.rollPitchRate);
serialize8(currentProfile.controlRateConfig.yawRate);
serialize8(currentProfile.dynThrPID);
serialize8(currentProfile.controlRateConfig.thrMid8);
serialize8(currentProfile.controlRateConfig.thrExpo8);
serialize8(currentProfile->controlRateConfig.rcRate8);
serialize8(currentProfile->controlRateConfig.rcExpo8);
serialize8(currentProfile->controlRateConfig.rollPitchRate);
serialize8(currentProfile->controlRateConfig.yawRate);
serialize8(currentProfile->dynThrPID);
serialize8(currentProfile->controlRateConfig.thrMid8);
serialize8(currentProfile->controlRateConfig.thrExpo8);
break;
case MSP_PID:
headSerialReply(3 * PID_ITEM_COUNT);
if (currentProfile.pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
if (currentProfile->pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
for (i = 0; i < 3; i++) {
serialize8(constrain(lrintf(currentProfile.pidProfile.P_f[i] * 10.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile.pidProfile.I_f[i] * 100.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile.pidProfile.D_f[i] * 1000.0f), 0, 100));
serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 100));
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
serialize8(constrain(lrintf(currentProfile.pidProfile.A_level * 10.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile.pidProfile.H_level * 10.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250));
serialize8(0);
} else {
serialize8(currentProfile.pidProfile.P8[i]);
serialize8(currentProfile.pidProfile.I8[i]);
serialize8(currentProfile.pidProfile.D8[i]);
serialize8(currentProfile->pidProfile.P8[i]);
serialize8(currentProfile->pidProfile.I8[i]);
serialize8(currentProfile->pidProfile.D8[i]);
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
serialize8(currentProfile.pidProfile.P8[i]);
serialize8(currentProfile.pidProfile.I8[i]);
serialize8(currentProfile.pidProfile.D8[i]);
serialize8(currentProfile->pidProfile.P8[i]);
serialize8(currentProfile->pidProfile.I8[i]);
serialize8(currentProfile->pidProfile.D8[i]);
}
}
break;
@ -703,9 +701,9 @@ static void evaluateCommand(void)
case MSP_BOX:
headSerialReply(4 * numberBoxItems);
for (i = 0; i < numberBoxItems; i++)
serialize16(currentProfile.activate[availableBoxes[i]] & ACTIVATE_MASK);
serialize16(currentProfile->activate[availableBoxes[i]] & ACTIVATE_MASK);
for (i = 0; i < numberBoxItems; i++)
serialize16((currentProfile.activate[availableBoxes[i]] >> 16) & ACTIVATE_MASK);
serialize16((currentProfile->activate[availableBoxes[i]] >> 16) & ACTIVATE_MASK);
break;
case MSP_BOXNAMES:
// headSerialReply(sizeof(boxnames) - 1);
@ -722,10 +720,10 @@ static void evaluateCommand(void)
serialize16(masterConfig.escAndServoConfig.minthrottle);
serialize16(masterConfig.escAndServoConfig.maxthrottle);
serialize16(masterConfig.escAndServoConfig.mincommand);
serialize16(currentProfile.failsafeConfig.failsafe_throttle);
serialize16(currentProfile->failsafeConfig.failsafe_throttle);
serialize16(0); // plog useless shit
serialize32(0); // plog useless shit
serialize16(currentProfile.mag_declination / 10); // TODO check this shit
serialize16(currentProfile->mag_declination / 10); // TODO check this shit
serialize8(masterConfig.batteryConfig.vbatscale);
serialize8(masterConfig.batteryConfig.vbatmincellvoltage);
serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage);
@ -802,7 +800,6 @@ static void evaluateCommand(void)
if (f.ARMED) {
headSerialError(0);
} else {
copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
writeEEPROM();
readEEPROM();
headSerialReply(0);
@ -819,8 +816,8 @@ static void evaluateCommand(void)
// Additional commands that are not compatible with MultiWii
case MSP_ACC_TRIM:
headSerialReply(4);
serialize16(currentProfile.accelerometerTrims.values.pitch);
serialize16(currentProfile.accelerometerTrims.values.roll);
serialize16(currentProfile->accelerometerTrims.values.pitch);
serialize16(currentProfile->accelerometerTrims.values.roll);
break;
case MSP_UID:
headSerialReply(12);

8
src/main/main.c
View File

@ -142,7 +142,7 @@ void init(void)
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, currentProfile.mag_declination);
sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, currentProfile->mag_declination);
// production debug output
#ifdef PROD_DEBUG
@ -197,7 +197,7 @@ void init(void)
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = currentProfile.gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
@ -224,8 +224,8 @@ void init(void)
&masterConfig.gpsConfig
);
navigationInit(
&currentProfile.gpsProfile,
&currentProfile.pidProfile
&currentProfile->gpsProfile,
&currentProfile->pidProfile
);
}
#endif

56
src/main/mw.c
View File

@ -96,10 +96,10 @@ extern pidControllerFuncPtr pid_controller;
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
currentProfile.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
currentProfile.accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
currentProfile->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
currentProfile->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
saveAndReloadCurrentProfileToCurrentProfileSlot();
saveConfigAndNotify();
}
#ifdef AUTOTUNE
@ -116,12 +116,12 @@ void updateAutotuneState(void)
autotuneReset();
landedAfterAutoTuning = false;
}
autotuneBeginNextPhase(&currentProfile.pidProfile, currentProfile.pidController);
autotuneBeginNextPhase(&currentProfile->pidProfile, currentProfile->pidController);
f.AUTOTUNE_MODE = 1;
autoTuneWasUsed = true;
} else {
if (havePidsBeenUpdatedByAutotune()) {
saveAndReloadCurrentProfileToCurrentProfileSlot();
saveConfigAndNotify();
autotuneReset();
}
}
@ -163,22 +163,22 @@ void annexCode(void)
static int32_t vbatCycleTime = 0;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < currentProfile.tpa_breakpoint) {
if (rcData[THROTTLE] < currentProfile->tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentProfile.dynThrPID * (rcData[THROTTLE] - currentProfile.tpa_breakpoint) / (2000 - currentProfile.tpa_breakpoint);
prop2 = 100 - (uint16_t)currentProfile->dynThrPID * (rcData[THROTTLE] - currentProfile->tpa_breakpoint) / (2000 - currentProfile->tpa_breakpoint);
} else {
prop2 = 100 - currentProfile.dynThrPID;
prop2 = 100 - currentProfile->dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile.deadband) {
if (tmp > currentProfile.deadband) {
tmp -= currentProfile.deadband;
if (currentProfile->deadband) {
if (tmp > currentProfile->deadband) {
tmp -= currentProfile->deadband;
} else {
tmp = 0;
}
@ -186,24 +186,24 @@ void annexCode(void)
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500;
prop1 = 100 - (uint16_t)currentProfile->controlRateConfig.rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
}
if (axis == YAW) {
if (currentProfile.yaw_deadband) {
if (tmp > currentProfile.yaw_deadband) {
tmp -= currentProfile.yaw_deadband;
if (currentProfile->yaw_deadband) {
if (tmp > currentProfile->yaw_deadband) {
tmp -= currentProfile->yaw_deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500;
prop1 = 100 - (uint16_t)currentProfile->controlRateConfig.yawRate * abs(tmp) / 500;
}
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)currentProfile.pidProfile.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)currentProfile.pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile.pidProfile.D8[axis] * prop1 / 100;
dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis];
@ -358,7 +358,7 @@ void updateMagHold(void)
dif -= 360;
dif *= -masterConfig.yaw_control_direction;
if (f.SMALL_ANGLE)
rcCommand[YAW] -= dif * currentProfile.pidProfile.P8[PIDMAG] / 30; // 18 deg
rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30; // 18 deg
} else
magHold = heading;
}
@ -462,7 +462,7 @@ void processRx(void)
resetErrorGyro();
}
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile.activate, masterConfig.retarded_arm);
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile->activate, masterConfig.retarded_arm);
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
@ -488,7 +488,7 @@ void processRx(void)
(rcData[AUX5 + i] > 1700) << (3 * i + 2)) << 16;
}
for (i = 0; i < CHECKBOX_ITEM_COUNT; i++)
rcOptions[i] = (auxState & currentProfile.activate[i]) > 0;
rcOptions[i] = (auxState & currentProfile->activate[i]) > 0;
if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
@ -585,7 +585,7 @@ void loop(void)
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + masterConfig.looptime;
computeIMU(&currentProfile.accelerometerTrims, masterConfig.mixerConfiguration);
computeIMU(&currentProfile->accelerometerTrims, masterConfig.mixerConfiguration);
// Measure loop rate just after reading the sensors
currentTime = micros();
@ -616,8 +616,8 @@ void loop(void)
#endif
if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile.throttle_correction_value);
if (currentProfile->throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
}
#ifdef GPS
@ -630,10 +630,10 @@ void loop(void)
// PID - note this is function pointer set by setPIDController()
pid_controller(
&currentProfile.pidProfile,
&currentProfile.controlRateConfig,
&currentProfile->pidProfile,
&currentProfile->controlRateConfig,
masterConfig.max_angle_inclination,
&currentProfile.accelerometerTrims
&currentProfile->accelerometerTrims
);
mixTable();

4
src/main/sensors/acceleration.c
View File

@ -94,7 +94,7 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
resetRollAndPitchTrims(rollAndPitchTrims);
saveAndReloadCurrentProfileToCurrentProfileSlot();
saveConfigAndNotify();
}
calibratingA--;
@ -149,7 +149,7 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
resetRollAndPitchTrims(rollAndPitchTrims);
saveAndReloadCurrentProfileToCurrentProfileSlot();
saveConfigAndNotify();
}
}

2
src/main/sensors/compass.c
View File

@ -98,7 +98,7 @@ void updateCompass(flightDynamicsTrims_t *magZero)
magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
}
saveAndReloadCurrentProfileToCurrentProfileSlot();
saveConfigAndNotify();
}
}
}