Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1968 → Rev 1969

/branches/dongfang_FC_rewrite/Flight-Ctrl_MEGA644p_NAVICTRL__ADXRS610_FC2.0_V0_74df_SVN0001.hex
File deleted
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
3,6 → 3,7
#include "analog.h"
#include "twimaster.h"
#include "configuration.h"
#include "eeprom.h"
#include "timer0.h"
 
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510
/branches/dongfang_FC_rewrite/analog.c
73,6 → 73,8
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
#define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
 
const char* recal = ", recalibration needed.";
 
/*
* For each A/D conversion cycle, each analog channel is sampled a number of times
* (see array channelsForStates), and the results for each channel are summed.
103,9 → 105,9
* to be centered on zero.
*/
 
volatile sensorOffset_t gyroOffset;
volatile sensorOffset_t accOffset;
volatile sensorOffset_t gyroAmplifierOffset;
sensorOffset_t gyroOffset;
sensorOffset_t accOffset;
sensorOffset_t gyroAmplifierOffset;
 
/*
* This allows some experimentation with the gyro filters.
342,10 → 344,6
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
else
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
 
debugOut.analog[3] = rawGyroSum[0];
debugOut.analog[4] = rawGyroSum[1];
debugOut.analog[5] = rawGyroSum[2];
}
 
void analog_updateAccelerometers(void) {
473,12 → 471,13
 
void analog_setNeutral() {
if (gyroAmplifierOffset_readFromEEProm()) {
printf("gyro amp invalid, you must recalibrate.");
printf("gyro amp invalid%s",recal);
gyro_loadOffsets(1);
}
} else
gyro_loadOffsets(0);
 
if (gyroOffset_readFromEEProm()) {
printf("gyro offsets invalid, you must recalibrate.");
printf("gyro offsets invalid%s",recal);
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL;
gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW;
}
487,7 → 486,7
debugOut.analog[7] = gyroOffset.offsets[ROLL];
 
if (accOffset_readFromEEProm()) {
printf("acc. meter offsets invalid, you must recalibrate.");
printf("acc. meter offsets invalid%s",recal);
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL;
accOffset.offsets[Z] = 512 * ACC_SUMMATION_FACTOR_Z;
}
/branches/dongfang_FC_rewrite/analog.h
185,9 → 185,9
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
 
extern volatile sensorOffset_t gyroOffset;
extern volatile sensorOffset_t accOffset;
extern volatile sensorOffset_t gyroAmplifierOffset;
extern sensorOffset_t gyroOffset;
extern sensorOffset_t accOffset;
extern sensorOffset_t gyroAmplifierOffset;
 
/*
* This is not really for external use - but the ENC-03 gyro modules needs it.
/branches/dongfang_FC_rewrite/commands.c
82,6 → 82,7
setActiveParamSet(argument);
}
paramSet_readFromEEProm(getActiveParamSet());
analog_calibrateGyros();
attitude_setNeutral();
flight_setNeutral();
controlMixer_setNeutral();
/branches/dongfang_FC_rewrite/configuration.c
55,12 → 55,12
#include "configuration.h"
#include "sensors.h"
#include "rc.h"
 
int16_t variables[VARIABLE_COUNT];
paramset_t staticParams;
channelMap_t channelMap;
mixerMatrix_t mixerMatrix;
dynamicParam_t dynamicParams;
volatile dynamicParam_t dynamicParams;
 
uint8_t CPUType = ATMEGA644;
uint8_t boardRelease = 13;
uint8_t requiredMotors;
/branches/dongfang_FC_rewrite/configuration.h
43,7 → 43,7
uint8_t motorSmoothing;
} dynamicParam_t;
 
extern dynamicParam_t dynamicParams;
extern volatile dynamicParam_t dynamicParams;
 
typedef struct {
uint8_t sourceIdx, targetIdx;
/branches/dongfang_FC_rewrite/eeprom.c
111,7 → 111,8
// length is the length of the pure data not including checksum and revision number.
void writeChecksummedBlock(uint8_t revisionNumber, uint8_t* data, uint16_t offset, uint16_t length) {
uint16_t CRC = CRC16(data, length);
eeprom_write_word(&EEPromArray[offset], CRC);
eeprom_write_byte(&EEPromArray[offset], CRC&0xff);
eeprom_write_byte(&EEPromArray[offset+1], CRC>>8);
eeprom_write_byte(&EEPromArray[offset+2], revisionNumber);
eeprom_write_block(data, &EEPromArray[offset+3], length);
}
119,10 → 120,10
// offset is where the checksum is stored, offset+1 is the revision number, and offset+2... are the data.
// length is the length of the pure data not including checksum and revision number.
uint8_t readChecksummedBlock(uint8_t revisionNumber, uint8_t* target, uint16_t offset, uint16_t length) {
uint16_t CRCRead = eeprom_read_word(&EEPromArray[offset]);
uint16_t CRCRead = eeprom_read_byte(&EEPromArray[offset]) | (eeprom_read_byte(&EEPromArray[offset+1])<<8);
uint8_t revisionNumberRead = eeprom_read_byte(&EEPromArray[offset+2]);
eeprom_read_block(target, &EEPromArray[offset+3], length);
uint16_t CRCCalculated = CRC(target, length);
uint16_t CRCCalculated = CRC16(target, length);
uint8_t CRCError = (CRCRead != CRCCalculated);
uint8_t revisionMismatch = (revisionNumber != revisionNumberRead);
130,7 → 131,7
if (CRCError && revisionMismatch) printf("\n\rEEPROM CRC error and revision mismatch; ");
else if (CRCError) printf("\n\rEEPROM CRC error; ");
else if (revisionMismatch) printf("\n\rEEPROM revision mismatch; ");
return (checksumError || revisionMismatch);
return (CRCError || revisionMismatch);
}
 
/***************************************************/
159,7 → 160,7
// parameter version check
if (setnumber<1 ||setnumber>5 || paramSet_readFromEEProm(setnumber)) {
// if version check faild
printf("writing default parameter sets");
printf("\n\rwriting default parameter sets");
for (uint8_t i=5; i>0; i--) {
paramSet_default(i);
paramSet_writeToEEProm(i);
174,10 → 175,6
/***************************************************/
/* MixerTable */
/***************************************************/
uint8_t mixerMatrix_readFromEEProm(void) {
return readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t));
}
 
void mixerMatrix_writeToEEProm(void) {
writeChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t));
}
184,7 → 181,7
 
void mixerMatrix_readOrDefault(void) {
// load mixer table
if (mixerMatrix_readFromEEProm()) {
if (readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t))) {
printf("writing default mixerMatrix");
mixerMatrix_default(); // Quadro
mixerMatrix_writeToEEProm();
196,23 → 193,19
requiredMotors++;
}
printf("\n\r\rMixer-Config: '%s' (%u Motors)",mixerMatrix.name, requiredMotors);
printf("\n\r\r==============================");
printf("\n\rMixer-Config: '%s' (%u Motors)",mixerMatrix.name, requiredMotors);
printf("\n\r===================================");
}
 
/***************************************************/
/* ChannelMap */
/***************************************************/
uint8_t channelMap_readFromEEProm(void) {
return readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t));
}
 
void channelMap_writeToEEProm(void) {
writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t));
}
 
void channelMap_readOrDefault(void) {
if (channelMap_readFromEEProm()) {
if (readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t))) {
printf("writing default channel map");
channelMap_default();
channelMap_writeToEEProm();
/branches/dongfang_FC_rewrite/eeprom.h
8,24 → 8,24
#define EEPROM_ADR_PARAM_BEGIN 0
#define EEPROM_CHECKSUMMED_BLOCK_OVERHEAD 3
 
#define PID_ACTIVE_SET 0 // byte
#define EEPROM_ADR_ACCOFFSET 1
#define EEPROM_ADR_GYROOFFSET (EEPROM_ADR_ACCOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
#define EEPROM_ADR_GYROAMPLIFIER (EEPROM_ADR_GYROOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
#define EEPROM_ADR_CHANNELMAP (EEPROM_ADR_GYROAMPLIFIER+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
#define EEPROM_ADR_MIXER_TABLE (EEPROM_ADR_CHANNELMAP+sizeof(channelMap_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
#define EEPROM_ADR_PARAMSET_BEGIN (EEPROM_ADR_MIXER_TABLE+sizeof(mixerMatrix_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
#define PID_ACTIVE_SET 0 // byte
//#define EEPROM_ADR_ACCOFFSET 1
//#define EEPROM_ADR_GYROOFFSET (EEPROM_ADR_ACCOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
//#define EEPROM_ADR_GYROAMPLIFIER (EEPROM_ADR_GYROOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
//#define EEPROM_ADR_CHANNELMAP (EEPROM_ADR_GYROAMPLIFIER+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
//#define EEPROM_ADR_MIXER_TABLE (EEPROM_ADR_CHANNELMAP+sizeof(channelMap_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
//#define EEPROM_ADR_PARAMSET_BEGIN (EEPROM_ADR_MIXER_TABLE+sizeof(mixerMatrix_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD)
 
//#define EEPROM_ADR_ACCOFFSET 10
//#define EEPROM_ADR_GYROOFFSET 20
//#define EEPROM_ADR_GYROAMPLIFIER 30
//#define EEPROM_ADR_CHANNELMAP 40
//#define EEPROM_ADR_MIXER_TABLE 60
//#define EEPROM_ADR_PARAMSET_BEGIN 200
#define EEPROM_ADR_ACCOFFSET 16
#define EEPROM_ADR_GYROOFFSET 32
#define EEPROM_ADR_GYROAMPLIFIER 48
#define EEPROM_ADR_CHANNELMAP 64
#define EEPROM_ADR_MIXER_TABLE 128
#define EEPROM_ADR_PARAMSET_BEGIN 256
 
#define CHANNELMAP_REVISION 0
#define EEPARAM_REVISION 0
#define EEMIXER_REVISION 0
#define EEPARAM_REVISION 0
#define EEMIXER_REVISION 0
#define SENSOROFFSET_REVISION 0
 
void paramSet_readOrDefault(void);
35,10 → 35,10
uint8_t paramSet_readFromEEProm(uint8_t setnumber);
void paramSet_writeToEEProm(uint8_t setnumber);
 
uint8_t channelMap_readFromEEProm(void);
//uint8_t channelMap_readFromEEProm(void);
void channelMap_writeToEEProm(void);
 
uint8_t mixerMatrix_eeadFromEEProm(void);
//uint8_t mixerMatrix_readFromEEProm(void);
void mixerMatrix_writeToEEProm(void);
 
uint8_t gyroAmplifierOffset_readFromEEProm(void);
/branches/dongfang_FC_rewrite/main.c
68,6 → 68,7
#include "configuration.h"
#include "printf_P.h"
#include "twimaster.h"
#include "controlMixer.h"
#ifdef USE_NAVICTRL
#include "spi.h"
#endif
91,6 → 92,10
WDTCSR |= (1 << WDCE) | (1 << WDE);
WDTCSR = 0;
 
// This is strange: It should NOT be necessarty to do. But the call of the same,
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
channelMap_default();
 
// initalize modules
output_init();
timer0_init();
107,6 → 112,11
MK3MAG_Init();
#endif
 
// Parameter Set handling
channelMap_readOrDefault();
mixerMatrix_readOrDefault();
paramSet_readOrDefault();
 
// enable interrupts global
sei();
 
119,11 → 129,6
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
printf("\n\r===================================");
 
// Parameter Set handling
channelMap_readOrDefault();
mixerMatrix_readOrDefault();
paramSet_readOrDefault();
 
// Wait for a short time (otherwise the RC channel check won't work below)
// timer = SetDelay(500);
// while(!CheckDelay(timer));
/branches/dongfang_FC_rewrite/makefile
1,6 → 1,6
#--------------------------------------------------------------------
# MCU name
MCU = atmega644
MCU = atmega644p
F_CPU = 20000000
#-------------------------------------------------------------------
VERSION_MAJOR = 0
16,7 → 16,8
 
# Use one of the extensions for a gps solution
#EXT = NAVICTRL
EXT = MK3MAG
#EXT = MK3MAG
EXT =
 
# Use optional one the RCs if EXT = NAVICTRL has been used
#RC = DSL
514,9 → 515,7
# Remove the '-' if you want to see the dependency files generated.
-include $(SRC:.c=.d)
 
 
 
# Listing of phony targets.
.PHONY : all begin finish end sizebefore sizeafter gccversion coff extcoff \
clean clean_list program
 
/branches/dongfang_FC_rewrite/mk3mag.c
60,7 → 60,7
#include "output.h"
 
uint8_t PWMTimeout = 12;
ToMk3Mag_t ToMk3Mag;
ToMk3Mag_t toMk3Mag;
 
/*********************************************/
/* Initialize Interface to MK3MAG Compass */
72,8 → 72,8
 
PWMTimeout = 0;
 
ToMk3Mag.CalState = 0;
ToMk3Mag.Orientation = 1;
toMk3Mag.CalState = 0;
toMk3Mag.Orientation = 1;
}
 
/*********************************************/
/branches/dongfang_FC_rewrite/mk3mag.h
8,7 → 8,7
uint8_t Orientation;
} ToMk3Mag_t;
 
extern ToMk3Mag_t ToMk3Mag;
extern ToMk3Mag_t toMk3Mag;
 
// Initialization
void MK3MAG_Init(void);
/branches/dongfang_FC_rewrite/rc.c
274,6 → 274,10
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
= RC_PRTY[CONTROL_YAW] = 0;
}
debugOut.analog[3] = channelMap.channels[0];
debugOut.analog[4] = channelMap.channels[1];
debugOut.analog[5] = channelMap.channels[2];
}
 
/*
/branches/dongfang_FC_rewrite/sensors.h
13,7 → 13,7
* Whether (pitch, roll, Z) acc. meters are reversed(see analog.h).
*/
extern const uint8_t ACC_REVERSED[3];
extern volatile sensorOffset_t gyroAmplifierOffset;
extern sensorOffset_t gyroAmplifierOffset;
 
/*
* Common procedures for all gyro types.
/branches/dongfang_FC_rewrite/uart0.c
112,7 → 112,7
 
uint16_t DebugData_Timer;
uint16_t Data3D_Timer;
uint16_t DebugData_Interval = 500; // in 1ms
uint16_t DebugData_Interval = 0; // in 1ms
uint16_t Data3D_Interval = 0; // in 1ms
 
#ifdef USE_MK3MAG
125,9 → 125,9
"AnglePitch ", //0
"AngleRoll ",
"AngleYaw ",
"GyroPitch(PID) ",
"GyroRoll(PID) ",
"GyroYaw ", //5
"rc 0 ",
"rc 1 ",
"rc 2 ", //5
"OffsPitch ",
"OffsRoll ",
"AttitudeControl ",
264,9 → 264,9
/****************************************************************/
ISR(USART0_RX_vect)
{
static uint16_t crc;
static uint16_t checksum;
static uint8_t ptr_rxd_buffer = 0;
uint8_t crc1, crc2;
uint8_t checksum1, checksum2;
uint8_t c;
 
c = UDR0; // catch the received byte
277,29 → 277,29
// the rxd buffer is unlocked
if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
crc = c; // init crc
checksum = c; // init checksum
}
#if 0
else if (ptr_rxd_buffer == 1) { // handle address
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
crc += c; // update crc
checksum += c; // update checksum
}
#endif
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
if (c != '\r') { // no termination character
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
crc += c; // update crc
checksum += c; // update checksum
} else { // termination character was received
// the last 2 bytes are no subject for checksum calculation
// they are the checksum itself
crc -= rxd_buffer[ptr_rxd_buffer - 2];
crc -= rxd_buffer[ptr_rxd_buffer - 1];
checksum -= rxd_buffer[ptr_rxd_buffer - 2];
checksum -= rxd_buffer[ptr_rxd_buffer - 1];
// calculate checksum from transmitted data
crc %= 4096;
crc1 = '=' + crc / 64;
crc2 = '=' + crc % 64;
checksum %= 4096;
checksum1 = '=' + checksum / 64;
checksum2 = '=' + checksum % 64;
// compare checksum to transmitted checksum bytes
if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2
if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2
== rxd_buffer[ptr_rxd_buffer - 1])) {
// checksum valid
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
321,14 → 321,14
}
 
// --------------------------------------------------------------------------
void AddCRC(uint16_t datalen) {
uint16_t tmpCRC = 0, i;
void Addchecksum(uint16_t datalen) {
uint16_t tmpchecksum = 0, i;
for (i = 0; i < datalen; i++) {
tmpCRC += txd_buffer[i];
tmpchecksum += txd_buffer[i];
}
tmpCRC %= 4096;
txd_buffer[i++] = '=' + tmpCRC / 64;
txd_buffer[i++] = '=' + tmpCRC % 64;
tmpchecksum %= 4096;
txd_buffer[i++] = '=' + tmpchecksum / 64;
txd_buffer[i++] = '=' + tmpchecksum % 64;
txd_buffer[i++] = '\r';
txd_complete = FALSE;
UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
381,7 → 381,7
txd_buffer[txd_bufferIndex] = 0;
}
va_end(ap);
AddCRC(pt); // add checksum after data block and initates the transmission
Addchecksum(pt); // add checksum after data block and initates the transmission
}
*/
 
447,7 → 447,7
txd_buffer[pt++] = '=' + (c & 0x3f);
}
va_end(ap);
AddCRC(pt); // add checksum after data block and initates the transmission
Addchecksum(pt); // add checksum after data block and initates the transmission
}
 
// --------------------------------------------------------------------------
695,9 → 695,8
request_DebugLabel = 0xFF;
}
 
if (ConfirmFrame && txd_complete) { // Datensatz ohne CRC best�tigen
SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame,
sizeof(ConfirmFrame));
if (ConfirmFrame && txd_complete) { // Datensatz ohne checksum bestätigen
SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, sizeof(ConfirmFrame));
ConfirmFrame = 0;
}
 
728,12 → 727,12
 
#ifdef USE_MK3MAG
if((checkDelay(Compass_Timer)) && txd_complete) {
ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
ToMk3Mag.UserParam[0] = dynamicParams.userParams[0];
ToMk3Mag.UserParam[1] = dynamicParams.userParams[1];
ToMk3Mag.CalState = compassCalState;
SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
toMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
toMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
toMk3Mag.UserParam[0] = dynamicParams.userParams[0];
toMk3Mag.UserParam[1] = dynamicParams.userParams[1];
toMk3Mag.CalState = compassCalState;
SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag));
// the last state is 5 and should be send only once to avoid multiple flash writing
if(compassCalState > 4) compassCalState = 0;
Compass_Timer = setDelay(99);