/branches/dongfang_FC_fixedwing/analog.c |
---|
439,10 → 439,10 |
int32_t deltaOffsets[3] = { 0, 0, 0 }; |
// Set the filters... to be removed again, once some good settings are found. |
GYROS_PID_FILTER = (dynamicParams.UserParams[4] & (0x7 & (1<<0))) + 1; |
GYROS_PID_FILTER = (staticParams.sensorFilterSettings & (0x7 & (1<<0))) + 1; |
GYROS_ATT_FILTER = 1; |
GYROS_D_FILTER = (dynamicParams.UserParams[4] & (0x3 & (1<<4))) + 1; |
ACC_FILTER = (dynamicParams.UserParams[4] & (0x3 & (1<<6))) + 1; |
GYROS_D_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<4))) + 1; |
ACC_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<6))) + 1; |
gyro_calibrate(); |
/branches/dongfang_FC_fixedwing/attitude.c |
---|
44,7 → 44,7 |
* horizontal plane, yaw relative to yaw at start. Not really used for anything else |
* than diagnostics. |
*/ |
int32_t angle[2]; |
int32_t angle[3]; |
/* |
* Error integrals. Stick is always positive. Gyro is configurable positive or negative. |
52,21 → 52,6 |
*/ |
int32_t error[3]; |
// Yaw angle and compass stuff. |
// This is updated/written from MM3. Negative angle indicates invalid data. |
int16_t compassHeading = -1; |
// This is NOT updated from MM3. Negative angle indicates invalid data. |
int16_t compassCourse = -1; |
// The difference between the above 2 (heading - course) on a -180..179 degree interval. |
// Not necessary. Never read anywhere. |
// int16_t compassOffCourse = 0; |
uint8_t updateCompassCourse = 0; |
uint8_t compassCalState = 0; |
uint16_t ignoreCompassTimer = 500; |
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t correctionSum[2] = { 0, 0 }; |
119,11 → 104,8 |
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
// update compass course to current heading |
compassCourse = compassHeading; |
// Inititialize YawGyroIntegral value with current compass heading |
yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW; |
angle[YAW] = 0; |
// Servo_On(); //enable servo output |
} |
159,43 → 141,35 |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
// TODO: Multiply on a factor. Wont work without... |
error[PITCH] += control[CONTROL_ELEVATOR]; |
error[ROLL] += control[CONTROL_AILERONS]; |
error[YAW] += control[CONTROL_RUDDER]; |
// TODO: Multiply on a factor on control. Wont work without... |
if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) { |
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
} else { |
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
angle[PITCH] += rate_ATT[PITCH]; |
angle[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
angle[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
} else { |
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
angle[PITCH] += rate_ATT[PITCH]; |
angle[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
angle[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
} |
for (axis=PITCH; axis<=YAW; axis++) { |
// TODO: Configurable. |
#define ERRORLIMIT 1000 |
for (axis=PITCH; axis<=YAW; axis++) { |
if (error[axis] > ERRORLIMIT) { |
error[axis] = ERRORLIMIT; |
} else if (angle[axis] <= -ERRORLIMIT) { |
angle[axis] = -ERRORLIMIT; |
} |
} |
} |
/* |
* Yaw |
* Calculate yaw gyro integral (~ to rotation angle) |
* Limit yawGyroHeading proportional to 0 deg to 360 deg |
*/ |
yawGyroHeading += ACYawRate; |
if (yawGyroHeading >= YAWOVER360) { |
yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
} else if (yawGyroHeading < 0) { |
yawGyroHeading += YAWOVER360; |
} |
/* |
* Pitch axis integration and range boundary wrap. |
*/ |
for (axis = PITCH; axis <= ROLL; axis++) { |
206,6 → 180,18 |
angle[axis] += PITCHROLLOVER360; |
} |
} |
/* |
* Yaw |
* Calculate yaw gyro integral (~ to rotation angle) |
* Limit yawGyroHeading proportional to 0 deg to 360 deg |
*/ |
if (angle[YAW] >= YAWOVER360) { |
angle[YAW] -= YAWOVER360; // 360 deg. wrap |
} else if (angle[YAW] < 0) { |
angle[YAW] += YAWOVER360; |
} |
} |
/************************************************************************ |
221,7 → 207,7 |
// Well actually the Z axis acc. check is not so silly. |
uint8_t axis; |
int32_t temp; |
if (acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
if (acc[Z] >= -staticParams.accCorrectionZAccLimit && acc[Z] |
<= dynamicParams.UserParams[7]) { |
DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
/branches/dongfang_FC_fixedwing/attitude.h |
---|
86,22 → 86,10 |
/* |
* Attitudes calculated by numerical integration of gyro rates |
*/ |
extern int32_t angle[2]; |
extern int32_t angle[3]; |
extern int32_t error[3]; |
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
/* |
* Compass navigation |
*/ |
extern int16_t compassHeading; |
extern int16_t compassCourse; |
// extern int16_t compassOffCourse; |
extern uint8_t compassCalState; |
extern int32_t yawGyroHeading; |
extern uint8_t updateCompassCourse; |
extern uint16_t ignoreCompassTimer; |
/* |
* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
* to help canceling out drift and vibration noise effects. The dynamic offsets themselves |
* can be updated in flight by different ways, for example: |
/branches/dongfang_FC_fixedwing/commands.c |
---|
12,7 → 12,6 |
*/ |
uint8_t command = controlMixer_getCommand(); |
uint8_t repeated = controlMixer_isCommandRepeated(); |
uint8_t argument = controlMixer_getArgument(); |
if (!(MKFlags & MKFLAG_MOTOR_RUN)) { |
if (command == COMMAND_GYROCAL && !repeated) { |
25,29 → 24,15 |
// if pitch stick is top or roll stick is left or right --> change parameter setting |
// according to roll/pitch stick position |
if (argument < 6) { |
// Gyro calinbration, with or without selecting a new parameter-set. |
if (argument > 0 && argument < 6) { |
// A valid parameter-set (1..5) was chosen - use it. |
setActiveParamSet(argument); |
} |
ParamSet_ReadFromEEProm(getActiveParamSet()); |
ParamSet_ReadFromEEProm(); |
attitude_setNeutral(); |
flight_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(getActiveParamSet()); |
} else if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE |
| CFG_GPS_ACTIVE) && argument == 7) { |
// If right stick is centered and down |
// TODO: Out of here! State machine instead. |
compassCalState = 1; |
beep(1000); |
} |
beepNumber(1); |
} |
// save the ACC neutral setting to eeprom |
else { |
if (command == COMMAND_ACCCAL && !repeated) { |
else if (command == COMMAND_ACCCAL && !repeated) { |
// Run gyro and acc. meter calibration but do not repeat it. |
GRN_OFF; |
analog_calibrateAcc(); |
54,8 → 39,7 |
attitude_setNeutral(); |
flight_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(getActiveParamSet()); |
} |
beepNumber(1); |
} |
} // end !MOTOR_RUN condition. |
if (command == COMMAND_START) { |
/branches/dongfang_FC_fixedwing/configuration.c |
---|
6,7 → 6,7 |
#include "uart0.h" |
int16_t variables[8] = {0,0,0,0,0,0,0,0}; |
dynamicParam_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0}; |
dynamicParam_t dynamicParams;// = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0}; |
uint8_t CPUType = ATMEGA644; |
uint8_t BoardRelease = 13; |
32,10 → 32,10 |
SET_POT(dynamicParams.GyroRollD,staticParams.GyroRollD); |
SET_POT(dynamicParams.GyroYawD,staticParams.GyroYawD); |
SET_POT(dynamicParams.IFactor,staticParams.IFactor); |
for (i=0; i<sizeof(staticParams.UserParams); i++) { |
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams[i]); |
} |
SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255); |
SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255); |
/branches/dongfang_FC_fixedwing/configuration.h |
---|
95,6 → 95,9 |
uint8_t ExternalControl; // for serial Control |
uint8_t BitConfig; // see upper defines for bitcoding |
uint8_t accCorrectionZAccLimit; |
uint8_t sensorFilterSettings; |
uint8_t Reserved[4]; |
} paramset_t; |
/branches/dongfang_FC_fixedwing/eeprom.c |
---|
32,20 → 32,13 |
*/ |
void setDefaultUserParams(void) { |
uint8_t i; |
for (i = 0; i < sizeof(staticParams.UserParams1); i++) { |
staticParams.UserParams1[i] = 0; |
for (i = 0; i < sizeof(staticParams.UserParams); i++) { |
staticParams.UserParams[i] = 0; |
} |
for (i = 0; i < sizeof(staticParams.UserParams2); i++) { |
staticParams.UserParams2[i] = 0; |
} |
/* |
* While we are still using userparams for flight parameters, do set |
* some safe & meaningful default values. |
*/ |
staticParams.UserParams2[0] = 0xd5; //0b11010101; // All gyro filter constants 2; acc. 4 |
staticParams.UserParams2[1] = 0; // H&I motor smoothing. |
staticParams.UserParams2[2] = 120; // Yaw I factor |
staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles. |
} |
void setOtherDefaults(void) { |
82,7 → 75,6 |
staticParams.StickRudderP = 10; |
staticParams.LowVoltageWarning = 105; |
staticParams.IFactor = 32; |
staticParams.ServoRefresh = 7; |
staticParams.BitConfig = 0; |
staticParams.J16Bitmask = 95; |
89,49 → 81,18 |
staticParams.J17Bitmask = 243; |
staticParams.J16Timing = 15; |
staticParams.J17Timing = 15; |
staticParams.ControlSigns = 2; |
} |
/***************************************************/ |
/* Default Values for parameter set 1 */ |
/***************************************************/ |
void ParamSet_DefaultSet1(void) { // sport |
void setDefaults(void) { |
setOtherDefaults(); |
gyro_setDefaults(); |
setDefaultUserParams(); |
staticParams.J16Timing = 10; |
staticParams.J17Timing = 10; |
memcpy(staticParams.Name, "Sport\0", 6); |
} |
/***************************************************/ |
/* Default Values for parameter set 2 */ |
/***************************************************/ |
void ParamSet_DefaultSet2(void) { // normal |
setOtherDefaults(); |
gyro_setDefaults(); |
setDefaultUserParams(); |
staticParams.Height_Gain = 3; |
staticParams.J16Timing = 20; |
staticParams.J17Timing = 20; |
memcpy(staticParams.Name, "Normal\0", 7); |
} |
/***************************************************/ |
/* Default Values for parameter set 3 */ |
/***************************************************/ |
void ParamSet_DefaultSet3(void) { // beginner |
setOtherDefaults(); |
gyro_setDefaults(); |
setDefaultUserParams(); |
staticParams.Height_Gain = 3; |
staticParams.J16Timing = 30; |
staticParams.J17Timing = 30; |
memcpy(staticParams.Name, "Beginner\0", 9); |
} |
/***************************************************/ |
/* Read Parameter from EEPROM as byte */ |
/***************************************************/ |
uint8_t GetParamByte(uint16_t param_id) { |
165,12 → 126,9 |
/* Read Parameter Set from EEPROM */ |
/***************************************************/ |
// number [1..5] |
void ParamSet_ReadFromEEProm(uint8_t setnumber) { |
if ((1 > setnumber) || (setnumber > 5)) |
setnumber = 3; |
void ParamSet_ReadFromEEProm() { |
eeprom_read_block((uint8_t *) &staticParams.ChannelAssignment[0], |
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber |
- 1)], PARAMSET_STRUCT_LEN); |
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN], PARAMSET_STRUCT_LEN); |
output_init(); |
} |
178,20 → 136,14 |
/* Write Parameter Set to EEPROM */ |
/***************************************************/ |
// number [1..5] |
void ParamSet_WriteToEEProm(uint8_t setnumber) { |
if (setnumber > 5) |
setnumber = 5; |
if (setnumber < 1) |
return; |
void ParamSet_WriteToEEProm() { |
eeprom_write_block((uint8_t *) &staticParams.ChannelAssignment[0], |
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber |
- 1)], PARAMSET_STRUCT_LEN); |
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN], PARAMSET_STRUCT_LEN); |
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAMSET_LENGTH], |
PARAMSET_STRUCT_LEN); |
eeprom_write_block(&staticParams.ChannelAssignment[0], |
&EEPromArray[EEPROM_ADR_CHANNELS], 8); // backup the first 8 bytes that is the rc channel mapping |
// set this parameter set to active set |
setActiveParamSet(setnumber); |
output_init(); |
} |
209,17 → 161,6 |
} |
/***************************************************/ |
/* Set active parameter set */ |
/***************************************************/ |
void setActiveParamSet(uint8_t setnumber) { |
if (setnumber > 5) |
setnumber = 5; |
if (setnumber < 1) |
setnumber = 1; |
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber); |
} |
/***************************************************/ |
/* Read MixerTable from EEPROM */ |
/***************************************************/ |
uint8_t MixerTable_ReadFromEEProm(void) { |
273,7 → 214,7 |
/* Initialize EEPROM Parameter Sets */ |
/***************************************************/ |
void ParamSet_Init(void) { |
uint8_t Channel_Backup = 1, i, j; |
uint8_t Channel_Backup = 1, j; |
// parameter version check |
if (eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) { |
// if version check faild |
284,35 → 225,21 |
Channel_Backup = 0; |
} |
// fill all 5 parameter settings |
for (i = 1; i < 6; i++) { |
switch (i) { |
case 1: |
ParamSet_DefaultSet1(); // Fill staticParams Structure to default parameter set 1 (Sport) |
break; |
case 2: |
ParamSet_DefaultSet2(); // Kamera |
break; |
case 3: |
ParamSet_DefaultSet3(); // Beginner |
break; |
default: |
ParamSet_DefaultSet2(); // Kamera |
break; |
} |
if (Channel_Backup) { // if we have a rc channel mapping backup in eeprom |
// restore it |
for (j = 0; j < 8; j++) { |
staticParams.ChannelAssignment[j] = eeprom_read_byte( |
&EEPromArray[EEPROM_ADR_CHANNELS + j]); |
} |
} |
ParamSet_WriteToEEProm(i); |
} |
// default-Setting is parameter set 3 |
setActiveParamSet(1); |
setDefaults(); // Fill staticParams Structure to default parameter set 1 (Sport) |
if (Channel_Backup) { // if we have a rc channel mapping backup in eeprom |
// restore it |
for (j = 0; j < 8; j++) { |
staticParams.ChannelAssignment[j] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS + j]); |
} |
} |
ParamSet_WriteToEEProm(); |
// update version info |
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION); |
} |
// read active parameter set to staticParams stucture |
ParamSet_ReadFromEEProm(getActiveParamSet()); |
ParamSet_ReadFromEEProm(); |
} |
/branches/dongfang_FC_fixedwing/eeprom.h |
---|
19,8 → 19,8 |
#define EEMIXER_REVISION 1 // is count up, if Mixer stucture has changed (compatibility) |
extern void ParamSet_Init(void); |
extern void ParamSet_ReadFromEEProm(uint8_t setnumber); |
extern void ParamSet_WriteToEEProm(uint8_t setnumber); |
extern void ParamSet_ReadFromEEProm(void); |
extern void ParamSet_WriteToEEProm(void); |
extern uint8_t MixerTable_ReadFromEEProm(void); |
extern uint8_t MixerTable_WriteToEEProm(void); |
31,6 → 31,5 |
extern void SetParamWord(uint16_t param_id, uint16_t value); |
extern uint8_t getActiveParamSet(void); |
extern void setActiveParamSet(uint8_t setnumber); |
#endif //_EEPROM_H |
/branches/dongfang_FC_fixedwing/flight.c |
---|
156,23 → 156,13 |
/************************************************************************/ |
/* Stick signals are positive and gyros are negative... */ |
/************************************************************************/ |
IPart[PITCH] = controlIntegrals[CONTROL_ELEVATOR] - angle[PITCH]; |
if (IPart[PITCH] > PITCHROLLOVER180) IPart[PITCH] -= PITCHROLLOVER360; |
else if (IPart[PITCH] <= -PITCHROLLOVER180) IPart[PITCH] += PITCHROLLOVER360; |
if (IPart[PITCH] > HH_RANGE) IPart[PITCH] = HH_RANGE; |
else if (IPart[PITCH] < -HH_RANGE) IPart[PITCH] = -HH_RANGE; |
IPart[ROLL] = controlIntegrals[CONTROL_AILERONS] - angle[ROLL]; |
if (IPart[ROLL] > PITCHROLLOVER180) IPart[ROLL] -= PITCHROLLOVER360; |
else if (IPart[ROLL] <= -PITCHROLLOVER180) IPart[ROLL] += PITCHROLLOVER360; |
if (IPart[ROLL] > HH_RANGE) IPart[ROLL] = HH_RANGE; |
else if (IPart[ROLL] < -HH_RANGE) IPart[ROLL] = -HH_RANGE; |
IPart[PITCH] = error[PITCH]; // * some factor configurable. |
IPart[ROLL] = error[ROLL]; |
// TODO: Add ipart. Or add/subtract depending, not sure. |
term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]); |
term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]); |
yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
210,7 → 200,7 |
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW; |
DebugOut.Analog[6] = pitchPFactor; |
DebugOut.Analog[7] = rollPFactor; |
219,8 → 209,8 |
DebugOut.Analog[10] = rollDFactor; |
DebugOut.Analog[11] = yawDFactor; |
DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
} |
/branches/dongfang_FC_fixedwing/uart0.c |
---|
494,7 → 494,7 |
else if (pRxData[0] > 5) |
pRxData[0] = 5; // limit to 5 |
// load requested parameter set |
ParamSet_ReadFromEEProm(pRxData[0]); |
ParamSet_ReadFromEEProm(); |
tempchar1 = pRxData[0]; |
tempchar2 = EEPARAM_REVISION; |
while (!txd_complete) |
511,9 → 511,8 |
== EEPARAM_REVISION)) // check for setting to be in range and version of settings |
{ |
memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams)); |
ParamSet_WriteToEEProm(pRxData[0]); |
tempchar1 = getActiveParamSet(); |
beepNumber(tempchar1); |
ParamSet_WriteToEEProm(); |
beepNumber(1); |
} else { |
tempchar1 = 0; //indicate bad data |
} |
656,7 → 655,7 |
/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL]) |
/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.Heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
Data3D.Heading = (int16_t) ((10 * angle[YAW]) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
Data3D_Timer = setDelay(Data3D_Interval); |
request_Data3D = FALSE; |
} |