/branches/V0.73d Code Redesign killagreg/analog.c |
---|
135,8 → 135,11 |
void SearchDacGyroOffset(void) |
{ |
uint8_t i, ready = 0; |
uint16_t timeout ; |
GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
timeout = SetDelay(2000); |
if(BoardRelease == 13) // the auto offset calibration is available only at board release 1.3 |
{ |
for(i = 140; i != 0; i--) |
146,12 → 149,18 |
if(AdValueGyroNick < 1020) DacOffsetGyroNick--; else if(AdValueGyroNick > 1030) DacOffsetGyroNick++; else ready++; |
if(AdValueGyroRoll < 1020) DacOffsetGyroRoll--; else if(AdValueGyroRoll > 1030) DacOffsetGyroRoll++; else ready++; |
if(AdValueGyroYaw < 1020) DacOffsetGyroYaw-- ; else if(AdValueGyroYaw > 1030) DacOffsetGyroYaw++ ; else ready++; |
twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset |
I2C_Start(); // initiate data transmission |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
if(DacOffsetGyroNick < 10) { GyroDefectNick = 1; DacOffsetGyroNick = 10;}; if(DacOffsetGyroNick > 245) { GyroDefectNick = 1; DacOffsetGyroNick = 245;}; |
if(DacOffsetGyroRoll < 10) { GyroDefectRoll = 1; DacOffsetGyroRoll = 10;}; if(DacOffsetGyroRoll > 245) { GyroDefectRoll = 1; DacOffsetGyroRoll = 245;}; |
if(DacOffsetGyroYaw < 10) { GyroDefectYaw = 1; DacOffsetGyroYaw = 10;}; if(DacOffsetGyroYaw > 245) { GyroDefectYaw = 1; DacOffsetGyroYaw = 245;}; |
while(twi_state); // wait for end of data transmission |
while(twi_state) |
{ |
if(CheckDelay(timeout)) |
{ |
printf("\r\n DAC or I2C Error1 check I2C, 3Vref, DAC, and BL-Ctrl"); |
break; |
} |
} // wait for end of data transmission |
average_pressure = 0; |
ADC_Enable(); |
while(average_pressure == 0); |
/branches/V0.73d Code Redesign killagreg/eeprom.c |
---|
69,15 → 69,16 |
#include "printf_P.h" |
#include "led.h" |
#include "main.h" |
#include "fc.h" |
// byte array in eeprom |
uint8_t EEPromArray[E2END+1] EEMEM; |
paramset_t ParamSet; |
paramset_t ParamSet; |
MixerTable_t Mixer; |
/***************************************************/ |
/* Default Values for parameter set 1 */ |
/***************************************************/ |
368,7 → 369,7 |
/***************************************************/ |
/* Read Parameter from EEPROM as byte */ |
/***************************************************/ |
uint8_t GetParamByte(uint8_t param_id) |
uint8_t GetParamByte(uint16_t param_id) |
{ |
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
} |
376,7 → 377,7 |
/***************************************************/ |
/* Write Parameter to EEPROM as byte */ |
/***************************************************/ |
void SetParamByte(uint8_t param_id, uint8_t value) |
void SetParamByte(uint16_t param_id, uint8_t value) |
{ |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
384,7 → 385,7 |
/***************************************************/ |
/* Read Parameter from EEPROM as word */ |
/***************************************************/ |
uint16_t GetParamWord(uint8_t param_id) |
uint16_t GetParamWord(uint16_t param_id) |
{ |
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
} |
392,12 → 393,11 |
/***************************************************/ |
/* Write Parameter to EEPROM as word */ |
/***************************************************/ |
void SetParamWord(uint8_t param_id, uint16_t value) |
void SetParamWord(uint16_t param_id, uint16_t value) |
{ |
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
/***************************************************/ |
/* Read Parameter Set from EEPROM */ |
/***************************************************/ |
426,6 → 426,57 |
} |
/***************************************************/ |
/* Read MixerTable from EEPROM */ |
/***************************************************/ |
uint8_t MixerTable_ReadFromEEProm(void) |
{ |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == EEMIXER_REVISION) |
{ |
eeprom_read_block((uint8_t *) &Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
return 1; |
} |
else return 0; |
} |
/***************************************************/ |
/* Write Mixer Table to EEPROM */ |
/***************************************************/ |
uint8_t MixerTable_WriteToEEProm(void) |
{ |
if(Mixer.Revision == EEMIXER_REVISION) |
{ |
eeprom_write_block((uint8_t *) &Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
return 1; |
} |
else return 0; |
} |
/***************************************************/ |
/* Default Values for Mixer Table */ |
/***************************************************/ |
void MixerTable_Default(void) // Quadro |
{ |
uint8_t i; |
Mixer.Revision = EEMIXER_REVISION; |
// clear mixer table |
for(i = 0; i < 16; i++) |
{ |
Mixer.Motor[i][MIX_GAS] = 0; |
Mixer.Motor[i][MIX_NICK] = 0; |
Mixer.Motor[i][MIX_ROLL] = 0; |
Mixer.Motor[i][MIX_YAW] = 0; |
} |
// default = Quadro |
Mixer.Motor[0][MIX_GAS] = 64; Mixer.Motor[0][MIX_NICK] = +64; Mixer.Motor[0][MIX_ROLL] = 0; Mixer.Motor[0][MIX_YAW] = +64; |
Mixer.Motor[1][MIX_GAS] = 64; Mixer.Motor[1][MIX_NICK] = -64; Mixer.Motor[1][MIX_ROLL] = 0; Mixer.Motor[1][MIX_YAW] = +64; |
Mixer.Motor[2][MIX_GAS] = 64; Mixer.Motor[2][MIX_NICK] = 0; Mixer.Motor[2][MIX_ROLL] = -64; Mixer.Motor[2][MIX_YAW] = -64; |
Mixer.Motor[3][MIX_GAS] = 64; Mixer.Motor[3][MIX_NICK] = 0; Mixer.Motor[3][MIX_ROLL] = +64; Mixer.Motor[3][MIX_YAW] = -64; |
memcpy(Mixer.Name, "Quadro\0", 7); |
} |
/***************************************************/ |
/* Get active parameter set */ |
/***************************************************/ |
uint8_t GetActiveParamSet(void) |
455,14 → 506,14 |
/***************************************************/ |
void ParamSet_Init(void) |
{ |
uint8_t Channel_Backup = 0; |
uint8_t Channel_Backup = 0, i; |
// parameter version check |
if(eeprom_read_byte(&EEPromArray[PID_VERSION]) != EEPARAM_VERSION) |
if(eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) |
{ |
// if version check faild |
printf("\n\rInit. EEPROM"); |
// check if cannel mapping backup is valid |
printf("\n\rInit Parameter in EEPROM"); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE], 0xFF); // reset also mixer table |
// check if channel mapping backup is valid |
if( (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]) < 12) |
&& (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]) < 12) |
&& (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]) < 12) |
472,7 → 523,7 |
Channel_Backup = 1; |
} |
// fill all 5 parameter settings |
for (uint8_t i = 1;i < 6; i++) |
for (i = 1;i < 6; i++) |
{ |
switch(i) |
{ |
506,9 → 557,26 |
// default-Setting is parameter set 3 |
SetActiveParamSet(3); |
// update version info |
SetParamByte(PID_VERSION, EEPARAM_VERSION); |
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION); |
} |
// read active parameter set to ParamSet stucture |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
printf("\n\rUsing Parameter Set %d", GetActiveParamSet()); |
// load mixer table |
if(!MixerTable_ReadFromEEProm() ) |
{ |
printf("\n\rGenerating default Mixer Table"); |
MixerTable_Default(); // Quadro |
MixerTable_WriteToEEProm(); |
} |
// determine motornumber |
RequiredMotors = 0; |
for(i = 0; i < 16; i++) |
{ |
if(Mixer.Motor[i][MIX_GAS] > 0) RequiredMotors++; |
} |
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name, RequiredMotors); |
printf("\n\r=============================="); |
} |
/branches/V0.73d Code Redesign killagreg/eeprom.h |
---|
4,7 → 4,7 |
#include <inttypes.h> |
#define EEPROM_ADR_PARAM_BEGIN 0 |
#define PID_VERSION 1 // byte |
#define PID_PARAM_REVISION 1 // byte |
#define PID_ACTIVE_SET 2 // byte |
#define PID_PRESSURE_OFFSET 3 // byte |
#define PID_ACC_NICK 4 // word |
12,19 → 12,40 |
#define PID_ACC_TOP 8 // word |
#ifdef USE_KILLAGREG |
#define PID_MM3_X_OFF 10 // byte |
#define PID_MM3_Y_OFF 11 // byte |
#define PID_MM3_Z_OFF 12 // byte |
#define PID_MM3_X_RANGE 13 // word |
#define PID_MM3_Y_RANGE 15 // word |
#define PID_MM3_Z_RANGE 17 // word |
#define PID_MM3_X_OFF 11 // byte |
#define PID_MM3_Y_OFF 12 // byte |
#define PID_MM3_Z_OFF 13 // byte |
#define PID_MM3_X_RANGE 14 // word |
#define PID_MM3_Y_RANGE 16 // word |
#define PID_MM3_Z_RANGE 18 // word |
#endif |
#define EEPROM_ADR_CHANNELS 80 // 8 bytes |
#define EEPROM_ADR_PARAMSET_LENGTH 98 // word |
#define EEPROM_ADR_PARAMSET_BEGIN 100 |
#define EEPROM_ADR_MIXER_TABLE 1000 // 1000 - 1076 |
#define MIX_GAS 0 |
#define MIX_NICK 1 |
#define MIX_ROLL 2 |
#define MIX_YAW 3 |
typedef struct |
{ |
uint8_t Revision; |
int8_t Name[12]; |
int8_t Motor[16][4]; |
} __attribute__((packed)) MixerTable_t; |
extern MixerTable_t Mixer; |
// bit mask for ParamSet.GlobalConfig |
#define CFG_HEIGHT_CONTROL 0x01 |
#define CFG_HEIGHT_SWITCH 0x02 |
52,7 → 73,8 |
#define CH_POTI3 6 |
#define CH_POTI4 7 |
#define EEPARAM_VERSION 74 // is count up, if EE_Paramater stucture has changed (compatibility) |
#define EEPARAM_REVISION 75 // is count up, if paramater stucture has changed (compatibility) |
#define EEMIXER_REVISION 1 // is count up, if Mixer stucture has changed (compatibility) |
// values above 250 representing poti1 to poti4 |
typedef struct |
141,10 → 163,14 |
extern uint8_t GetActiveParamSet(void); |
extern void SetActiveParamSet(uint8_t setnumber); |
extern uint8_t MixerTable_ReadFromEEProm(void); |
extern uint8_t MixerTable_WriteToEEProm(void); |
extern uint8_t GetParamByte(uint8_t param_id); |
extern void SetParamByte(uint8_t param_id, uint8_t value); |
extern uint16_t GetParamWord(uint8_t param_id); |
extern void SetParamWord(uint8_t param_id, uint16_t value); |
extern uint8_t GetParamByte(uint16_t param_id); |
extern void SetParamByte(uint16_t param_id, uint8_t value); |
extern uint16_t GetParamWord(uint16_t param_id); |
extern void SetParamWord(uint16_t param_id, uint16_t value); |
#endif //_EEPROM_H |
/branches/V0.73d Code Redesign killagreg/fc.c |
---|
126,18 → 126,19 |
// MK flags |
uint16_t ModelIsFlying = 0; |
uint8_t MKFlags = 0; |
uint8_t volatile MKFlags = 0; |
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L; |
uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control |
uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control |
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control |
int16_t Ki = 10300 / 33; |
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
// setpoints for motors |
volatile uint8_t Motor1, Motor2, Motor3, Motor4, Motor5, Motor6, Motor7, Motor8; |
uint8_t RequiredMotors = 0; |
// stick values derived by rc channels readings |
158,7 → 159,7 |
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0; |
fc_param_t FCParam = {48,251,16,58,64,8,150,150,2,10,0,0,0,0,0,0,0,0,100,70,90,65,64,100}; |
fc_param_t FCParam = {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}; |
529,53 → 530,27 |
/************************************************************************/ |
void SendMotorData(void) |
{ |
uint8_t i; |
if(!(MKFlags & MKFLAG_MOTOR_RUN)) |
{ |
#ifdef USE_QUADRO |
Motor1 = 0; |
Motor2 = 0; |
Motor3 = 0; |
Motor4 = 0; |
if(MotorTest[0]) Motor1 = MotorTest[0]; |
if(MotorTest[1]) Motor2 = MotorTest[1]; |
if(MotorTest[2]) Motor3 = MotorTest[2]; |
if(MotorTest[3]) Motor4 = MotorTest[3]; |
#else |
Motor1 = 0; |
Motor2 = 0; |
Motor3 = 0; |
Motor4 = 0; |
Motor5 = 0; |
Motor6 = 0; |
Motor7 = 0; |
Motor8 = 0; |
if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];} |
if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];} |
if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];} |
if(MotorTest[2]) {Motor7 = MotorTest[2]; Motor8 = MotorTest[2];} |
#endif |
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if(!MotorTest_Active) Motor[i].SetPoint = 0; |
else Motor[i].SetPoint = MotorTest[i]; |
} |
if(MotorTest_Active) MotorTest_Active--; |
} |
#ifdef USE_QUADRO |
DebugOut.Analog[12] = Motor1; // Front |
DebugOut.Analog[13] = Motor2; // Rear |
DebugOut.Analog[14] = Motor4; // Left |
DebugOut.Analog[15] = Motor3; // Right |
#else // OCTO Motor addresses are counted clockwise starting at the head |
DebugOut.Analog[12] = (Motor1 + Motor2) / 2; |
DebugOut.Analog[13] = (Motor5 + Motor6) / 2; |
DebugOut.Analog[14] = (Motor7 + Motor8) / 2; |
DebugOut.Analog[15] = (Motor3 + Motor4) / 2; |
#endif |
DebugOut.Analog[12] = Motor[0].SetPoint; // Front |
DebugOut.Analog[13] = Motor[1].SetPoint; // Rear |
DebugOut.Analog[14] = Motor[3].SetPoint; // Left |
DebugOut.Analog[15] = Motor[2].SetPoint; // Right |
//Start I2C Interrupt Mode |
twi_state = TWI_STATE_MOTOR_TX; |
I2C_Start(); |
I2C_Start(TWI_STATE_MOTOR_TX); |
} |
/************************************************************************/ |
/* Map the parameter to poti values */ |
/************************************************************************/ |
652,7 → 627,7 |
/************************************************************************/ |
void MotorControl(void) |
{ |
int16_t MotorValue, h, tmp_int; |
int16_t h, tmp_int; |
// Mixer Fractions that are combined for Motor Control |
int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction; |
672,10 → 647,8 |
static int8_t TimerDebugOut = 0; |
static uint16_t UpdateCompassCourse = 0; |
// high resolution motor values for smoothing of PID motor outputs |
static int16_t MotorValue1 = 0, MotorValue2 = 0, MotorValue3 = 0, MotorValue4 = 0; |
#ifndef USE_QUADRO |
static int16_t MotorValue5 = 0, MotorValue6 = 0, MotorValue7 = 0, MotorValue8 = 0; |
#endif |
static int16_t MotorValue[MAX_MOTORS]; |
uint8_t i; |
Mean(); |
GRN_ON; |
962,6 → 935,8 |
// update gyro control loop factors |
GyroPFactor = FCParam.GyroP + 10; |
GyroIFactor = FCParam.GyroI; |
GyroYawPFactor = FCParam.GyroP + 10; |
GyroYawIFactor = FCParam.GyroI; |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1054,7 → 1029,9 |
StickNick = 0; |
StickRoll = 0; |
GyroPFactor = 90; |
GyroIFactor = 120; |
GyroIFactor = 120; |
GyroYawPFactor = 90; |
GyroYawIFactor = 120; |
LoopingRoll = 0; |
LoopingNick = 0; |
MaxStickNick = 0; |
1446,7 → 1423,6 |
// DebugOut.Analog[25] = GyroRoll/2; |
DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift; |
// DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion; |
// DebugOut.Analog[29] = (int16_t)FCParam.KalmanK; |
DebugOut.Analog[30] = GPSStickNick; |
DebugOut.Analog[31] = GPSStickRoll; |
} |
1485,13 → 1461,7 |
} |
PDPartRoll = PPartRoll + (int32_t)((int32_t)GyroRoll * GyroPFactor + (int32_t)TrimRoll * 128L) / (256L / STICK_GAIN); // +D-Part |
// octo has a double yaw momentum because of the doubled motor number |
// therefore double D-Part and halfen P-Part for the same result |
#ifdef USE_OCTO |
PDPartYaw = (int32_t)(GyroYaw * 4 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (4 * (44000 / STICK_GAIN)); |
#else |
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (2 * (44000 / STICK_GAIN)); |
#endif |
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / STICK_GAIN)); |
//DebugOut.Analog[21] = PDPartNick; |
//DebugOut.Analog[22] = PDPartRoll; |
1503,6 → 1473,17 |
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// all BL-Ctrl connected? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(MissingMotor) |
{ |
// if we are in the lift off condition |
if( (ModelIsFlying > 1) && (ModelIsFlying < 50) && (GasMixFraction > 0) ) |
ModelIsFlying = 1; // keep within lift off condition |
GasMixFraction = ParamSet.GasMin; // reduce gas to min to avoid lift of |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Height Control |
// The height control algorithm reduces the gas but does not increase the gas. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1638,207 → 1619,24 |
CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int); |
CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int); |
#ifdef USE_QUADRO |
// QuadroKopter Mixer |
// Motor Front (++) |
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction; |
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1); |
MotorValue = MotorValue1 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor1 = MotorValue; |
// Motor Rear (-+) |
MotorValue = GasMixFraction - NickMixFraction + YawMixFraction; |
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2); |
MotorValue = MotorValue2 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor2 = MotorValue; |
// Motor Right (--) |
MotorValue = GasMixFraction - RollMixFraction - YawMixFraction; |
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3); |
MotorValue = MotorValue3 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor3 = MotorValue; |
// Motor Left (+-) |
MotorValue = GasMixFraction + RollMixFraction - YawMixFraction; |
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4); |
MotorValue = MotorValue4 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor4 = MotorValue; |
#endif |
#ifdef USE_OCTO |
// OctoKopter Mixer |
// Motor 1 (+++) |
MotorValue = GasMixFraction + NickMixFraction + RollMixFraction + YawMixFraction; |
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1); |
MotorValue = MotorValue1 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor1= MotorValue; |
// Motor 2 (+--) |
MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction; |
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2); |
MotorValue = MotorValue2 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor2 = MotorValue; |
// Motor 3 (+-+) |
MotorValue = GasMixFraction + NickMixFraction - RollMixFraction + YawMixFraction; |
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3); |
MotorValue = MotorValue3 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor3 = MotorValue; |
// Motor 4 (---) |
MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction; |
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4); |
MotorValue = MotorValue4 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor4 = MotorValue; |
// Motor 5 (--+) |
MotorValue = GasMixFraction - NickMixFraction - RollMixFraction + YawMixFraction; |
MotorValue5 = MotorSmoothing(MotorValue, MotorValue5); |
MotorValue = MotorValue5 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor5 = MotorValue; |
// Motor 6 (-+-) |
MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction; |
MotorValue6 = MotorSmoothing(MotorValue, MotorValue6); |
MotorValue = MotorValue6 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor6 = MotorValue; |
// Motor7 (-++) |
MotorValue = GasMixFraction - NickMixFraction + RollMixFraction + YawMixFraction; |
MotorValue7 = MotorSmoothing(MotorValue, MotorValue7); |
MotorValue = MotorValue7 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor7 = MotorValue; |
// Motor8 (++-) |
MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction; |
MotorValue8 = MotorSmoothing(MotorValue, MotorValue8); |
MotorValue = MotorValue8 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor8 = MotorValue; |
#endif |
#ifdef USE_OCTO2 |
// Octokopter Mixer alternativ setup |
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction; |
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1); |
MotorValue = MotorValue1 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor1 = MotorValue; |
MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction; |
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2); |
MotorValue = MotorValue2 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor2 = MotorValue; |
MotorValue = GasMixFraction - RollMixFraction + YawMixFraction; |
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3); |
MotorValue = MotorValue3 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor3 = MotorValue; |
MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction; |
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4); |
MotorValue = MotorValue4 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor4 = MotorValue; |
MotorValue = GasMixFraction - RollMixFraction + YawMixFraction; |
MotorValue5 = MotorSmoothing(MotorValue, MotorValue5); |
MotorValue = MotorValue5 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor5 = MotorValue; |
MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction; |
MotorValue6 = MotorSmoothing(MotorValue, MotorValue6); |
MotorValue = MotorValue6 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor6 = MotorValue; |
MotorValue = GasMixFraction + RollMixFraction + YawMixFraction; |
MotorValue7 = MotorSmoothing(MotorValue, MotorValue7); |
MotorValue = MotorValue7 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor7 = MotorValue; |
MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction; |
MotorValue8 = MotorSmoothing(MotorValue, MotorValue8); |
MotorValue = MotorValue8 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor8 = MotorValue; |
#endif |
#ifdef USE_OCTO3 |
// Octokopter Mixer alternativ setup |
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction; |
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1); |
MotorValue = MotorValue1 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor1 = MotorValue; |
MotorValue = GasMixFraction + NickMixFraction - YawMixFraction; |
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2); |
MotorValue = MotorValue2 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor2 = MotorValue; |
MotorValue = GasMixFraction - RollMixFraction + YawMixFraction; |
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3); |
MotorValue = MotorValue3 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor3 = MotorValue; |
MotorValue = GasMixFraction - RollMixFraction - YawMixFraction; |
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4); |
MotorValue = MotorValue4 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor4 = MotorValue; |
MotorValue = GasMixFraction - NickMixFraction + YawMixFraction; |
MotorValue5 = MotorSmoothing(MotorValue, MotorValue5); |
MotorValue = MotorValue5 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor5 = MotorValue; |
MotorValue = GasMixFraction - NickMixFraction - YawMixFraction; |
MotorValue6 = MotorSmoothing(MotorValue, MotorValue6); |
MotorValue = MotorValue6 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor6 = MotorValue; |
MotorValue = GasMixFraction + RollMixFraction + YawMixFraction; |
MotorValue7 = MotorSmoothing(MotorValue, MotorValue7); |
MotorValue = MotorValue7 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor7 = MotorValue; |
MotorValue = GasMixFraction + RollMixFraction - YawMixFraction; |
MotorValue8 = MotorSmoothing(MotorValue, MotorValue8); |
MotorValue = MotorValue8 / STICK_GAIN; |
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax); |
Motor8 = MotorValue; |
#endif |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
int16_t tmp; |
if(Mixer.Motor[i][MIX_GAS] > 0) // if gas then mixer |
{ |
tmp = ((int32_t)GasMixFraction * Mixer.Motor[i][MIX_GAS] ) / 64L; |
tmp += ((int32_t)NickMixFraction * Mixer.Motor[i][MIX_NICK]) / 64L; |
tmp += ((int32_t)RollMixFraction * Mixer.Motor[i][MIX_ROLL]) / 64L; |
tmp += ((int32_t)YawMixFraction * Mixer.Motor[i][MIX_YAW] ) / 64L; |
MotorValue[i] = MotorSmoothing(tmp, MotorValue[i]); // Spike Filter |
tmp = MotorValue[i] / STICK_GAIN; |
CHECK_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax); |
Motor[i].SetPoint = tmp; |
} |
else Motor[i].SetPoint = 0; |
} |
} |
/branches/V0.73d Code Redesign killagreg/fc.h |
---|
26,6 → 26,8 |
#define GYRO_DEG_FACTOR ((int16_t)(ParamSet.GyroAccFactor) * ACC_DEG_FACTOR) |
extern uint8_t RequiredMotors; |
typedef struct |
{ |
uint8_t HeightD; |
124,9 → 126,6 |
extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8; |
// setpoints for motors |
extern volatile uint8_t Motor1, Motor2, Motor3, Motor4, Motor5, Motor6, Motor7, Motor8; |
// current stick values |
extern int16_t StickNick; |
extern int16_t StickRoll; |
152,7 → 151,7 |
#define MKFLAG_RESERVE2 0x40 |
#define MKFLAG_RESERVE3 0x80 |
extern uint8_t MKFlags; |
extern volatile uint8_t MKFlags; |
#endif //_FC_H |
/branches/V0.73d Code Redesign killagreg/main.c |
---|
128,7 → 128,8 |
int16_t main (void) |
{ |
unsigned int timer; |
uint16_t timer; |
uint8_t i; |
// disable interrupts global |
cli(); |
173,6 → 174,7 |
// enable interrupts global |
sei(); |
printf("\n\r==================================="); |
printf("\n\rFlightControl"); |
printf("\n\rHardware: %d.%d", BoardRelease/10, BoardRelease%10); |
if(CPUType == ATMEGA644P) |
180,12 → 182,34 |
else |
printf("\r\n CPU: Atmega644"); |
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\n\r=============================="); |
printf("\n\r==================================="); |
GRN_ON; |
// Parameter Set handling |
ParamSet_Init(); |
// Check connected BL-Ctrls |
printf("\n\rFound BL-Ctrl: "); |
motor_read = 0; |
UpdateMotor = 0; |
SendMotorData(); |
while(!UpdateMotor); |
motor_read = 0; // read the first I2C-Data |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
UpdateMotor = 0; |
SendMotorData(); |
while(!UpdateMotor); |
if(Motor[i].Present) printf("%d ",i+1); |
} |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if(!Motor[i].Present && Mixer.Motor[i][MIX_GAS] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1); |
Motor[i].Error = 0; |
} |
printf("\n\r==================================="); |
if(GetParamWord(PID_ACC_NICK) > 1023) |
{ |
printf("\n\rACC not calibrated!"); |
238,7 → 262,7 |
printf("\n\rControl: "); |
if (ParamSet.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); |
else printf("Neutral"); |
else printf("Neutral (ACC-Mode)"); |
printf("\n\n\r"); |
269,10 → 293,27 |
ExternStickYaw = 0; |
} |
if(RC_Quality) RC_Quality--; |
if(!I2CTimeout) |
#ifdef USE_NAVICTRL |
if(NCDataOkay) |
{ |
I2CTimeout = 5; |
I2C_Reset(); |
if(--NCDataOkay == 0) // no data from NC |
{ // set gps control sticks neutral |
GPSStickNick = 0; |
GPSStickRoll = 0; |
NCSerialDataOkay = 0; |
} |
} |
#endif |
if(!--I2CTimeout || MissingMotor) // try to reset the i2c if motor is missing ot timeout |
{ |
RED_ON; |
if(!I2CTimeout) |
{ |
I2C_Reset(); |
I2CTimeout = 5; |
} |
if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
BeepTime = 10000; // 1 second |
281,7 → 322,6 |
} |
else |
{ |
I2CTimeout--; |
RED_OFF; |
} |
310,7 → 350,6 |
} |
LED_Update(); |
//J4LOW; |
} |
#ifdef USE_NAVICTRL |
/branches/V0.73d Code Redesign killagreg/makefile |
---|
4,12 → 4,12 |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 72 |
VERSION_PATCH = 15 |
VERSION_MINOR = 73 |
VERSION_PATCH = 3 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version |
VERSION_SERIAL_MINOR = 0 # Serial Protocol Minor Version |
NC_SPI_COMPATIBLE = 5 # SPI Protocol Version |
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version |
NC_SPI_COMPATIBLE = 6 # SPI Protocol Version |
#------------------------------------------------------------------- |
#OPTIONS |
23,53 → 23,6 |
RC = DSL |
#RC = SPECTRUM |
# Use one of the motor setups |
# Standard |
SETUP = QUADRO |
# 2 Arms in Front |
#SETUP = OCTO |
# 1 Arm in front |
#SETUP = OCTO2 |
# 1 Arm with two Motors in front or Coax |
#SETUP = OCTO3 |
#------------ |
# Quadro: |
# 1 |
# 4 3 |
# 2 |
#------------ |
# Reverse Props on 1 2 |
#------------ |
# Octo: |
# 1 2 |
# 8 3 |
# 7 4 |
# 6 5 |
#------------ |
#------------ |
# Octo2: |
# 1 |
# 8 2 |
# 7 3 |
# 6 4 |
# 5 |
#------------ |
#------------ |
# Octo3: |
# 1 |
# 2 |
# 8 7 3 4 |
# 5 |
# 6 |
#------------ |
# Reverse Props on octo: 1 3 5 7 |
#------------------------------------------------------------------- |
# get SVN revision |
REV := $(shell sh -c "cat .svn/entries | sed -n '4p'") |
76,12 → 29,12 |
ifeq ($(MCU), atmega644) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
HEX_NAME = MEGA644_$(EXT)_$(RC)_$(SETUP) |
HEX_NAME = MEGA644_$(EXT)_$(RC) |
endif |
ifeq ($(MCU), atmega644p) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
HEX_NAME = MEGA644p_$(EXT)_$(RC)_$(SETUP) |
HEX_NAME = MEGA644p_$(EXT)_$(RC) |
endif |
/branches/V0.73d Code Redesign killagreg/menu.c |
---|
58,6 → 58,7 |
#include "uart0.h" |
#include "printf_P.h" |
#include "analog.h" |
#include "twimaster.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
68,14 → 69,14 |
#endif |
#if (!defined (USE_KILLAGREG) && !defined (USE_MK3MAG)) |
uint8_t MaxMenuItem = 11; |
uint8_t MaxMenuItem = 13; |
#else |
#ifdef USE_MK3MAG |
uint8_t MaxMenuItem = 12; |
uint8_t MaxMenuItem = 14; |
#endif |
#ifdef USE_KILLAGREG |
uint8_t MaxMenuItem = 14; |
uint8_t MaxMenuItem = 16; |
#endif |
#endif |
uint8_t MenuItem = 0; |
128,7 → 129,7 |
// print menu item number in the upper right corner |
if(MenuItem < 10) |
{ |
LCD_printfxy(17,0,"[%i]",MenuItem); |
LCD_printfxy(17,0,"[%i]",MenuItem); |
} |
else |
{ |
137,112 → 138,133 |
switch(MenuItem) |
{ |
case 0:// Version Info Menu Item |
LCD_printfxy(0,0,"+ MikroKopter +"); |
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",BoardRelease/10,BoardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a'); |
#ifndef USE_QUADRO |
LCD_printfxy(0,2,"OCTO Setting: %d ", GetActiveParamSet()); |
#else |
LCD_printfxy(0,2,"QUADRO Setting: %d ", GetActiveParamSet()); |
#endif |
LCD_printfxy(0,3,"(c) Holger Buss"); |
break; |
case 1:// Height Control Menu Item |
if(ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) |
{ |
LCD_printfxy(0,0,"Height: %5i",ReadingHeight); |
LCD_printfxy(0,1,"Set Point: %5i",SetPointHeight); |
LCD_printfxy(0,2,"Air Press.:%5i",ReadingAirPressure); |
LCD_printfxy(0,3,"Offset :%5i",PressureSensorOffset); |
} |
else |
{ |
LCD_printfxy(0,1,"No "); |
LCD_printfxy(0,2,"Height Control"); |
} |
case 0:// Version Info Menu Item |
LCD_printfxy(0,0,"+ MikroKopter +"); |
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",BoardRelease/10,BoardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a'); |
LCD_printfxy(0,2,"Setting: %d %s", GetActiveParamSet(), Mixer.Name); |
if(I2CTimeout < 6) |
{ |
LCD_printfxy(0,3,"I2C Error!!!"); |
} |
else if (MissingMotor) |
{ |
LCD_printfxy(0,3,"Missing BL-Ctrl:%d", MissingMotor); |
} |
else LCD_printfxy(0,3,"(c) Holger Buss"); |
break; |
case 1:// Height Control Menu Item |
if(ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) |
{ |
LCD_printfxy(0,0,"Height: %5i",ReadingHeight); |
LCD_printfxy(0,1,"Set Point: %5i",SetPointHeight); |
LCD_printfxy(0,2,"Air Press.:%5i",ReadingAirPressure); |
LCD_printfxy(0,3,"Offset :%5i",PressureSensorOffset); |
} |
else |
{ |
LCD_printfxy(0,1,"No "); |
LCD_printfxy(0,2,"Height Control"); |
} |
break; |
break; |
case 2:// Attitude Menu Item |
LCD_printfxy(0,0,"Attitude"); |
LCD_printfxy(0,1,"Nick: %5i",IntegralGyroNick/1024); |
LCD_printfxy(0,2,"Roll: %5i",IntegralGyroRoll/1024); |
LCD_printfxy(0,3,"Heading: %5i",CompassHeading); |
break; |
case 3:// Remote Control Channel Menu Item |
LCD_printfxy(0,0,"C1:%4i C2:%4i ",PPM_in[1],PPM_in[2]); |
LCD_printfxy(0,1,"C3:%4i C4:%4i ",PPM_in[3],PPM_in[4]); |
LCD_printfxy(0,2,"C5:%4i C6:%4i ",PPM_in[5],PPM_in[6]); |
LCD_printfxy(0,3,"C7:%4i C8:%4i ",PPM_in[7],PPM_in[8]); |
break; |
case 4:// Remote Control Mapping Menu Item |
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_NICK]],PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); |
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_GAS]],PPM_in[ParamSet.ChannelAssignment[CH_YAW]]); |
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI1]],PPM_in[ParamSet.ChannelAssignment[CH_POTI2]]); |
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI3]],PPM_in[ParamSet.ChannelAssignment[CH_POTI4]]); |
break; |
case 2:// Attitude Menu Item |
LCD_printfxy(0,0,"Attitude"); |
LCD_printfxy(0,1,"Nick: %5i",IntegralGyroNick/1024); |
LCD_printfxy(0,2,"Roll: %5i",IntegralGyroRoll/1024); |
LCD_printfxy(0,3,"Heading: %5i",CompassHeading); |
break; |
case 3:// Remote Control Channel Menu Item |
LCD_printfxy(0,0,"C1:%4i C2:%4i ",PPM_in[1],PPM_in[2]); |
LCD_printfxy(0,1,"C3:%4i C4:%4i ",PPM_in[3],PPM_in[4]); |
LCD_printfxy(0,2,"C5:%4i C6:%4i ",PPM_in[5],PPM_in[6]); |
LCD_printfxy(0,3,"C7:%4i C8:%4i ",PPM_in[7],PPM_in[8]); |
break; |
case 4:// Remote Control Mapping Menu Item |
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_NICK]],PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); |
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_GAS]],PPM_in[ParamSet.ChannelAssignment[CH_YAW]]); |
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI1]],PPM_in[ParamSet.ChannelAssignment[CH_POTI2]]); |
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI3]],PPM_in[ParamSet.ChannelAssignment[CH_POTI4]]); |
break; |
case 5:// Gyro Sensor Menu Item |
LCD_printfxy(0,0,"Gyro - Sensor"); |
switch(BoardRelease) |
{ |
case 10: |
LCD_printfxy(0,1,"Nick %4i (%3i.%i)",AdValueGyroNick - BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick % HIRES_GYRO_AMPLIFY); |
LCD_printfxy(0,2,"Roll %4i (%3i.%i)",AdValueGyroRoll - BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll % HIRES_GYRO_AMPLIFY); |
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw); |
break; |
LCD_printfxy(0,0,"Gyro - Sensor"); |
switch(BoardRelease) |
{ |
case 10: |
LCD_printfxy(0,1,"Nick %4i (%3i.%i)",AdValueGyroNick - BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick % HIRES_GYRO_AMPLIFY); |
LCD_printfxy(0,2,"Roll %4i (%3i.%i)",AdValueGyroRoll - BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll % HIRES_GYRO_AMPLIFY); |
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw); |
break; |
case 11: |
case 12: |
case 20: // divice Offests by 2 becuse 2 samples are added in adc isr |
LCD_printfxy(0,1,"Nick %4i (%3i.%i)",AdValueGyroNick - BiasHiResGyroNick/HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroNick % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,2,"Roll %4i (%3i.%i)",AdValueGyroRoll - BiasHiResGyroRoll/HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroRoll % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw/2); |
break; |
case 11: |
case 12: |
case 20: // divice Offests by 2 becuse 2 samples are added in adc isr |
LCD_printfxy(0,1,"Nick %4i (%3i.%i)",AdValueGyroNick - BiasHiResGyroNick/HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroNick % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,2,"Roll %4i (%3i.%i)",AdValueGyroRoll - BiasHiResGyroRoll/HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroRoll % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw/2); |
break; |
case 13: |
default: // divice Offests by 2 becuse 2 samples are added in adc isr |
LCD_printfxy(0,1,"Nick %4i (%3i.%i)(%3i)",AdValueGyroNick - BiasHiResGyroNick/HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroNick % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroNick); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",AdValueGyroRoll - BiasHiResGyroRoll/HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroRoll % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroRoll); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw/2, DacOffsetGyroYaw ); |
break; |
} |
break; |
case 6:// Acceleration Sensor Menu Item |
LCD_printfxy(0,0,"ACC - Sensor"); |
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueAccNick/2, AdBiasAccNick/2); // factor 2 because of adding 2 samples in ADC ISR |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueAccRoll/2, AdBiasAccRoll/2); // factor 2 because of adding 2 samples in ADC ISR |
LCD_printfxy(0,3,"Height %4i (%3i)",AdValueAccTop, (int16_t)AdBiasAccTop); |
break; |
case 7:// Accumulator Voltage / Remote Control Level |
LCD_printfxy(0,1,"Voltage: %3i.%1iV",UBat/10, UBat%10); |
LCD_printfxy(0,2,"RC-Level: %5i",RC_Quality); |
break; |
case 8:// Compass Menu Item |
LCD_printfxy(0,0,"Compass "); |
LCD_printfxy(0,1,"Course: %5i",CompassCourse); |
LCD_printfxy(0,2,"Heading: %5i",CompassHeading); |
LCD_printfxy(0,3,"OffCourse: %5i",CompassOffCourse); |
break; |
case 9:// Poti Menu Item |
LCD_printfxy(0,0,"Po1: %3i Po5: %3i" ,Poti1,Poti5); //PPM24-Extesion |
LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,Poti2,Poti6); //PPM24-Extesion |
LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,Poti3,Poti7); //PPM24-Extesion |
LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,Poti4,Poti8); //PPM24-Extesion |
break; |
case 10:// Servo Menu Item |
LCD_printfxy(0,0,"Servo " ); |
LCD_printfxy(0,1,"Setpoint %3i",FCParam.ServoNickControl); |
LCD_printfxy(0,2,"Position: %3i",ServoNickValue); |
LCD_printfxy(0,3,"Range:%3i-%3i",ParamSet.ServoNickMin, ParamSet.ServoNickMax); |
break; |
case 11://Extern Control |
LCD_printfxy(0,0,"ExternControl " ); |
LCD_printfxy(0,1,"Ni:%4i Ro:%4i ",ExternControl.Nick, ExternControl.Roll); |
LCD_printfxy(0,2,"Gs:%4i Ya:%4i ",ExternControl.Gas, ExternControl.Yaw); |
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Height, ExternControl.Config); |
break; |
case 13: |
default: // divice Offests by 2 becuse 2 samples are added in adc isr |
LCD_printfxy(0,1,"Nick %4i (%3i.%i)(%3i)",AdValueGyroNick - BiasHiResGyroNick/HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroNick % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroNick); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",AdValueGyroRoll - BiasHiResGyroRoll/HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroRoll % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroRoll); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw/2, DacOffsetGyroYaw ); |
break; |
} |
break; |
case 6:// Acceleration Sensor Menu Item |
LCD_printfxy(0,0,"ACC - Sensor"); |
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueAccNick/2, AdBiasAccNick/2); // factor 2 because of adding 2 samples in ADC ISR |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueAccRoll/2, AdBiasAccRoll/2); // factor 2 because of adding 2 samples in ADC ISR |
LCD_printfxy(0,3,"Height %4i (%3i)",AdValueAccTop, (int16_t)AdBiasAccTop); |
break; |
case 7:// Accumulator Voltage / Remote Control Level |
LCD_printfxy(0,1,"Voltage: %3i.%1iV",UBat/10, UBat%10); |
LCD_printfxy(0,2,"RC-Level: %5i",RC_Quality); |
break; |
case 8:// Compass Menu Item |
LCD_printfxy(0,0,"Compass "); |
LCD_printfxy(0,1,"Course: %5i",CompassCourse); |
LCD_printfxy(0,2,"Heading: %5i",CompassHeading); |
LCD_printfxy(0,3,"OffCourse: %5i",CompassOffCourse); |
break; |
case 9:// Poti Menu Item |
LCD_printfxy(0,0,"Po1: %3i Po5: %3i" ,Poti1,Poti5); //PPM24-Extesion |
LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,Poti2,Poti6); //PPM24-Extesion |
LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,Poti3,Poti7); //PPM24-Extesion |
LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,Poti4,Poti8); //PPM24-Extesion |
break; |
case 10:// Servo Menu Item |
LCD_printfxy(0,0,"Servo " ); |
LCD_printfxy(0,1,"Setpoint %3i",FCParam.ServoNickControl); |
LCD_printfxy(0,2,"Position: %3i",ServoNickValue); |
LCD_printfxy(0,3,"Range:%3i-%3i",ParamSet.ServoNickMin, ParamSet.ServoNickMax); |
break; |
case 11://Extern Control |
LCD_printfxy(0,0,"ExternControl " ); |
LCD_printfxy(0,1,"Ni:%4i Ro:%4i ",ExternControl.Nick, ExternControl.Roll); |
LCD_printfxy(0,2,"Gs:%4i Ya:%4i ",ExternControl.Gas, ExternControl.Yaw); |
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Height, ExternControl.Config); |
break; |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
case 12://GPS Lat/Lon coords |
case 12://BL Communication errors |
LCD_printfxy(0,0,"BL-Ctrl Errors " ); |
LCD_printfxy(0,1," %3d %3d %3d %3d ",Motor[0].Error,Motor[1].Error,Motor[2].Error,Motor[3].Error); |
LCD_printfxy(0,2," %3d %3d %3d %3d ",Motor[4].Error,Motor[5].Error,Motor[6].Error,Motor[7].Error); |
LCD_printfxy(0,3," %3d %3d %3d %3d ",Motor[8].Error,Motor[9].Error,Motor[10].Error,Motor[11].Error); |
break; |
case 13://BL Overview |
LCD_printfxy(0,0,"BL-Ctrl found " ); |
LCD_printfxy(0,1," %c %c %c %c ",Motor[0].Present + '-',Motor[1].Present + '-',Motor[2].Present + '-',Motor[3].Present + '-'); |
LCD_printfxy(0,2," %c %c %c %c ",Motor[4].Present + '-',Motor[5].Present + '-',Motor[6].Present + '-',Motor[7].Present + '-'); |
LCD_printfxy(0,3," %c - - - ",Motor[8].Present + '-'); |
if(Motor[9].Present) LCD_printfxy(4,3,"10"); |
if(Motor[10].Present) LCD_printfxy(8,3,"11"); |
if(Motor[11].Present) LCD_printfxy(12,3,"12"); |
break; |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
case 14://GPS Lat/Lon coords |
if (GPSInfo.status == INVALID) |
{ |
LCD_printfxy(0,0,"No GPS data!"); |
268,25 → 290,25 |
i1 = (int16_t)(GPSInfo.longitude/10000000L); |
i2 = abs((int16_t)((GPSInfo.longitude%10000000L)/10000L)); |
i3 = abs((int16_t)(((GPSInfo.longitude%10000000L)%10000L)/10L)); |
LCD_printfxy(0,1,"Lon: %d.%.3d%.3d deg",i1, i2, i3); |
LCD_printfxy(0,1,"Lon: %d.%03d%03d deg",i1, i2, i3); |
i1 = (int16_t)(GPSInfo.latitude/10000000L); |
i2 = abs((int16_t)((GPSInfo.latitude%10000000L)/10000L)); |
i3 = abs((int16_t)(((GPSInfo.latitude%10000000L)%10000L)/10L)); |
LCD_printfxy(0,2,"Lat: %d.%.3d%.3d deg",i1, i2, i3); |
LCD_printfxy(0,2,"Lat: %d.%03d%03d deg",i1, i2, i3); |
i1 = (int16_t)(GPSInfo.altitude/1000L); |
i2 = abs((int16_t)(GPSInfo.altitude%1000L)); |
LCD_printfxy(0,3,"Alt: %d.%.3d m",i1, i2); |
LCD_printfxy(0,3,"Alt: %d.%03d m",i1, i2); |
} |
break; |
#endif |
#ifdef USE_KILLAGREG |
case 13:// MM3 Kompass |
case 15:// MM3 Kompass |
LCD_printfxy(0,0,"MM3 Offset"); |
LCD_printfxy(0,1,"X_Offset: %3i",MM3_calib.X_off); |
LCD_printfxy(0,2,"Y_Offset: %3i",MM3_calib.Y_off); |
LCD_printfxy(0,3,"Z_Offset: %3i",MM3_calib.Z_off); |
break; |
case 14://MM3 Range |
case 16://MM3 Range |
LCD_printfxy(0,0,"MM3 Range"); |
LCD_printfxy(0,1,"X_Range: %4i",MM3_calib.X_range); |
LCD_printfxy(0,2,"Y_Range: %4i",MM3_calib.Y_range); |
294,9 → 316,10 |
break; |
#endif |
default: MaxMenuItem = MenuItem - 1; |
MenuItem = 0; |
break; |
} |
RemoteKeys = 0; |
default: |
MaxMenuItem = MenuItem - 1; |
MenuItem = 0; |
break; |
} |
RemoteKeys = 0; |
} |
/branches/V0.73d Code Redesign killagreg/spectrum.c |
---|
2,10 → 2,119 |
Decodieren eines RC Summen Signals oder Spektrum Empfänger-Satellit |
#######################################################################################*/ |
#include "spectrum.h" |
#include rc.h |
#include "Spectrum.h" |
#include "main.h" |
//--------------------------------------------------------------// |
//--------------------------------------------------------------// |
void SpektrumBinding(void) |
{ |
unsigned int timerTimeout = SetDelay(10000); // Timeout 10 sec. |
unsigned char connected = 0; |
unsigned int delaycounter; |
UCSR1B &= ~(1 << RXCIE1); // disable rx-interrupt |
UCSR1B &= ~(1<<RXEN1); // disable Uart-Rx |
PORTD &= ~(1 << PORTD2); // disable pull-up |
printf("\n\rPlease connect Spektrum receiver for binding NOW..."); |
while(!CheckDelay(timerTimeout)) |
{ |
if (PIND & (1 << PORTD2)) { timerTimeout = SetDelay(90); connected = 1; break; } |
} |
if (connected) |
{ |
printf("ok.\n\r"); |
DDRD |= (1 << DDD2); // Rx as output |
while(!CheckDelay(timerTimeout)); // delay after startup of RX |
for (delaycounter = 0; delaycounter < 100; delaycounter++) PORTD |= (1 << PORTD2); |
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2); |
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2); |
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2); |
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2); |
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2); |
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2); |
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2); |
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2); |
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2); |
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2); |
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2); |
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2); |
} |
else |
{ printf("Timeout.\n\r"); |
} |
DDRD &= ~(1 << DDD2); // RX as input |
PORTD &= ~(1 << PORTD2); |
Uart1Init(); // init Uart again |
} |
//############################################################################ |
// zum Decodieren des Spektrum Satelliten wird USART1 benutzt. |
// USART1 initialisation from killagreg |
void Uart1Init(void) |
//############################################################################ |
{ |
// -- Start of USART1 initialisation for Spekturm seriell-mode |
// USART1 Control and Status Register A, B, C and baud rate register |
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * 115200) - 1); |
// disable RX-Interrupt |
UCSR1B &= ~(1 << RXCIE1); |
// disable TX-Interrupt |
UCSR1B &= ~(1 << TXCIE1); |
// disable DRE-Interrupt |
UCSR1B &= ~(1 << UDRIE1); |
// set direction of RXD1 and TXD1 pins |
// set RXD1 (PD2) as an input pin |
PORTD |= (1 << PORTD2); |
DDRD &= ~(1 << DDD2); |
// USART0 Baud Rate Register |
// set clock divider |
UBRR1H = (uint8_t)(ubrr>>8); |
UBRR1L = (uint8_t)ubrr; |
// enable double speed operation |
UCSR1A |= (1 << U2X1); |
// enable receiver and transmitter |
//UCSR1B = (1<<RXEN1)|(1<<TXEN1); |
UCSR1B = (1<<RXEN1); |
// set asynchronous mode |
UCSR1C &= ~(1 << UMSEL11); |
UCSR1C &= ~(1 << UMSEL10); |
// no parity |
UCSR1C &= ~(1 << UPM11); |
UCSR1C &= ~(1 << UPM10); |
// 1 stop bit |
UCSR1C &= ~(1 << USBS1); |
// 8-bit |
UCSR1B &= ~(1 << UCSZ12); |
UCSR1C |= (1 << UCSZ11); |
UCSR1C |= (1 << UCSZ10); |
// flush receive buffer explicit |
while(UCSR1A & (1<<RXC1)) UDR1; |
// enable RX-interrupts at the end |
UCSR1B |= (1 << RXCIE1); |
// -- End of USART1 initialisation |
return; |
} |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) Rainer Walther |
// + RC-routines from original MK rc.c (c) H&I |
53,7 → 162,7 |
// byte9: and byte10: channel data |
// byte11: and byte12: channel data |
// byte13: and byte14: channel data |
// byte15: and byte16: channel data |
// byte15: and byte16: channel data |
// 2nd Frame: |
// byte1: unknown |
// byte2: unknown |
76,20 → 185,24 |
// |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
void spectrum_parser(uint8_t c) |
//############################################################################ |
//Diese Routine startet und inizialisiert den USART1 für seriellen Spektrum satellite reciever |
SIGNAL(USART1_RX_vect) |
//############################################################################ |
{ |
static unsigned int Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0, FrameTimer; |
static unsigned int Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0, FrameTimer; |
unsigned int Channel, index; |
signed int signal, tmp; |
int bCheckDelay; |
uint8_t c; |
c = UDR1; // get data byte |
if (ReSync == 1) |
{ |
// wait for beginning of new frame |
ReSync = 0; |
FrameTimer = SetDelay(7); // minimum 7ms zwischen den frames |
FrameCnt = 0; |
Sync = 0; |
96,7 → 209,7 |
ByteHigh = 0; |
} |
else |
{ |
{ |
bCheckDelay = CheckDelay(FrameTimer); |
if ( Sync == 0 ) |
{ |
123,7 → 236,7 |
{ |
// Datenbyte high |
ByteHigh = c; |
if (FrameCnt == 2) |
{ |
// is 1st Byte of Channel-data |
134,7 → 247,7 |
// DS9: Frame 2 with Channel 8-9 comming next |
Frame2 = 1; |
} |
} |
} |
Sync = 3; |
FrameCnt ++; |
} |
141,11 → 254,11 |
else if((Sync == 3) && !bCheckDelay) |
{ |
// Datenbyte low |
// High-Byte for next channel comes next |
Sync = 2; |
FrameCnt ++; |
index = (ByteHigh >> 2) & 0x0f; |
index ++; |
Channel = (ByteHigh << 8) | c; |
152,15 → 265,15 |
signal = Channel & 0x3ff; |
signal -= 0x200; // Offset, range 0x000..0x3ff? |
signal = signal/3; // scaling to fit PPM resolution |
if(index >= 0 && index <= 10) |
{ |
// Stabiles Signal |
if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;} |
tmp = (3 * (PPM_in[index]) + signal) / 4; |
tmp = (3 * (PPM_in[index]) + signal) / 4; |
if(tmp > signal+1) tmp--; else |
if(tmp < signal-1) tmp++; |
if(SenderOkay >= 180) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; |
if(tmp < signal-1) tmp++; |
if(SenderOkay >= 180) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; |
else PPM_diff[index] = 0; |
PPM_in[index] = tmp; |
} |
172,7 → 285,7 |
FrameCnt = 0; |
Frame2 = 0; |
} |
// 16 Bytes per frame |
if(FrameCnt >= 16) |
{ |
181,9 → 294,9 |
{ |
// Null bedeutet: Neue Daten |
// nur beim ersten Frame (CH 0-7) setzen |
NewPpmData = 0; |
NewPpmData = 0; |
} |
// new frame next, nach fruehestens 7ms erwartet |
FrameCnt = 0; |
Frame2 = 0; |
192,6 → 305,6 |
// Zeit bis zum nächsten Zeichen messen |
FrameTimer = SetDelay(7); |
} |
} |
} |
/branches/V0.73d Code Redesign killagreg/spectrum.h |
---|
1,10 → 1,9 |
/*####################################################################################### |
Dekodieren eines Spectrum Signals |
#######################################################################################*/ |
#ifndef _SPECTRUM_H |
#define _SPECTRUM_H |
#include <inttypes.h> |
#define USART1_BAUD 115200 |
// this function should be called within the UART RX ISR |
extern void spectrum_parser(uint8_t c); |
#endif //_SPECTRUM_H |
void Uart1Init(void); |
void SpektrumBinding(void); |
#endif //_RC_H |
/branches/V0.73d Code Redesign killagreg/spi.c |
---|
152,7 → 152,9 |
uint8_t SPI_TxBufferIndex = 0; |
uint8_t SPITransferCompleted, SPI_ChkSum; |
uint8_t SPI_RxDataValid; |
uint8_t SPI_RxDataValid = 0; |
uint8_t NCDataOkay = 0; |
uint8_t NCSerialDataOkay = 0; |
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION }; |
uint8_t SPI_CommandCounter = 0; |
179,6 → 181,8 |
ToNaviCtrl.Command = SPI_CMD_USER; |
ToNaviCtrl.IntegralNick = 0; |
ToNaviCtrl.IntegralRoll = 0; |
NCSerialDataOkay = 0; |
NCDataOkay = 0; |
SPI_RxDataValid = 0; |
294,6 → 298,7 |
{ |
GPSStickNick = FromNaviCtrl.GPSStickNick; |
GPSStickRoll = FromNaviCtrl.GPSStickRoll; |
NCDataOkay = 250; |
} |
// update compass readings |
if(FromNaviCtrl.CompassHeading <= 360) |
311,6 → 316,8 |
FCParam.KalmanK = FromNaviCtrl.Param.Byte[0]; |
FCParam.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1]; |
FCParam.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2]; |
NCSerialDataOkay = FromNaviCtrl.Param.Byte[3]; |
DebugOut.Analog[29] = NCSerialDataOkay; |
break; |
default: |
/branches/V0.73d Code Redesign killagreg/spi.h |
---|
78,6 → 78,19 |
extern ToNaviCtrl_t ToNaviCtrl; |
extern FromNaviCtrl_t FromNaviCtrl; |
typedef struct |
{ |
int8_t KalmanK; |
int8_t KalmanMaxDrift; |
int8_t KalmanMaxFusion; |
uint8_t SerialDataOkay; |
} __attribute__((packed)) NCData_t; |
extern uint8_t NCDataOkay; |
extern uint8_t NCSerialDataOkay; |
void SPI_MasterInit(void); |
void SPI_StartTransmitPacket(void); |
void SPI_TransmitByte(void); |
/branches/V0.73d Code Redesign killagreg/twimaster.c |
---|
51,46 → 51,34 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <util/twi.h> |
#include "main.h" |
#include "eeprom.h" |
#include "twimaster.h" |
#include "fc.h" |
#include "analog.h" |
volatile uint8_t twi_state = 0; |
uint8_t motor_write = 0; |
uint8_t motor_read = 0; |
volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
volatile uint8_t dac_channel = 0; |
volatile uint8_t motor_write = 0; |
volatile uint8_t motor_read = 0; |
#ifdef USE_QUADRO |
uint8_t motor_rx[8]; |
#else |
uint8_t motor_rx[16]; |
#endif |
volatile uint16_t I2CTimeout = 100; |
uint8_t MissingMotor = 0; |
MotorData_t Motor[MAX_MOTORS]; |
#define SCL_CLOCK 200000L |
#define I2C_TIMEOUT 30000 |
#define TWSR_STATUS_MASK 0xF8 |
// for Master Transmitter Mode |
#define I2C_STATUS_START 0x08 |
#define I2C_STATUS_REPEATSTART 0x10 |
#define I2C_STATUS_TX_SLA_ACK 0x18 |
#define I2C_STATUS_SLAW_NOACK 0x20 |
#define I2C_STATUS_TX_DATA_ACK 0x28 |
#define I2C_STATUS_TX_DATA_NOTACK 0x30 |
#define I2C_STATUS_RX_DATA_ACK 0x50 |
#define I2C_STATUS_RX_DATA_NOTACK 0x58 |
/**************************************************/ |
/* Initialize I2C (TWI) */ |
/**************************************************/ |
void I2C_Init(void) |
{ |
uint8_t i; |
uint8_t sreg = SREG; |
cli(); |
108,10 → 96,18 |
// set TWI Bit Rate Register |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
twi_state = 0; |
twi_state = TWI_STATE_MOTOR_TX; |
motor_write = 0; |
motor_read = 0; |
for(i=0; i < MAX_MOTORS; i++) |
{ |
Motor[i].SetPoint = 0; |
Motor[i].Present = 0; |
Motor[i].Error = 0; |
Motor[i].MaxPWM = 0; |
} |
SREG = sreg; |
} |
118,8 → 114,9 |
/****************************************/ |
/* Start I2C */ |
/****************************************/ |
void I2C_Start(void) |
void I2C_Start(uint8_t start_state) |
{ |
twi_state = start_state; |
// TWI Control Register |
// clear TWI interrupt flag (TWINT=1) |
// disable TWI Acknowledge Bit (TWEA = 0) |
134,8 → 131,9 |
/****************************************/ |
/* Stop I2C */ |
/****************************************/ |
void I2C_Stop(void) |
void I2C_Stop(uint8_t start_state) |
{ |
twi_state = start_state; |
// TWI Control Register |
// clear TWI interrupt flag (TWINT=1) |
// disable TWI Acknowledge Bit (TWEA = 0) |
153,12 → 151,12 |
/****************************************/ |
void I2C_WriteByte(int8_t byte) |
{ |
// move byte to send into TWI Data Register |
TWDR = byte; |
// clear interrupt flag (TWINT = 1) |
// enable i2c bus (TWEN = 1) |
// enable interrupt (TWIE = 1) |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
// move byte to send into TWI Data Register |
TWDR = byte; |
// clear interrupt flag (TWINT = 1) |
// enable i2c bus (TWEN = 1) |
// enable interrupt (TWIE = 1) |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
} |
167,7 → 165,7 |
/****************************************/ |
void I2C_ReceiveByte(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
} |
/****************************************/ |
175,7 → 173,7 |
/****************************************/ |
void I2C_ReceiveLastByte(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
} |
185,7 → 183,7 |
void I2C_Reset(void) |
{ |
// stop i2c bus |
I2C_Stop(); |
I2C_Stop(TWI_STATE_MOTOR_TX); |
twi_state = 0; |
motor_write = TWDR; |
motor_write = 0; |
197,8 → 195,7 |
TWSR = 0; |
TWBR = 0; |
I2C_Init(); |
I2C_Start(); |
I2C_WriteByte(0); |
I2C_Start(TWI_STATE_MOTOR_TX); |
} |
205,199 → 202,68 |
/****************************************/ |
/* I2C ISR */ |
/****************************************/ |
#ifdef USE_QUADRO |
ISR (TWI_vect) |
{ |
static uint8_t missing_motor = 0; |
switch (twi_state++) // First i2c_start from SendMotorData() |
{ |
// Master Transmit |
case 0: // Send SLA-W |
I2C_WriteByte(0x52 + (motor_write * 2) ); |
case 0: // TWI_STATE_MOTOR_TX |
// skip motor if not used in mixer |
while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
if(motor_write >= MAX_MOTORS) // writing finished, read now |
{ |
motor_write = 0; |
twi_state = TWI_STATE_MOTOR_RX; |
I2C_WriteByte(0x53 + (motor_read * 2) ); // select slave adress in rx mode |
} |
else I2C_WriteByte(0x52 + (motor_write * 2) ); // select slave adress in tx mode |
break; |
case 1: // Send Data to Slave |
switch(motor_write) |
{ |
case 0: |
I2C_WriteByte(Motor1); |
break; |
case 1: |
I2C_WriteByte(Motor2); |
break; |
case 2: |
I2C_WriteByte(Motor3); |
break; |
case 3: |
I2C_WriteByte(Motor4); |
break; |
} |
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit rotation rate setpoint |
break; |
case 2: // repeat case 0+1 for all motors |
I2C_Stop(); |
if (motor_write < 3) |
{ |
motor_write++; // jump to next motor |
twi_state = 0; // and repeat from state 0 |
} |
else |
{ // data to last motor send |
motor_write = 0; // reset motor write counter |
} |
I2C_Start(); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
break; |
// Master Receive |
case 3: // Send SLA-R |
I2C_WriteByte(0x53 + (motor_read * 2) ); |
break; |
case 4: |
//Transmit 1st byte |
I2C_ReceiveByte(); |
break; |
case 5: //Read 1st byte and transmit 2nd Byte |
motor_rx[motor_read] = TWDR; |
I2C_ReceiveLastByte(); |
break; |
case 6: |
//Read 2nd byte |
motor_rx[motor_read + 4] = TWDR; |
motor_read++; |
if (motor_read > 3) motor_read = 0; |
I2C_Stop(); |
twi_state = 0; |
I2CTimeout = 10; |
break; |
// Gyro-Offsets |
case 7: |
I2C_WriteByte(0x98); // Address the DAC |
break; |
case 8: |
I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
break; |
case 9: |
switch(dac_channel) |
if(TWSR == TW_MT_DATA_NACK) // Data transmitted, NACK received |
{ |
case 0: |
I2C_WriteByte(DacOffsetGyroNick); // 1st byte for Channel A |
break; |
case 1: |
I2C_WriteByte(DacOffsetGyroRoll); // 1st byte for Channel B |
break; |
case 2: |
I2C_WriteByte(DacOffsetGyroYaw ); // 1st byte for Channel C |
break; |
if(!missing_motor) missing_motor = motor_write + 1; |
if(++Motor[motor_write].Error == 0) Motor[motor_write].Error = 255; // increment error counter and handle overflow |
} |
break; |
case 10: |
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
break; |
case 11: |
I2C_Stop(); |
I2CTimeout = 10; |
// repeat case 7...10 until all DAC Channels are updated |
if(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
twi_state = 7; // and repeat from state 7 |
I2C_Start(); // start transmission for next channel |
} |
else |
{ // data to last motor send |
dac_channel = 0; // reset dac channel counter |
twi_state = 0; // reset twi_state |
} |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write++; // next motor |
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
break; |
default: |
I2C_Stop(); |
twi_state = 0; |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
} |
} |
#else // USE_OCTO, USE_OCTO2, USE_OCTO3 |
ISR (TWI_vect) |
{ |
switch (twi_state++) // First i2c_start from SendMotorData() |
{ |
// Master Transmit |
case 0: // Send SLA-W |
I2C_WriteByte(0x52 + (motor_write * 2) ); |
break; |
case 1: // Send Data to Slave |
switch(motor_write) |
{ |
case 0: |
I2C_WriteByte(Motor1); |
break; |
case 1: |
I2C_WriteByte(Motor2); |
break; |
case 2: |
I2C_WriteByte(Motor3); |
break; |
case 3: |
I2C_WriteByte(Motor4); |
break; |
case 5: |
I2C_WriteByte(Motor5); |
break; |
case 6: |
I2C_WriteByte(Motor6); |
break; |
case 7: |
I2C_WriteByte(Motor7); |
break; |
case 8: |
I2C_WriteByte(Motor8); |
break; |
} |
break; |
case 2: // repeat case 0+1 for all motors |
I2C_Stop(); |
if (motor_write < 7) |
{ |
motor_write++; // jump to next motor |
twi_state = 0; // and repeat from state 0 |
// Master Receive Data |
case 3: |
if(TWSR != TW_MR_SLA_ACK) // SLA+R transmitted, if not ACK received |
{ // no response from the addressed slave received |
Motor[motor_read].Present = 0; |
motor_read++; // next motor |
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
I2C_Stop(TWI_STATE_MOTOR_TX); |
} |
else |
{ // data to last motor send |
motor_write = 0; // reset motor write counter |
{ |
Motor[motor_read].Present = ('1' - '-') + motor_read; |
I2C_ReceiveByte(); //Transmit 1st byte |
} |
I2C_Start(); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
MissingMotor = missing_motor; |
missing_motor = 0; |
break; |
// Master Receive |
case 3: // Send SLA-R |
I2C_WriteByte(0x53 + (motor_read * 2) ); |
break; |
case 4: |
//Transmit 1st byte |
I2C_ReceiveByte(); |
break; |
case 5: //Read 1st byte and transmit 2nd Byte |
motor_rx[motor_read] = TWDR; |
I2C_ReceiveLastByte(); |
case 4: //Read 1st byte and transmit 2nd Byte |
Motor[motor_read].Current = TWDR; |
I2C_ReceiveLastByte(); // nack |
break; |
case 6: |
case 5: |
//Read 2nd byte |
motor_rx[motor_read + 8] = TWDR; |
motor_read++; |
if (motor_read > 7) motor_read = 0; |
I2C_Stop(); |
twi_state = 0; |
I2CTimeout = 10; |
Motor[motor_read].MaxPWM = TWDR;; |
motor_read++; // next motor |
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
I2C_Stop(TWI_STATE_MOTOR_TX); |
break; |
// Gyro-Offsets |
// writing Gyro-Offsets |
case 7: |
I2C_WriteByte(0x98); // Address the DAC |
break; |
426,29 → 292,24 |
break; |
case 11: |
I2C_Stop(); |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
// repeat case 7...10 until all DAC Channels are updated |
if(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
twi_state = 7; // and repeat from state 7 |
I2C_Start(); // start transmission for next channel |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
} |
else |
{ // data to last motor send |
dac_channel = 0; // reset dac channel counter |
twi_state = 0; // reset twi_state |
} |
break; |
default: |
I2C_Stop(); |
twi_state = 0; |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
} |
} |
#endif // USE_OCTO, USE_OCTO2 |
/branches/V0.73d Code Redesign killagreg/twimaster.h |
---|
1,4 → 1,3 |
#ifndef _I2C_MASTER_H |
#define _I2C_MASTER_H |
@@ -6,21 +5,33 @@ |
#include <inttypes.h> |
#define TWI_STATE_MOTOR_TX 0 |
+#define TWI_STATE_MOTOR_RX 3 |
#define TWI_STATE_GYRO_OFFSET_TX 7 |
extern volatile uint8_t twi_state; |
+extern volatile uint8_t motor_write; |
+extern volatile uint8_t motor_read; |
-#ifdef USE_QUADRO |
-extern uint8_t motor_rx[8]; |
-#else |
-extern uint8_t motor_rx[16]; |
-#endif |
+extern uint8_t MissingMotor; |
+#define MAX_MOTORS 12 |
+ |
+typedef struct |
+{ |
+ uint8_t SetPoint; // written by attitude controller |
+ uint8_t Present; // 0 if BL was found |
+ uint8_t Error; // I2C error counter |
+ uint8_t Current; // read byck from BL |
+ uint8_t MaxPWM; // read back from BL |
+} __attribute__((packed)) MotorData_t; |
+ |
+extern MotorData_t Motor[MAX_MOTORS]; |
+ |
extern volatile uint16_t I2CTimeout; |
extern void I2C_Init (void); // Initialize I2C |
-extern void I2C_Start(void); // Start I2C |
-extern void I2C_Stop (void); // Stop I2C |
+extern void I2C_Start(uint8_t start_state); // Start I2C |
+extern void I2C_Stop (uint8_t start_state); // Stop I2C |
extern void I2C_Reset(void); // Reset I2C |
#endif |
/branches/V0.73d Code Redesign killagreg/uart0.c |
---|
97,7 → 97,9 |
volatile uint8_t RxDataLen = 0; |
uint8_t PcAccess = 100; |
uint8_t MotorTest[4] = {0,0,0,0}; |
uint8_t MotorTest_Active = 0; |
uint8_t MotorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; |
uint8_t ConfirmFrame; |
typedef struct |
152,7 → 154,7 |
" ", |
"Kalman Max Drift", |
" ", |
" ", |
"Navi Serial Data", |
"GPS Nick ", //30 |
"GPSS Roll " |
}; |
485,11 → 487,39 |
#endif |
case 't':// motor test |
memcpy(&MotorTest[0], (uint8_t*)pRxData, sizeof(MotorTest)); |
if(RxDataLen > 20) // |
{ |
memcpy(&MotorTest[0], (uint8_t*)pRxData, sizeof(MotorTest)); |
} |
else |
{ |
memcpy(&MotorTest[0], (uint8_t*)pRxData, 4); |
} |
//Request_MotorTest = TRUE; |
MotorTest_Active = 255; |
PcAccess = 255; |
break; |
case 'n':// "Get Mixer Table |
while(!txd_complete); // wait for previous frame to be sent |
SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer)); |
break; |
case 'm':// "Set Mixer Table |
if(pRxData[0] == EEMIXER_REVISION) |
{ |
memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer)); |
MixerTable_WriteToEEProm(); |
while(!txd_complete); // wait for previous frame to be sent |
tempchar1 = 1; |
} |
else |
{ |
tempchar1 = 0; |
} |
SendOutData('M', FC_ADDRESS, 1, &tempchar1, 1); |
break; |
case 'p': // get PPM channels |
Request_PPMChannels = TRUE; |
break; |
504,9 → 534,8 |
else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5 |
// load requested parameter set |
ParamSet_ReadFromEEProm(pRxData[0]); |
tempchar1 = pRxData[0]; |
tempchar2 = EEPARAM_VERSION; |
tempchar2 = EEPARAM_REVISION; |
while(!txd_complete); // wait for previous frame to be sent |
SendOutData('Q', FC_ADDRESS,3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (uint8_t *) &ParamSet, sizeof(ParamSet)); |
break; |
514,7 → 543,7 |
case 's': // save settings |
if(!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors ar off |
{ |
if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_VERSION)) // check for setting to be in range and version of settings |
if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION)) // check for setting to be in range and version of settings |
{ |
memcpy(&ParamSet, (uint8_t*)&pRxData[2], sizeof(ParamSet)); |
ParamSet_WriteToEEProm(pRxData[0]); |
680,6 → 709,7 |
Compass_Timer = SetDelay(99); |
} |
#endif |
if(Request_MotorTest && txd_complete) |
{ |
SendOutData('T', FC_ADDRESS, 0); |
/branches/V0.73d Code Redesign killagreg/uart0.h |
---|
19,8 → 19,10 |
extern uint8_t PcAccess; |
extern uint8_t RemotePollDisplayLine; |
extern uint8_t MotorTest[4]; |
extern uint8_t MotorTest_Active; |
extern uint8_t MotorTest[16]; |
typedef struct |
{ |
uint8_t Digital[2]; |
/branches/V0.73d Code Redesign killagreg/version.txt |
---|
236,14 → 236,30 |
0.72p: H.Buss 01.03.2009 |
- Octo3 erstellt |
- Analogwerte umbenannt |
0.73a: H.Buss 12.03.2009 |
- MixerTabelle implementiert |
Anpassungen bzgl. V0.72p |
G.Stobrawa 01.03.2009: |
0.73b: H.Buss 14.03.2009 |
- Es wird geprüft, ob alle notwendigen BL-Regler angeschlossen sind |
0.73a-d: H.Buss 05.04.2009 |
- MixerTabelle implementiert |
- I2C-Bus auf bis zu 12 Motoren erweitert |
- die Busfehler der BL-Regler werden im Menü angezeigt |
- Revision der MixerTabelle eingeführt |
- MixerTabelle wird bei Parameterreset neu initialisiert |
- Motortest auf [12] erweitert |
- Motorschalter nicht mehr 3-Stufig |
Anpassungen bzgl. V0.73d |
G.Stobrawa 6.04.2009: |
- Code stärker modularisiert und restrukturiert |
- viele Kommentare zur Erklärug eingefügt |
- konsequent englische Variablennamen |
- PPM24 Support für bis zu 12 RC-Kanäle. |
- Suport for DSL Receiver at 2nd UART |
- Makefile: EXT=NAVICTRL Unterstützung der SPI Communikation zum Naviboard |
256,9 → 272,6 |
- GPS-Home-Funktion hinzugefügt (wird beim Motorstart gelernt, und bei Motorenstop wieder gelöscht) |
- Zusätzliche Punkte im Menü des KopterTool zur Anzeige des GPS-Status und der MM3-Kalibierparameter |
- Makefile: SETUP = QUADRO für Quadrokopter |
- Makefile: SETUP = OCTO für Oktokopter |
Weiter Detail siehe Readme.txt im Verzeichnis Hex-Files. |