mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 04:15:44 +03:00
Remove sensor_acceleration.c's dependency on mw.h/board.h.
In doing so accelerometer sensor and trim code had to be cleaned. Added a new method to buzzer.c to avoid exposing toggleBeep. Renamed current_profile to current_profile_index to avoid confusion.
This commit is contained in:
parent
1092fa5b40
commit
fbfb75b24a
15 changed files with 293 additions and 190 deletions
37
src/buzzer.c
37
src/buzzer.c
|
@ -11,14 +11,15 @@
|
|||
|
||||
#include "buzzer.h"
|
||||
|
||||
#define LONG_PAUSE_DELAY 50
|
||||
#define LONG_PAUSE_DURATION_MILLIS 50
|
||||
#define SHORT_CONFIRMATION_BEEP_DURATION_MILLIS 50
|
||||
|
||||
static uint8_t buzzerIsOn = 0, beepDone = 0;
|
||||
static uint32_t buzzerLastToggleTime;
|
||||
static void beep(uint16_t pulse);
|
||||
static void beep(uint16_t pulseMillis);
|
||||
static void beep_code(char first, char second, char third, char pause);
|
||||
|
||||
uint8_t toggleBeep = 0;
|
||||
static uint8_t toggleBeep = 0;
|
||||
|
||||
typedef enum {
|
||||
FAILSAFE_IDLE = 0,
|
||||
|
@ -97,6 +98,11 @@ void buzzer(bool warn_vbat)
|
|||
}
|
||||
}
|
||||
|
||||
// duration is specified in multiples of SHORT_CONFIRMATION_BEEP_DURATION_MILLIS
|
||||
void queueConfirmationBeep(uint8_t duration) {
|
||||
toggleBeep = duration;
|
||||
}
|
||||
|
||||
void beep_code(char first, char second, char third, char pause)
|
||||
{
|
||||
char patternChar[4];
|
||||
|
@ -121,7 +127,7 @@ void beep_code(char first, char second, char third, char pause)
|
|||
Duration = 0;
|
||||
break;
|
||||
default:
|
||||
Duration = LONG_PAUSE_DELAY;
|
||||
Duration = LONG_PAUSE_DURATION_MILLIS;
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -140,18 +146,23 @@ void beep_code(char first, char second, char third, char pause)
|
|||
}
|
||||
}
|
||||
|
||||
static void beep(uint16_t pulse)
|
||||
static void beep(uint16_t pulseMillis)
|
||||
{
|
||||
if (!buzzerIsOn && (millis() >= (buzzerLastToggleTime + LONG_PAUSE_DELAY))) { // Buzzer is off and long pause time is up -> turn it on
|
||||
if (buzzerIsOn) {
|
||||
if (millis() >= buzzerLastToggleTime + pulseMillis) {
|
||||
buzzerIsOn = 0;
|
||||
BEEP_OFF;
|
||||
buzzerLastToggleTime = millis();
|
||||
if (toggleBeep >0)
|
||||
toggleBeep--;
|
||||
beepDone = 1;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (millis() >= (buzzerLastToggleTime + LONG_PAUSE_DURATION_MILLIS)) { // Buzzer is off and long pause time is up -> turn it on
|
||||
buzzerIsOn = 1;
|
||||
BEEP_ON;
|
||||
buzzerLastToggleTime = millis(); // save the time the buzer turned on
|
||||
} else if (buzzerIsOn && (millis() >= buzzerLastToggleTime + pulse)) { // Buzzer is on and time is up -> turn it off
|
||||
buzzerIsOn = 0;
|
||||
BEEP_OFF;
|
||||
buzzerLastToggleTime = millis();
|
||||
if (toggleBeep >0)
|
||||
toggleBeep--;
|
||||
beepDone = 1;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -1,3 +1,4 @@
|
|||
#pragma once
|
||||
|
||||
void buzzer(bool warn_vbat);
|
||||
void queueConfirmationBeep(uint8_t duration);
|
||||
|
|
30
src/config.c
30
src/config.c
|
@ -90,6 +90,8 @@ void activateConfig(void)
|
|||
gpsUseProfile(¤tProfile.gpsProfile);
|
||||
gpsUsePIDs(¤tProfile.pidProfile);
|
||||
useFailsafeConfig(¤tProfile.failsafeConfig);
|
||||
|
||||
setAccelerationTrims(&masterConfig.accZero);
|
||||
}
|
||||
|
||||
void readEEPROM(void)
|
||||
|
@ -101,9 +103,9 @@ void readEEPROM(void)
|
|||
// Read flash
|
||||
memcpy(&masterConfig, (char *)FLASH_WRITE_ADDR, sizeof(master_t));
|
||||
// Copy current profile
|
||||
if (masterConfig.current_profile > 2) // sanity check
|
||||
masterConfig.current_profile = 0;
|
||||
memcpy(¤tProfile, &masterConfig.profile[masterConfig.current_profile], sizeof(profile_t));
|
||||
if (masterConfig.current_profile_index > 2) // sanity check
|
||||
masterConfig.current_profile_index = 0;
|
||||
memcpy(¤tProfile, &masterConfig.profile[masterConfig.current_profile_index], sizeof(profile_t));
|
||||
|
||||
activateConfig();
|
||||
}
|
||||
|
@ -171,6 +173,13 @@ void resetEEPROM(void)
|
|||
writeEEPROM();
|
||||
}
|
||||
|
||||
static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
|
||||
{
|
||||
accelerometerTrims->trims.pitch = 0;
|
||||
accelerometerTrims->trims.roll = 0;
|
||||
accelerometerTrims->trims.yaw = 0;
|
||||
}
|
||||
|
||||
static void resetPidProfile(pidProfile_t *pidProfile)
|
||||
{
|
||||
pidProfile->P8[ROLL] = 40;
|
||||
|
@ -230,13 +239,11 @@ static void resetConf(void)
|
|||
featureSet(FEATURE_VBAT);
|
||||
|
||||
// global settings
|
||||
masterConfig.current_profile = 0; // default profile
|
||||
masterConfig.current_profile_index = 0; // default profile
|
||||
masterConfig.gyro_cmpf_factor = 600; // default MWC
|
||||
masterConfig.gyro_cmpfm_factor = 250; // default MWC
|
||||
masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
|
||||
masterConfig.accZero[0] = 0;
|
||||
masterConfig.accZero[1] = 0;
|
||||
masterConfig.accZero[2] = 0;
|
||||
resetAccelerometerTrims(&masterConfig.accZero);
|
||||
masterConfig.gyro_align = ALIGN_DEFAULT;
|
||||
masterConfig.acc_align = ALIGN_DEFAULT;
|
||||
masterConfig.mag_align = ALIGN_DEFAULT;
|
||||
|
@ -299,8 +306,7 @@ static void resetConf(void)
|
|||
currentProfile.controlRateConfig.thrExpo8 = 0;
|
||||
// for (i = 0; i < CHECKBOXITEMS; i++)
|
||||
// cfg.activate[i] = 0;
|
||||
currentProfile.angleTrim[0] = 0;
|
||||
currentProfile.angleTrim[1] = 0;
|
||||
resetRollAndPitchTrims(¤tProfile.accelerometerTrims);
|
||||
currentProfile.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
|
||||
currentProfile.acc_lpf_factor = 4;
|
||||
currentProfile.accz_deadband = 40;
|
||||
|
@ -352,6 +358,12 @@ static void resetConf(void)
|
|||
memcpy(&masterConfig.profile[i], ¤tProfile, sizeof(profile_t));
|
||||
}
|
||||
|
||||
void saveAndReloadCurrentProfileToCurrentProfileSlot(void)
|
||||
{
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
|
||||
writeEEPROM();
|
||||
readEEPROMAndNotify();
|
||||
}
|
||||
|
||||
bool feature(uint32_t mask)
|
||||
{
|
||||
|
|
|
@ -31,3 +31,4 @@ void readEEPROM(void);
|
|||
void readEEPROMAndNotify(void);
|
||||
void writeEEPROM();
|
||||
void ensureEEPROMContainsValidData(void);
|
||||
void saveAndReloadCurrentProfileToCurrentProfileSlot(void);
|
||||
|
|
|
@ -36,7 +36,7 @@ typedef struct master_t {
|
|||
uint16_t gyro_cmpfm_factor; // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter
|
||||
uint8_t moron_threshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
||||
uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees).
|
||||
int16_t accZero[3];
|
||||
int16_flightDynamicsTrims_t accZero;
|
||||
int16_t magZero[3];
|
||||
|
||||
batteryConfig_t batteryConfig;
|
||||
|
@ -59,8 +59,8 @@ typedef struct master_t {
|
|||
uint8_t telemetry_provider; // See TelemetryProvider 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.
|
||||
profile_t profile[3]; // 3 separate profiles
|
||||
uint8_t current_profile; // currently loaded profile
|
||||
profile_t profile[3]; // 3 separate profiles
|
||||
uint8_t current_profile_index; // currently loaded profile
|
||||
|
||||
uint8_t magic_ef; // magic number, should be 0xEF
|
||||
uint8_t chk; // XOR checksum
|
||||
|
|
|
@ -8,9 +8,9 @@ typedef struct profile_s {
|
|||
controlRateConfig_t controlRateConfig;
|
||||
|
||||
uint8_t dynThrPID;
|
||||
uint16_t tpaBreakPoint; // Breakpoint where TPA is activated
|
||||
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
|
||||
rollAndPitchTrims_t accelerometerTrims; // 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
|
||||
|
|
|
@ -21,9 +21,9 @@ uint8_t dynP8[3], dynI8[3], dynD8[3];
|
|||
static int32_t errorGyroI[3] = { 0, 0, 0 };
|
||||
static int32_t errorAngleI[2] = { 0, 0 };
|
||||
|
||||
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim);
|
||||
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);
|
||||
|
||||
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim); // pid controller function prototype
|
||||
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype
|
||||
|
||||
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
|
||||
|
||||
|
@ -34,6 +34,12 @@ void mwDisarm(void)
|
|||
f.ARMED = 0;
|
||||
}
|
||||
|
||||
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
|
||||
{
|
||||
rollAndPitchTrims->trims.roll = 0;
|
||||
rollAndPitchTrims->trims.pitch = 0;
|
||||
}
|
||||
|
||||
void resetErrorAngle(void)
|
||||
{
|
||||
errorAngleI[ROLL] = 0;
|
||||
|
@ -47,7 +53,7 @@ void resetErrorGyro(void)
|
|||
errorGyroI[YAW] = 0;
|
||||
}
|
||||
|
||||
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim)
|
||||
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
|
||||
{
|
||||
int axis, prop;
|
||||
int32_t error, errorAngle;
|
||||
|
@ -60,16 +66,16 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
// **** PITCH & ROLL & YAW PID ****
|
||||
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
|
||||
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 == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
|
||||
// 50 degrees max inclination
|
||||
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim[axis];
|
||||
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim->raw[axis];
|
||||
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);
|
||||
|
||||
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
|
||||
ITermACC = (errorAngleI[axis] * pidProfile->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 == FD_YAW) { // MODE relying on GYRO or YAW axis
|
||||
error = (int32_t)rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
|
||||
error -= gyroData[axis];
|
||||
|
||||
|
@ -80,11 +86,11 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
errorGyroI[axis] = 0;
|
||||
ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) >> 6;
|
||||
}
|
||||
if (f.HORIZON_MODE && axis < 2) {
|
||||
if (f.HORIZON_MODE && (axis == FD_ROLL || axis == FD_PITCH)) {
|
||||
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
|
||||
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
|
||||
} else {
|
||||
if (f.ANGLE_MODE && axis < 2) {
|
||||
if (f.ANGLE_MODE && (axis == FD_ROLL || axis == FD_PITCH)) {
|
||||
PTerm = PTermACC;
|
||||
ITerm = ITermACC;
|
||||
} else {
|
||||
|
@ -106,7 +112,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
|
||||
#define GYRO_I_MAX 256
|
||||
|
||||
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim)
|
||||
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
|
||||
{
|
||||
int32_t errorAngle = 0;
|
||||
int axis;
|
||||
|
@ -119,11 +125,11 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
// ----------PID controller----------
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
// -----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 == FD_PITCH || axis == FD_ROLL)) { // MODE relying on ACC
|
||||
// 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) - angle[axis] + angleTrim[axis]; // 16 bits is ok here
|
||||
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim->raw[axis]; // 16 bits is ok here
|
||||
}
|
||||
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
|
||||
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
|
||||
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);
|
||||
} else {
|
||||
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
|
||||
|
|
|
@ -30,16 +30,55 @@ enum {
|
|||
|
||||
extern int16_t angle[ANGLE_INDEX_COUNT]; // see angle_index_t
|
||||
|
||||
// See http://en.wikipedia.org/wiki/Flight_dynamics
|
||||
enum {
|
||||
GI_ROLL = 0,
|
||||
GI_PITCH,
|
||||
GI_YAW
|
||||
} gyro_index_t;
|
||||
FD_ROLL = 0,
|
||||
FD_PITCH,
|
||||
FD_YAW
|
||||
} flight_dynamics_index_t;
|
||||
|
||||
#define GYRO_INDEX_COUNT 3
|
||||
#define FLIGHT_DYNAMICS_INDEX_COUNT 3
|
||||
|
||||
extern int16_t gyroData[GYRO_INDEX_COUNT]; // see gyro_index_t
|
||||
extern int16_t gyroZero[GYRO_INDEX_COUNT]; // see gyro_index_t
|
||||
typedef struct fp_vector {
|
||||
float X;
|
||||
float Y;
|
||||
float Z;
|
||||
} t_fp_vector_def;
|
||||
|
||||
typedef union {
|
||||
float A[3];
|
||||
t_fp_vector_def V;
|
||||
} t_fp_vector;
|
||||
|
||||
typedef struct fp_angles {
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
} fp_angles_t;
|
||||
|
||||
typedef struct int16_flightDynamicsTrims_s {
|
||||
int16_t roll;
|
||||
int16_t pitch;
|
||||
int16_t yaw;
|
||||
} int16_flightDynamicsTrims_def_t;
|
||||
|
||||
typedef union {
|
||||
int16_t raw[3];
|
||||
int16_flightDynamicsTrims_def_t trims;
|
||||
} int16_flightDynamicsTrims_t;
|
||||
|
||||
typedef struct rollAndPitchTrims_s {
|
||||
int16_t roll;
|
||||
int16_t pitch;
|
||||
} rollAndPitchTrims_t_def;
|
||||
|
||||
typedef union {
|
||||
int16_t raw[2];
|
||||
rollAndPitchTrims_t_def trims;
|
||||
} rollAndPitchTrims_t;
|
||||
|
||||
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT]; // see gyro_index_t
|
||||
extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT]; // see gyro_index_t
|
||||
|
||||
extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
|
||||
extern int32_t accSum[XYZ_AXIS_COUNT];
|
||||
|
@ -49,6 +88,7 @@ extern int16_t heading, magHold;
|
|||
|
||||
void mwDisarm(void);
|
||||
void setPIDController(int type);
|
||||
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
|
||||
void resetErrorAngle(void);
|
||||
void resetErrorGyro(void);
|
||||
|
||||
|
|
|
@ -66,8 +66,8 @@ float magneticDeclination = 0.0f; // calculated at startup from config
|
|||
// **************
|
||||
// gyro+acc IMU
|
||||
// **************
|
||||
int16_t gyroData[GYRO_INDEX_COUNT] = { 0, 0, 0 };
|
||||
int16_t gyroZero[GYRO_INDEX_COUNT] = { 0, 0, 0 };
|
||||
int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
|
||||
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
|
||||
|
@ -94,7 +94,7 @@ void computeIMU(void)
|
|||
|
||||
gyroGetADC();
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
accGetADC();
|
||||
updateAccelerationReadings(¤tProfile.accelerometerTrims);
|
||||
getEstimatedAttitude();
|
||||
} else {
|
||||
accADC[X] = 0;
|
||||
|
@ -103,10 +103,10 @@ void computeIMU(void)
|
|||
}
|
||||
|
||||
if (masterConfig.mixerConfiguration == MULTITYPE_TRI) {
|
||||
gyroData[GI_YAW] = (gyroYawSmooth * 2 + gyroADC[GI_YAW]) / 3;
|
||||
gyroYawSmooth = gyroData[GI_YAW];
|
||||
gyroData[GI_ROLL] = gyroADC[GI_ROLL];
|
||||
gyroData[GI_PITCH] = gyroADC[GI_PITCH];
|
||||
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
|
||||
gyroYawSmooth = gyroData[FD_YAW];
|
||||
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
|
||||
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
|
||||
} else {
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
gyroData[axis] = gyroADC[axis];
|
||||
|
@ -127,25 +127,9 @@ void computeIMU(void)
|
|||
//
|
||||
// **************************************************
|
||||
|
||||
typedef struct fp_vector {
|
||||
float X;
|
||||
float Y;
|
||||
float Z;
|
||||
} t_fp_vector_def;
|
||||
|
||||
typedef union {
|
||||
float A[3];
|
||||
t_fp_vector_def V;
|
||||
} t_fp_vector;
|
||||
|
||||
t_fp_vector EstG;
|
||||
|
||||
typedef struct fp_angles {
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
} fp_angles_t;
|
||||
|
||||
// Normalize a vector
|
||||
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
|
||||
{
|
||||
|
@ -200,9 +184,9 @@ void rotateAnglesV(struct fp_vector *v, fp_angles_t *delta)
|
|||
// deprecated - it uses legacy indices for ROLL/PITCH/YAW, see rc_alias_e - use rotateAnglesV instead
|
||||
void rotateV(struct fp_vector *v, float *delta) {
|
||||
fp_angles_t temp;
|
||||
temp.roll = delta[GI_ROLL];
|
||||
temp.pitch = delta[GI_PITCH];
|
||||
temp.yaw = delta[GI_YAW];
|
||||
temp.roll = delta[FD_ROLL];
|
||||
temp.pitch = delta[FD_PITCH];
|
||||
temp.yaw = delta[FD_YAW];
|
||||
|
||||
rotateAnglesV(v, &temp);
|
||||
}
|
||||
|
|
24
src/mw.c
24
src/mw.c
|
@ -4,10 +4,12 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/typeconversion.h"
|
||||
|
||||
#include "buzzer.h"
|
||||
#include "sensors_sonar.h"
|
||||
#include "sensors_gyro.h"
|
||||
#include "sensors_compass.h"
|
||||
#include "sensors_barometer.h"
|
||||
#include "sensors_acceleration.h"
|
||||
#include "flight_common.h"
|
||||
#include "serial_cli.h"
|
||||
#include "telemetry_common.h"
|
||||
|
@ -34,7 +36,7 @@ uint16_t rssi; // range: [0;1023]
|
|||
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||
extern failsafe_t *failsafe;
|
||||
|
||||
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim);
|
||||
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims);
|
||||
|
||||
extern pidControllerFuncPtr pid_controller;
|
||||
|
||||
|
@ -305,9 +307,9 @@ void loop(void)
|
|||
} else {
|
||||
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
|
||||
if (AccInflightCalibrationArmed) {
|
||||
toggleBeep = 2;
|
||||
queueConfirmationBeep(2);
|
||||
} else {
|
||||
toggleBeep = 3;
|
||||
queueConfirmationBeep(3);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -320,7 +322,7 @@ void loop(void)
|
|||
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
|
||||
i = 3;
|
||||
if (i) {
|
||||
masterConfig.current_profile = i - 1;
|
||||
masterConfig.current_profile_index = i - 1;
|
||||
writeEEPROM();
|
||||
readEEPROM();
|
||||
blinkLedAndSoundBeeper(2, 40, i);
|
||||
|
@ -335,27 +337,27 @@ void loop(void)
|
|||
mwArm();
|
||||
// Calibrating Acc
|
||||
else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
|
||||
calibratingA = CALIBRATING_ACC_CYCLES;
|
||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||
// Calibrating Mag
|
||||
else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
|
||||
f.CALIBRATE_MAG = 1;
|
||||
i = 0;
|
||||
// Acc Trim
|
||||
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
|
||||
currentProfile.angleTrim[PITCH] += 2;
|
||||
currentProfile.accelerometerTrims.trims.pitch += 2;
|
||||
i = 1;
|
||||
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
|
||||
currentProfile.angleTrim[PITCH] -= 2;
|
||||
currentProfile.accelerometerTrims.trims.pitch -= 2;
|
||||
i = 1;
|
||||
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
|
||||
currentProfile.angleTrim[ROLL] += 2;
|
||||
currentProfile.accelerometerTrims.trims.roll += 2;
|
||||
i = 1;
|
||||
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
|
||||
currentProfile.angleTrim[ROLL] -= 2;
|
||||
currentProfile.accelerometerTrims.trims.roll -= 2;
|
||||
i = 1;
|
||||
}
|
||||
if (i) {
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile);
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
|
||||
writeEEPROM();
|
||||
readEEPROMAndNotify();
|
||||
rcDelayCommand = 0; // allow autorepetition
|
||||
|
@ -602,7 +604,7 @@ void loop(void)
|
|||
}
|
||||
|
||||
// PID - note this is function pointer set by setPIDController()
|
||||
pid_controller(¤tProfile.pidProfile, ¤tProfile.controlRateConfig, masterConfig.max_angle_inclination, currentProfile.angleTrim);
|
||||
pid_controller(¤tProfile.pidProfile, ¤tProfile.controlRateConfig, masterConfig.max_angle_inclination, ¤tProfile.accelerometerTrims);
|
||||
|
||||
mixTable();
|
||||
writeServos();
|
||||
|
|
|
@ -1,8 +1,17 @@
|
|||
#include "board.h"
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "drivers/accgyro_common.h"
|
||||
#include "flight_common.h"
|
||||
|
||||
#include "mw.h"
|
||||
#include "sensors_common.h"
|
||||
#include "buzzer.h"
|
||||
#include "boardalignment.h"
|
||||
#include "runtime_config.h"
|
||||
#include "config.h"
|
||||
|
||||
#include "sensors_acceleration.h"
|
||||
|
||||
|
@ -18,102 +27,139 @@ extern bool AccInflightCalibrationMeasurementDone;
|
|||
extern bool AccInflightCalibrationSavetoEEProm;
|
||||
extern bool AccInflightCalibrationActive;
|
||||
|
||||
int16_flightDynamicsTrims_t *accelerationTrims;
|
||||
|
||||
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||
{
|
||||
calibratingA = calibrationCyclesRequired;
|
||||
}
|
||||
|
||||
void accCommon(void)
|
||||
bool isAccelerationCalibrationComplete(void)
|
||||
{
|
||||
static int32_t a[3];
|
||||
int axis;
|
||||
|
||||
if (calibratingA > 0) {
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
// Reset a[axis] at start of calibration
|
||||
if (calibratingA == CALIBRATING_ACC_CYCLES)
|
||||
a[axis] = 0;
|
||||
// Sum up CALIBRATING_ACC_CYCLES readings
|
||||
a[axis] += accADC[axis];
|
||||
// Clear global variables for next reading
|
||||
accADC[axis] = 0;
|
||||
masterConfig.accZero[axis] = 0;
|
||||
}
|
||||
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
|
||||
if (calibratingA == 1) {
|
||||
masterConfig.accZero[GI_ROLL] = (a[GI_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
|
||||
masterConfig.accZero[GI_PITCH] = (a[GI_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
|
||||
masterConfig.accZero[GI_YAW] = (a[GI_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
|
||||
currentProfile.angleTrim[AI_ROLL] = 0;
|
||||
currentProfile.angleTrim[AI_PITCH] = 0;
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile);
|
||||
writeEEPROM(); // write accZero in EEPROM
|
||||
readEEPROMAndNotify();
|
||||
}
|
||||
calibratingA--;
|
||||
}
|
||||
|
||||
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
||||
static int32_t b[3];
|
||||
static int16_t accZero_saved[3] = { 0, 0, 0 };
|
||||
static int16_t angleTrim_saved[2] = { 0, 0 };
|
||||
// Saving old zeropoints before measurement
|
||||
if (InflightcalibratingA == 50) {
|
||||
accZero_saved[GI_ROLL] = masterConfig.accZero[GI_ROLL];
|
||||
accZero_saved[GI_PITCH] = masterConfig.accZero[GI_PITCH];
|
||||
accZero_saved[GI_YAW] = masterConfig.accZero[GI_YAW];
|
||||
angleTrim_saved[AI_ROLL] = currentProfile.angleTrim[AI_ROLL];
|
||||
angleTrim_saved[AI_PITCH] = currentProfile.angleTrim[AI_PITCH];
|
||||
}
|
||||
if (InflightcalibratingA > 0) {
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
// Reset a[axis] at start of calibration
|
||||
if (InflightcalibratingA == 50)
|
||||
b[axis] = 0;
|
||||
// Sum up 50 readings
|
||||
b[axis] += accADC[axis];
|
||||
// Clear global variables for next reading
|
||||
accADC[axis] = 0;
|
||||
masterConfig.accZero[axis] = 0;
|
||||
}
|
||||
// all values are measured
|
||||
if (InflightcalibratingA == 1) {
|
||||
AccInflightCalibrationActive = false;
|
||||
AccInflightCalibrationMeasurementDone = true;
|
||||
toggleBeep = 2; // buzzer for indicatiing the end of calibration
|
||||
// recover saved values to maintain current flight behavior until new values are transferred
|
||||
masterConfig.accZero[GI_ROLL] = accZero_saved[GI_ROLL];
|
||||
masterConfig.accZero[GI_PITCH] = accZero_saved[GI_PITCH];
|
||||
masterConfig.accZero[GI_YAW] = accZero_saved[GI_YAW];
|
||||
currentProfile.angleTrim[AI_ROLL] = angleTrim_saved[AI_ROLL];
|
||||
currentProfile.angleTrim[AI_PITCH] = angleTrim_saved[AI_PITCH];
|
||||
}
|
||||
InflightcalibratingA--;
|
||||
}
|
||||
// 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
|
||||
AccInflightCalibrationSavetoEEProm = false;
|
||||
masterConfig.accZero[GI_ROLL] = b[GI_ROLL] / 50;
|
||||
masterConfig.accZero[GI_PITCH] = b[GI_PITCH] / 50;
|
||||
masterConfig.accZero[GI_YAW] = b[GI_YAW] / 50 - acc_1G; // for nunchuk 200=1G
|
||||
currentProfile.angleTrim[AI_ROLL] = 0;
|
||||
currentProfile.angleTrim[AI_PITCH] = 0;
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile);
|
||||
writeEEPROM(); // write accZero in EEPROM
|
||||
readEEPROMAndNotify();
|
||||
}
|
||||
}
|
||||
|
||||
accADC[GI_ROLL] -= masterConfig.accZero[GI_ROLL];
|
||||
accADC[GI_PITCH] -= masterConfig.accZero[GI_PITCH];
|
||||
accADC[GI_YAW] -= masterConfig.accZero[GI_YAW];
|
||||
return calibratingA == 0;
|
||||
}
|
||||
|
||||
void accGetADC(void)
|
||||
bool isOnFinalAccelerationCalibrationCycle(void)
|
||||
{
|
||||
return calibratingA == 1;
|
||||
}
|
||||
|
||||
bool isOnFirstAccelerationCalibrationCycle(void)
|
||||
{
|
||||
return calibratingA == CALIBRATING_ACC_CYCLES;
|
||||
}
|
||||
|
||||
void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
|
||||
{
|
||||
static int32_t a[3];
|
||||
uint8_t axis;
|
||||
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
|
||||
// Reset a[axis] at start of calibration
|
||||
if (isOnFirstAccelerationCalibrationCycle())
|
||||
a[axis] = 0;
|
||||
|
||||
// Sum up CALIBRATING_ACC_CYCLES readings
|
||||
a[axis] += accADC[axis];
|
||||
|
||||
// Reset global variables to prevent other code from using un-calibrated data
|
||||
accADC[axis] = 0;
|
||||
accelerationTrims->raw[axis] = 0;
|
||||
}
|
||||
|
||||
if (isOnFinalAccelerationCalibrationCycle()) {
|
||||
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
|
||||
accelerationTrims->raw[FD_ROLL] = (a[FD_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
|
||||
accelerationTrims->raw[FD_PITCH] = (a[FD_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
|
||||
accelerationTrims->raw[FD_YAW] = (a[FD_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
|
||||
|
||||
resetRollAndPitchTrims(rollAndPitchTrims);
|
||||
|
||||
saveAndReloadCurrentProfileToCurrentProfileSlot();
|
||||
}
|
||||
|
||||
calibratingA--;
|
||||
}
|
||||
|
||||
void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
|
||||
{
|
||||
uint8_t axis;
|
||||
static int32_t b[3];
|
||||
static int16_t accZero_saved[3] = { 0, 0, 0 };
|
||||
static rollAndPitchTrims_t angleTrim_saved = { { 0, 0 } };
|
||||
|
||||
// Saving old zeropoints before measurement
|
||||
if (InflightcalibratingA == 50) {
|
||||
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;
|
||||
}
|
||||
if (InflightcalibratingA > 0) {
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
// Reset a[axis] at start of calibration
|
||||
if (InflightcalibratingA == 50)
|
||||
b[axis] = 0;
|
||||
// Sum up 50 readings
|
||||
b[axis] += accADC[axis];
|
||||
// Clear global variables for next reading
|
||||
accADC[axis] = 0;
|
||||
accelerationTrims->raw[axis] = 0;
|
||||
}
|
||||
// all values are measured
|
||||
if (InflightcalibratingA == 1) {
|
||||
AccInflightCalibrationActive = false;
|
||||
AccInflightCalibrationMeasurementDone = true;
|
||||
queueConfirmationBeep(2); // buzzer to indicatiing the end of calibration
|
||||
// recover saved values to maintain current flight behavior until new values are transferred
|
||||
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;
|
||||
}
|
||||
InflightcalibratingA--;
|
||||
}
|
||||
// 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
|
||||
AccInflightCalibrationSavetoEEProm = false;
|
||||
accelerationTrims->raw[FD_ROLL] = b[FD_ROLL] / 50;
|
||||
accelerationTrims->raw[FD_PITCH] = b[FD_PITCH] / 50;
|
||||
accelerationTrims->raw[FD_YAW] = b[FD_YAW] / 50 - acc_1G; // for nunchuk 200=1G
|
||||
|
||||
resetRollAndPitchTrims(rollAndPitchTrims);
|
||||
|
||||
saveAndReloadCurrentProfileToCurrentProfileSlot();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void applyAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrims)
|
||||
{
|
||||
accADC[FD_ROLL] -= accelerationTrims->raw[FD_ROLL];
|
||||
accADC[FD_PITCH] -= accelerationTrims->raw[FD_PITCH];
|
||||
accADC[FD_YAW] -= accelerationTrims->raw[FD_YAW];
|
||||
}
|
||||
|
||||
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
|
||||
{
|
||||
acc.read(accADC);
|
||||
alignSensors(accADC, accADC, accAlign);
|
||||
|
||||
accCommon();
|
||||
if (!isAccelerationCalibrationComplete()) {
|
||||
performAcclerationCalibration(rollAndPitchTrims);
|
||||
}
|
||||
|
||||
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
||||
performInflightAccelerationCalibration(rollAndPitchTrims);
|
||||
}
|
||||
|
||||
applyAccelerationTrims(accelerationTrims);
|
||||
}
|
||||
|
||||
void setAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrimsToUse)
|
||||
{
|
||||
accelerationTrims = accelerationTrimsToUse;
|
||||
}
|
||||
|
|
|
@ -16,6 +16,5 @@ extern acc_t acc;
|
|||
extern uint16_t calibratingA;
|
||||
|
||||
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||
void accCommon(void);
|
||||
void accGetADC(void);
|
||||
|
||||
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
|
||||
void setAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrimsToUse);
|
||||
|
|
|
@ -62,7 +62,7 @@ int compassGetADC(void)
|
|||
for (axis = 0; axis < 3; axis++) {
|
||||
masterConfig.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
|
||||
}
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile);
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
|
||||
writeEEPROM();
|
||||
readEEPROMAndNotify();
|
||||
}
|
||||
|
|
|
@ -194,8 +194,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.angleTrim[PITCH], -300, 300 },
|
||||
{ "acc_trim_roll", VAR_INT16, ¤tProfile.angleTrim[ROLL], -300, 300 },
|
||||
{ "acc_trim_pitch", VAR_INT16, ¤tProfile.accelerometerTrims.trims.pitch, -300, 300 },
|
||||
{ "acc_trim_roll", VAR_INT16, ¤tProfile.accelerometerTrims.trims.roll, -300, 300 },
|
||||
{ "baro_tab_size", VAR_UINT8, ¤tProfile.baro_tab_size, 0, BARO_TAB_SIZE_MAX },
|
||||
{ "baro_noise_lpf", VAR_FLOAT, ¤tProfile.baro_noise_lpf, 0, 1 },
|
||||
{ "baro_cf_vel", VAR_FLOAT, ¤tProfile.baro_cf_vel, 0, 1 },
|
||||
|
@ -821,12 +821,12 @@ static void cliProfile(char *cmdline)
|
|||
|
||||
len = strlen(cmdline);
|
||||
if (len == 0) {
|
||||
printf("Current profile: %d\r\n", masterConfig.current_profile);
|
||||
printf("Current profile: %d\r\n", masterConfig.current_profile_index);
|
||||
return;
|
||||
} else {
|
||||
i = atoi(cmdline);
|
||||
if (i >= 0 && i <= 2) {
|
||||
masterConfig.current_profile = i;
|
||||
masterConfig.current_profile_index = i;
|
||||
writeEEPROM();
|
||||
readEEPROM();
|
||||
cliProfile("");
|
||||
|
@ -843,7 +843,7 @@ static void cliReboot(void) {
|
|||
static void cliSave(char *cmdline)
|
||||
{
|
||||
cliPrint("Saving...");
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile);
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
|
||||
writeEEPROM();
|
||||
cliReboot();
|
||||
}
|
||||
|
|
|
@ -8,6 +8,7 @@
|
|||
#include "telemetry_common.h"
|
||||
#include "flight_common.h"
|
||||
#include "sensors_compass.h"
|
||||
#include "sensors_acceleration.h"
|
||||
|
||||
// Multiwii Serial Protocol 0
|
||||
#define MSP_VERSION 0
|
||||
|
@ -294,8 +295,8 @@ static void evaluateCommand(void)
|
|||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_SET_ACC_TRIM:
|
||||
currentProfile.angleTrim[PITCH] = read16();
|
||||
currentProfile.angleTrim[ROLL] = read16();
|
||||
currentProfile.accelerometerTrims.trims.pitch = read16();
|
||||
currentProfile.accelerometerTrims.trims.roll = read16();
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_SET_RAW_GPS:
|
||||
|
@ -353,9 +354,9 @@ static void evaluateCommand(void)
|
|||
break;
|
||||
case MSP_SELECT_SETTING:
|
||||
if (!f.ARMED) {
|
||||
masterConfig.current_profile = read8();
|
||||
if (masterConfig.current_profile > 2) {
|
||||
masterConfig.current_profile = 0;
|
||||
masterConfig.current_profile_index = read8();
|
||||
if (masterConfig.current_profile_index > 2) {
|
||||
masterConfig.current_profile_index = 0;
|
||||
}
|
||||
writeEEPROM();
|
||||
readEEPROM();
|
||||
|
@ -402,7 +403,7 @@ static void evaluateCommand(void)
|
|||
junk |= 1 << i;
|
||||
}
|
||||
serialize32(junk);
|
||||
serialize8(masterConfig.current_profile);
|
||||
serialize8(masterConfig.current_profile_index);
|
||||
break;
|
||||
case MSP_RAW_IMU:
|
||||
headSerialReply(18);
|
||||
|
@ -609,7 +610,7 @@ static void evaluateCommand(void)
|
|||
break;
|
||||
case MSP_ACC_CALIBRATION:
|
||||
if (!f.ARMED)
|
||||
calibratingA = CALIBRATING_ACC_CYCLES;
|
||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_MAG_CALIBRATION:
|
||||
|
@ -621,7 +622,7 @@ static void evaluateCommand(void)
|
|||
if (f.ARMED) {
|
||||
headSerialError(0);
|
||||
} else {
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile);
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
|
||||
writeEEPROM();
|
||||
readEEPROM();
|
||||
headSerialReply(0);
|
||||
|
@ -638,8 +639,8 @@ static void evaluateCommand(void)
|
|||
// Additional commands that are not compatible with MultiWii
|
||||
case MSP_ACC_TRIM:
|
||||
headSerialReply(4);
|
||||
serialize16(currentProfile.angleTrim[PITCH]);
|
||||
serialize16(currentProfile.angleTrim[ROLL]);
|
||||
serialize16(currentProfile.accelerometerTrims.trims.pitch);
|
||||
serialize16(currentProfile.accelerometerTrims.trims.roll);
|
||||
break;
|
||||
case MSP_UID:
|
||||
headSerialReply(12);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue