diff --git a/obj/cleanflight_NAZE.hex b/obj/cleanflight_NAZE.hex index 4f92c4a93..70339f6ef 100644 --- a/obj/cleanflight_NAZE.hex +++ b/obj/cleanflight_NAZE.hex @@ -1883,11 +1883,11 @@ :10759000DFF8B4A1069B35F90A20595F801A084428 :1075A000F9F73CFB5A49F9F741FC8346FFF780FEA7 :1075B00030B1584A5146905D5A46FFF711FC834658 -:1075C000DFF888A19AF80320C2B9059B39F90500B4 -:1075D0001A7914325043F9F721FB4A49F9F726FC8E -:1075E0009AF8043081467BB1D8F848105846F9F72C -:1075F00069FB01464846F9F75DFA04E05846D8F8B9 -:107600004410F9F75FFB8146434B0136585FF9F7A9 +:1075C000DFF888A19AF803202AB15846D8F8441069 +:1075D000F9F778FB17E0059B39F905001A791432A1 +:1075E0005043F9F71BFB4749F9F720FC9AF80430A0 +:1075F00081464BB1D8F848105846F9F763FB01466D +:107600004846F9F757FA8146434B0136585FF9F778 :1076100005FB424B0437D968F9F754FB82465146C3 :107620004846F9F745FAF9698346F9F74BFBDFF865 :107630002091049907905846F9F744FBB96AF9F785 @@ -4771,8 +4771,8 @@ :102A0000753701087C370108873701088C370108C2 :102A1000000000004166726F333220434C4920763B :102A2000657273696F6E20322E32204D61792032CB -:102A3000382032303134202F2031333A33363A3592 -:102A400030202D2028636C65616E666C6967687440 +:102A3000382032303134202F2032303A30353A319C +:102A400034202D2028636C65616E666C696768743C :102A50002900417661696C61626C6520636F6D6D00 :102A6000616E64733A0D0A0025730925730D0A001F :102A700053797374656D20557074696D653A2025BE diff --git a/obj/cleanflight_OLIMEXINO.hex b/obj/cleanflight_OLIMEXINO.hex index 2aa427b36..9f2419e87 100644 --- a/obj/cleanflight_OLIMEXINO.hex +++ b/obj/cleanflight_OLIMEXINO.hex @@ -1463,11 +1463,11 @@ :105B5000FDF722FFDFF8B4A1069B35F90A20595F53 :105B6000801A0844FAF7D2FF5A49FBF7D7F8834660 :105B7000FFF780FE30B1584A5146905D5A46FFF714 -:105B800011FC8346DFF888A19AF80320C2B9059B6F -:105B900039F905001A7914325043FAF7B7FF4A4928 -:105BA000FBF7BCF89AF8043081467BB1D8F848106E -:105BB0005846FAF7FFFF01464846FAF7F3FE04E0BD -:105BC0005846D8F84410FAF7F5FF8146434B0136A2 +:105B800011FC8346DFF888A19AF803202AB1584611 +:105B9000D8F84410FBF70EF817E0059B39F905001B +:105BA0001A7914325043FAF7B1FF4749FBF7B6F8B8 +:105BB0009AF8043081464BB1D8F848105846FAF7A5 +:105BC000F9FF01464846FAF7EDFE8146434B0136A0 :105BD000585FFAF79BFF424B0437D968FAF7EAFFA0 :105BE000824651464846FAF7DBFEF9698346FAF7E2 :105BF000E1FFDFF82091049907905846FAF7DAFFA1 @@ -4107,7 +4107,7 @@ :10008000FF0D0108040E0108000000004166726FB8 :10009000333220434C492076657273696F6E20328B :1000A0002E32204D61792032382032303134202FE9 -:1000B0002031333A33363A3539202D2028636C65A8 +:1000B0002032303A30343A3031202D2028636C65BC :1000C000616E666C696768742900417661696C616C :1000D000626C6520636F6D6D616E64733A0D0A002A :1000E00025730925730D0A0053797374656D2055C6 diff --git a/src/config.c b/src/config.c index 8cc799d3c..8c6638ebb 100755 --- a/src/config.c +++ b/src/config.c @@ -60,11 +60,11 @@ profile_t currentProfile; // profile config struct static const uint8_t EEPROM_CONF_VERSION = 70; -static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims) +static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { - accelerometerTrims->trims.pitch = 0; - accelerometerTrims->trims.roll = 0; - accelerometerTrims->trims.yaw = 0; + accelerometerTrims->values.pitch = 0; + accelerometerTrims->values.roll = 0; + accelerometerTrims->values.yaw = 0; } static void resetPidProfile(pidProfile_t *pidProfile) diff --git a/src/config_master.h b/src/config_master.h index e2cd07d4e..6efb7ed04 100644 --- a/src/config_master.h +++ b/src/config_master.h @@ -34,8 +34,8 @@ typedef struct master_t { gyroConfig_t gyroConfig; uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees). - int16_flightDynamicsTrims_t accZero; - int16_flightDynamicsTrims_t magZero; + flightDynamicsTrims_t accZero; + flightDynamicsTrims_t magZero; batteryConfig_t batteryConfig; diff --git a/src/flight_autotune.c b/src/flight_autotune.c index b6cd98d89..a90dab720 100644 --- a/src/flight_autotune.c +++ b/src/flight_autotune.c @@ -129,10 +129,10 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin updatePidIndex(); if (rising) { - currentAngle = DECIDEGREES_TO_DEGREES(inclination->rawAngles[angleIndex]); + currentAngle = DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]); } else { targetAngle = -targetAngle; - currentAngle = DECIDEGREES_TO_DEGREES(-inclination->rawAngles[angleIndex]); + currentAngle = DECIDEGREES_TO_DEGREES(-inclination->raw[angleIndex]); } #if 1 @@ -244,7 +244,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin updateTargetAngle(); - return targetAngle - DECIDEGREES_TO_DEGREES(inclination->rawAngles[angleIndex]); + return targetAngle - DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]); } void autotuneReset(void) diff --git a/src/flight_common.c b/src/flight_common.c index 96b368546..e3a7a6655 100644 --- a/src/flight_common.c +++ b/src/flight_common.c @@ -35,8 +35,8 @@ pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are w void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) { - rollAndPitchTrims->trims.roll = 0; - rollAndPitchTrims->trims.pitch = 0; + rollAndPitchTrims->values.roll = 0; + rollAndPitchTrims->values.pitch = 0; } void resetErrorAngle(void) @@ -84,7 +84,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f; } else { // calculate error and limit the angle to 50 degrees max inclination - errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - inclination.rawAngles[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here + errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here if (shouldAutotune()) { errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle); @@ -94,7 +94,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control // it's the ANGLE mode - control is angle based, so control loop is needed AngleRate = errorAngle * pidProfile->A_level; } else { - //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID + //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate if (f.HORIZON_MODE) { // mix up angle error to desired AngleRate to add a little auto-level feel @@ -156,12 +156,12 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC // observe max inclination errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; if (shouldAutotune()) { errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); } - + PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5); @@ -224,7 +224,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat } else { // calculate error and limit the angle to max configured inclination errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here + +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here if (shouldAutotune()) { errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); diff --git a/src/flight_common.h b/src/flight_common.h index 083b86c1e..7d42e1d78 100644 --- a/src/flight_common.h +++ b/src/flight_common.h @@ -69,12 +69,12 @@ typedef struct int16_flightDynamicsTrims_s { int16_t roll; int16_t pitch; int16_t yaw; -} int16_flightDynamicsTrims_def_t; +} flightDynamicsTrims_def_t; typedef union { int16_t raw[3]; - int16_flightDynamicsTrims_def_t trims; -} int16_flightDynamicsTrims_t; + flightDynamicsTrims_def_t values; +} flightDynamicsTrims_t; typedef struct rollAndPitchTrims_s { int16_t roll; @@ -83,7 +83,7 @@ typedef struct rollAndPitchTrims_s { typedef union { int16_t raw[2]; - rollAndPitchTrims_t_def trims; + rollAndPitchTrims_t_def values; } rollAndPitchTrims_t; typedef struct rollAndPitchInclination_s { @@ -93,8 +93,8 @@ typedef struct rollAndPitchInclination_s { } rollAndPitchInclination_t_def; typedef union { - int16_t rawAngles[ANGLE_INDEX_COUNT]; - rollAndPitchInclination_t_def angle; + int16_t raw[ANGLE_INDEX_COUNT]; + rollAndPitchInclination_t_def values; } rollAndPitchInclination_t; diff --git a/src/flight_imu.c b/src/flight_imu.c index bc3422bb1..b77acefe2 100755 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -327,8 +327,8 @@ static void getEstimatedAttitude(void) // Attitude of the estimated vector anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z); anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)); - inclination.angle.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI)); - inclination.angle.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI)); + inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI)); + inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI)); if (sensors(SENSOR_MAG)) heading = calculateHeading(&EstM); @@ -361,7 +361,7 @@ static void getEstimatedAttitude(void) bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination) { - return abs(inclination->angle.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->angle.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; + return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES; } int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old) diff --git a/src/flight_mixer.c b/src/flight_mixer.c index c4c864e59..123ffed8c 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -450,8 +450,8 @@ void mixTable(void) break; case MULTITYPE_GIMBAL: - servo[0] = (((int32_t)servoConf[0].rate * inclination.angle.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0); - servo[1] = (((int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1); + servo[0] = (((int32_t)servoConf[0].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0); + servo[1] = (((int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1); break; case MULTITYPE_AIRPLANE: @@ -504,11 +504,11 @@ void mixTable(void) if (rcOptions[BOXCAMSTAB]) { if (gimbalConfig->gimbal_flags & GIMBAL_MIXTILT) { - servo[0] -= (-(int32_t)servoConf[0].rate) * inclination.angle.pitchDeciDegrees / 50 - (int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees / 50; - servo[1] += (-(int32_t)servoConf[0].rate) * inclination.angle.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees / 50; + servo[0] -= (-(int32_t)servoConf[0].rate) * inclination.values.pitchDeciDegrees / 50 - (int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees / 50; + servo[1] += (-(int32_t)servoConf[0].rate) * inclination.values.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees / 50; } else { - servo[0] += (int32_t)servoConf[0].rate * inclination.angle.pitchDeciDegrees / 50; - servo[1] += (int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees / 50; + servo[0] += (int32_t)servoConf[0].rate * inclination.values.pitchDeciDegrees / 50; + servo[1] += (int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees / 50; } } } diff --git a/src/mw.c b/src/mw.c index 1566dcf57..acba11059 100755 --- a/src/mw.c +++ b/src/mw.c @@ -267,7 +267,7 @@ void annexCode(void) static uint32_t LEDTime; if ((int32_t)(currentTime - LEDTime) >= 0) { LEDTime = currentTime + 50000; - ledringState(f.ARMED, inclination.angle.pitchDeciDegrees, inclination.angle.rollDeciDegrees); + ledringState(f.ARMED, inclination.values.pitchDeciDegrees, inclination.values.rollDeciDegrees); } } #endif @@ -450,16 +450,16 @@ void loop(void) i = 0; // Acc Trim if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { - currentProfile.accelerometerTrims.trims.pitch += 2; + currentProfile.accelerometerTrims.values.pitch += 2; i = 1; } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { - currentProfile.accelerometerTrims.trims.pitch -= 2; + currentProfile.accelerometerTrims.values.pitch -= 2; i = 1; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { - currentProfile.accelerometerTrims.trims.roll += 2; + currentProfile.accelerometerTrims.values.roll += 2; i = 1; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { - currentProfile.accelerometerTrims.trims.roll -= 2; + currentProfile.accelerometerTrims.values.roll -= 2; i = 1; } if (i) { diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c index e2abd098e..421b4df10 100644 --- a/src/sensors_acceleration.c +++ b/src/sensors_acceleration.c @@ -28,7 +28,7 @@ extern bool AccInflightCalibrationMeasurementDone; extern bool AccInflightCalibrationSavetoEEProm; extern bool AccInflightCalibrationActive; -static int16_flightDynamicsTrims_t *accelerationTrims; +static flightDynamicsTrims_t *accelerationTrims; void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) { @@ -95,8 +95,8 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri accZero_saved[FD_ROLL] = accelerationTrims->raw[FD_ROLL]; accZero_saved[FD_PITCH] = accelerationTrims->raw[FD_PITCH]; accZero_saved[FD_YAW] = accelerationTrims->raw[FD_YAW]; - angleTrim_saved.trims.roll = rollAndPitchTrims->trims.roll; - angleTrim_saved.trims.pitch = rollAndPitchTrims->trims.pitch; + angleTrim_saved.values.roll = rollAndPitchTrims->values.roll; + angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch; } if (InflightcalibratingA > 0) { for (axis = 0; axis < 3; axis++) { @@ -118,8 +118,8 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri accelerationTrims->raw[FD_ROLL] = accZero_saved[FD_ROLL]; accelerationTrims->raw[FD_PITCH] = accZero_saved[FD_PITCH]; accelerationTrims->raw[FD_YAW] = accZero_saved[FD_YAW]; - rollAndPitchTrims->trims.roll = angleTrim_saved.trims.roll; - rollAndPitchTrims->trims.pitch = angleTrim_saved.trims.pitch; + rollAndPitchTrims->values.roll = angleTrim_saved.values.roll; + rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch; } InflightcalibratingA--; } @@ -137,7 +137,7 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri } -void applyAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrims) +void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims) { accADC[FD_ROLL] -= accelerationTrims->raw[FD_ROLL]; accADC[FD_PITCH] -= accelerationTrims->raw[FD_PITCH]; @@ -160,7 +160,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) applyAccelerationTrims(accelerationTrims); } -void setAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrimsToUse) +void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse) { accelerationTrims = accelerationTrimsToUse; } diff --git a/src/sensors_acceleration.h b/src/sensors_acceleration.h index 5a0e50071..13aaf57db 100644 --- a/src/sensors_acceleration.h +++ b/src/sensors_acceleration.h @@ -20,4 +20,4 @@ extern uint16_t acc_1G; bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims); -void setAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrimsToUse); +void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse); diff --git a/src/sensors_compass.c b/src/sensors_compass.c index 0add266b2..9b639b776 100644 --- a/src/sensors_compass.c +++ b/src/sensors_compass.c @@ -33,11 +33,11 @@ void compassInit(void) magInit = 1; } -int compassGetADC(int16_flightDynamicsTrims_t *magZero) +int compassGetADC(flightDynamicsTrims_t *magZero) { static uint32_t t, tCal = 0; - static int16_flightDynamicsTrims_t magZeroTempMin; - static int16_flightDynamicsTrims_t magZeroTempMax; + static flightDynamicsTrims_t magZeroTempMin; + static flightDynamicsTrims_t magZeroTempMax; uint32_t axis; if ((int32_t)(currentTime - t) < 0) diff --git a/src/sensors_compass.h b/src/sensors_compass.h index b163ef58c..8d4622688 100644 --- a/src/sensors_compass.h +++ b/src/sensors_compass.h @@ -2,7 +2,7 @@ #ifdef MAG void compassInit(void); -int compassGetADC(int16_flightDynamicsTrims_t *magZero); +int compassGetADC(flightDynamicsTrims_t *magZero); #endif extern int16_t magADC[XYZ_AXIS_COUNT]; diff --git a/src/serial_cli.c b/src/serial_cli.c index 3dd33d85a..70b94cca5 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -242,8 +242,8 @@ const clivalue_t valueTable[] = { { "accxy_deadband", VAR_UINT8, ¤tProfile.accxy_deadband, 0, 100 }, { "accz_deadband", VAR_UINT8, ¤tProfile.accz_deadband, 0, 100 }, { "acc_unarmedcal", VAR_UINT8, ¤tProfile.acc_unarmedcal, 0, 1 }, - { "acc_trim_pitch", VAR_INT16, ¤tProfile.accelerometerTrims.trims.pitch, -300, 300 }, - { "acc_trim_roll", VAR_INT16, ¤tProfile.accelerometerTrims.trims.roll, -300, 300 }, + { "acc_trim_pitch", VAR_INT16, ¤tProfile.accelerometerTrims.values.pitch, -300, 300 }, + { "acc_trim_roll", VAR_INT16, ¤tProfile.accelerometerTrims.values.roll, -300, 300 }, { "baro_tab_size", VAR_UINT8, ¤tProfile.barometerConfig.baro_sample_count, 0, BARO_SAMPLE_COUNT_MAX }, { "baro_noise_lpf", VAR_FLOAT, ¤tProfile.barometerConfig.baro_noise_lpf, 0, 1 }, diff --git a/src/serial_msp.c b/src/serial_msp.c index 04dc0af44..f1aa8f9fa 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -390,8 +390,8 @@ static void evaluateCommand(void) rxMspFrameRecieve(); break; case MSP_SET_ACC_TRIM: - currentProfile.accelerometerTrims.trims.pitch = read16(); - currentProfile.accelerometerTrims.trims.roll = read16(); + currentProfile.accelerometerTrims.values.pitch = read16(); + currentProfile.accelerometerTrims.values.roll = read16(); headSerialReply(0); break; case MSP_SET_RAW_GPS: @@ -608,7 +608,7 @@ static void evaluateCommand(void) case MSP_ATTITUDE: headSerialReply(6); for (i = 0; i < 2; i++) - serialize16(inclination.rawAngles[i]); + serialize16(inclination.raw[i]); serialize16(heading); break; case MSP_ALTITUDE: @@ -779,8 +779,8 @@ static void evaluateCommand(void) // Additional commands that are not compatible with MultiWii case MSP_ACC_TRIM: headSerialReply(4); - serialize16(currentProfile.accelerometerTrims.trims.pitch); - serialize16(currentProfile.accelerometerTrims.trims.roll); + serialize16(currentProfile.accelerometerTrims.values.pitch); + serialize16(currentProfile.accelerometerTrims.values.roll); break; case MSP_UID: headSerialReply(12);