Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2188 → Rev 2189

/branches/dongfang_FC_rewrite/uart0.c
6,14 → 6,13
#include <string.h>
 
#include "eeprom.h"
#include "menu.h"
#include "timer0.h"
#include "uart0.h"
#include "rc.h"
#include "externalControl.h"
#include "output.h"
#include "attitude.h"
#include "commands.h"
#include "debug.h"
#include "profiler.h"
#include "beeper.h"
 
#ifdef USE_DIRECT_GPS
#include "mk3mag.h"
26,19 → 25,23
#define FALSE 0
#define TRUE 1
 
uint8_t requestedDebugLabel = 255;
DebugOut_t debugOut;
 
uint8_t request_verInfo = FALSE;
uint8_t request_externalControl = FALSE;
uint8_t request_display = FALSE;
uint8_t request_display1 = FALSE;
uint8_t request_debugData = FALSE;
uint8_t request_data3D = FALSE;
uint8_t request_PPMChannels = FALSE;
uint8_t request_motorTest = FALSE;
uint8_t request_variables = FALSE;
uint8_t request_OSD = FALSE;
uint8_t requestedAnalogLabel = 255;
uint8_t requestedProfilerLabel = 255;
 
uint8_t request_verInfo;
uint8_t request_externalControl;
uint8_t request_display;
uint8_t request_display1;
uint8_t request_debugData;
uint8_t request_profilerData;
uint8_t request_PPMChannels;
uint8_t request_outputTest;
uint8_t request_variables;
uint8_t request_OSD;
uint8_t request_DCM_matrix;
 
/*
#define request_verInfo (1<<0)
#define request_externalControl (1<<1)
54,18 → 57,16
 
//uint16_t request = 0;
 
uint8_t displayLine = 0;
 
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
volatile uint8_t rxd_buffer_locked = FALSE;
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
volatile uint8_t txd_complete = TRUE;
volatile uint8_t receivedBytes = 0;
volatile uint8_t *pRxData = 0;
volatile uint8_t rxDataLen = 0;
volatile uint8_t txd_complete;
volatile uint8_t receivedBytes;
volatile uint8_t *pRxData;
volatile uint8_t rxDataLen;
 
uint8_t motorTestActive = 0;
uint8_t motorTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
uint8_t outputTestActive;
uint8_t outputTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
uint8_t confirmFrame;
 
typedef struct {
72,14 → 73,15
int16_t heading;
}__attribute__((packed)) Heading_t;
 
Data3D_t data3D;
IMUData imuData;
 
uint16_t debugData_timer;
uint16_t data3D_timer;
uint16_t OSD_timer;
uint16_t debugData_interval = 0; // in 1ms
uint16_t data3D_interval = 0; // in 1ms
uint16_t OSD_interval = 0;
uint16_t debugDataTimer;
uint16_t profilerDataTimer;
uint16_t OSDDataTimer;
uint16_t debugDataInterval; // in 1ms
uint16_t profilerDataInterval; // in 1ms
uint16_t imuDataInterval; // in 1ms
uint16_t OSDDataInterval;
 
#ifdef USE_DIRECT_GPS
int16_t toMk3MagTimer;
86,48 → 88,81
#endif
 
// keep lables in flash to save 512 bytes of sram space
const prog_uint8_t ANALOG_LABEL[32][16] = {
//1234567890123456
"AnglePitch ", //0
"AngleRoll ",
"AngleYaw ",
"GyroPitch ",
"GyroRoll ",
"GyroYaw ", //5
"AccPitch(0.1deg)",
"AccRoll(0.1deg) ",
"AccZ ",
"RC pitch ",
"RC roll ", //10
" ",
"zerothOrderCorr ",
"DriftCompPitch ",
"DriftCompRoll ",
"GActivityDivider", //15
"AccPitch ",
"AccRoll ",
"CorrectionSum pi",
"CorrectionSum ro",
"control act wghd", //20
"acc vector wghd ",
"Height[dm] ",
"dHeight ",
"acc vector ",
"EFT ", //25
"naviPitch ",
"naviRoll ",
"Rate Tolerance ",
"Gyro Act Cont. ",
"GPS altitude ", //30
"GPS vert accura "
};
//1234567890123456
const char analogLabel0[] PROGMEM = "AngleRoll";
const char analogLabel1[] PROGMEM = "AnglePitch";
const char analogLabel2[] PROGMEM = "AngleYaw";
const char analogLabel3[] PROGMEM = "GyroX(Roll)";
const char analogLabel4[] PROGMEM = "GyroY(Pitch)";
const char analogLabel5[] PROGMEM = "GyroZ(Yaw)";
const char analogLabel6[] PROGMEM = "AccX(0.01m/s^2)";
const char analogLabel7[] PROGMEM = "AccY(0.01m/s^2)";
const char analogLabel8[] PROGMEM = "AccZ(0.01m/s^2)";
const char analogLabel9[] PROGMEM = "RC pitch";
const char analogLabel10[] PROGMEM = "RC yaw";
const char analogLabel11[] PROGMEM = "RC throttle";
const char analogLabel12[] PROGMEM = "Roll";
const char analogLabel13[] PROGMEM = "Pitch";
const char analogLabel14[] PROGMEM = "rollControl";
const char analogLabel15[] PROGMEM = "pitchControl";
const char analogLabel16[] PROGMEM = "M1";
const char analogLabel17[] PROGMEM = "M2";
const char analogLabel18[] PROGMEM = "M3";
const char analogLabel19[] PROGMEM = "M4";
const char analogLabel20[] PROGMEM = "flightmode";
const char analogLabel21[] PROGMEM = "Att freq";
const char analogLabel22[] PROGMEM = "Height[dm]";
const char analogLabel23[] PROGMEM = "dHeight";
const char analogLabel24[] PROGMEM = "attitudeSumCount";
const char analogLabel25[] PROGMEM = "simpleAirPressure";
const char analogLabel26[] PROGMEM = "OCR0A";
const char analogLabel27[] PROGMEM = "filteredAirPressure";
const char analogLabel28[] PROGMEM = "height";
const char analogLabel29[] PROGMEM = "Gyro Act Cont.";
const char analogLabel30[] PROGMEM = "GPS altitude";
const char analogLabel31[] PROGMEM = "GPS vert accura";
 
PGM_P ANALOG_LABELS[] PROGMEM = {
analogLabel0,
analogLabel1,
analogLabel2,
analogLabel3,
analogLabel4,
analogLabel5,
analogLabel6,
analogLabel7,
analogLabel8,
analogLabel9,
analogLabel10,
analogLabel11,
analogLabel12,
analogLabel13,
analogLabel14,
analogLabel15,
analogLabel16,
analogLabel17,
analogLabel18,
analogLabel19,
analogLabel20,
analogLabel21,
analogLabel22,
analogLabel23,
analogLabel24,
analogLabel25,
analogLabel26,
analogLabel27,
analogLabel28,
analogLabel29,
analogLabel30,
analogLabel31
};
 
/****************************************************************/
/* Initialization of the USART0 */
/****************************************************************/
void usart0_init(void) {
uint8_t sreg = SREG;
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1);
uint16_t ubrr = (F_CPU / (8 * USART0_BAUD) - 1);
 
// disable all interrupts before configuration
cli();
180,7 → 215,8
UCSR0B |= (1 << TXCIE0);
 
// initialize the debug timer
debugData_timer = setDelay(debugData_interval);
debugDataTimer = setDelay(debugDataInterval);
profilerDataTimer = setDelay(profilerDataInterval);
 
// unlock rxd_buffer
rxd_buffer_locked = FALSE;
240,11 → 276,11
return; // if rxd buffer is locked immediately return
 
// the rxd buffer is unlocked
if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and synchronization character is received
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
checksum = c; // init checksum
}
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incoming bytes
if (c != '\r') { // no termination character
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
checksum += c; // update checksum
295,9 → 331,9
 
// --------------------------------------------------------------------------
// application example:
// sendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
// sendData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
/*
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
void sendData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
va_list ap;
uint16_t txd_bufferIndex = 0;
uint8_t *currentBuffer;
344,7 → 380,7
}
*/
 
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
void sendData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
va_list ap;
uint16_t pt = 0;
uint8_t a, b, c;
447,12 → 483,13
 
// --------------------------------------------------------------------------
void usart0_processRxData(void) {
// We control the motorTestActive var from here: Count it down.
if (motorTestActive)
motorTestActive--;
// We control the outputTestActive var from here: Count it down.
if (outputTestActive)
outputTestActive--;
// if data in the rxd buffer are not locked immediately return
if (!rxd_buffer_locked)
return;
 
uint8_t tempchar[3];
decode64(); // decode data block in rxd_buffer
 
468,27 → 505,24
break;
#endif
case 't': // motor test
if (rxDataLen > 20) {
memcpy(&motorTest[0], (uint8_t*) pRxData, sizeof(motorTest));
} else {
memcpy(&motorTest[0], (uint8_t*) pRxData, 4);
}
motorTestActive = 255;
memcpy(&outputTest[0], (uint8_t*) pRxData, /*sizeof(outputTest)*/ 12); // 12 is an mktool limitation.
outputTestActive = 255;
// Huh??
externalControlActive = 255;
break;
 
case 'n':// Read motor mixer
tempchar[0] = EEMIXER_REVISION;
tempchar[1] = sizeof(MotorMixer_t);
tempchar[1] = sizeof(OutputMixer_t);
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('N', FC_ADDRESS, 2, &tempchar, 2, (uint8_t*)&motorMixer, sizeof(motorMixer));
sendData('N', FC_ADDRESS, 2, &tempchar, 2, (uint8_t*)&outputMixer, sizeof(OutputMixer_t));
break;
 
case 'm':// "Set Mixer Table
if (pRxData[0] == EEMIXER_REVISION && (pRxData[1] == sizeof(MotorMixer_t))) {
memcpy(&motorMixer, (uint8_t*)&pRxData[2], sizeof(motorMixer));
motorMixer_writeToEEProm();
if (pRxData[0] == EEMIXER_REVISION && (pRxData[1] == sizeof(OutputMixer_t))) {
memcpy(&outputMixer, (uint8_t*)&pRxData[2], sizeof(OutputMixer_t));
outputMixer_writeToEEProm();
while (!txd_complete)
; // wait for previous frame to be sent
tempchar[0] = 1;
495,7 → 529,7
} else {
tempchar[0] = 0;
}
sendOutData('M', FC_ADDRESS, 1, &tempchar, 1);
sendData('M', FC_ADDRESS, 1, &tempchar, 1);
break;
 
case 'p': // get PPM channels
507,7 → 541,7
tempchar[1] = sizeof(IMUConfig);
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig));
sendData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig));
break;
 
case 'j':// Save IMU configuration
522,7 → 556,7
}
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('J', FC_ADDRESS, 1, &tempchar, 1);
sendData('J', FC_ADDRESS, 1, &tempchar, 1);
}
break;
 
544,7 → 578,7
tempchar[2] = sizeof(staticParams);
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams));
sendData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams));
break;
 
case 's': // save settings
559,11 → 593,11
tempchar[0] = getActiveParamSet();
beepNumber(tempchar[0]);
} else {
tempchar[0] = 0; //indicate bad data
tempchar[0] = sizeof(staticParams); //indicate bad data
}
while (!txd_complete)
; // wait for previous frame to be sent
sendOutData('S', FC_ADDRESS, 1, &tempchar, 1);
sendData('S', FC_ADDRESS, 1, &tempchar, 1);
}
break;
 
575,32 → 609,42
default: // any Slave Address
switch (rxd_buffer[2]) {
case 'a':// request for labels of the analog debug outputs
requestedDebugLabel = pRxData[0];
if (requestedDebugLabel > 31)
requestedDebugLabel = 31;
requestedAnalogLabel = pRxData[0];
if (requestedAnalogLabel > 31)
requestedAnalogLabel = 31;
break;
 
case 'b': // submit extern control
memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl));
memcpy(&externalControl, (uint8_t*) pRxData, sizeof(ExternalControl_t));
confirmFrame = externalControl.frame;
externalControlActive = 255;
break;
 
case 'h':// request for display columns
remoteKeys |= pRxData[0];
if (remoteKeys)
displayLine = 0;
request_display = TRUE;
break;
case 'd': // request for the debug data
debugDataInterval = (uint16_t) pRxData[0] * 10;
if (debugDataInterval > 0)
request_debugData = TRUE;
break;
 
case 'l':// request for display columns
menuItem = pRxData[0];
request_display1 = TRUE;
break;
case 'e': // Requeset for the DCM matrix
request_DCM_matrix = TRUE;
break;
 
case 'f':
requestedProfilerLabel = pRxData[0];
if (requestedProfilerLabel > 15)
requestedProfilerLabel = 15;
break;
 
case 'u':
profilerDataInterval = (uint16_t) pRxData[0] * 10;
if (profilerDataInterval > 0)
request_profilerData = TRUE;
break;
 
case 'o':// request for OSD data (FC style)
OSD_interval = (uint16_t) pRxData[0] * 10;
if (OSD_interval > 0)
OSDDataInterval = (uint16_t) pRxData[0] * 10;
if (OSDDataInterval > 0)
request_OSD = TRUE;
break;
616,18 → 660,6
request_externalControl = TRUE;
break;
 
case 'd': // request for the debug data
debugData_interval = (uint16_t) pRxData[0] * 10;
if (debugData_interval > 0)
request_debugData = TRUE;
break;
 
case 'c': // request for the 3D data
data3D_interval = (uint16_t) pRxData[0] * 10;
if (data3D_interval > 0)
request_data3D = TRUE;
break;
 
default:
//unsupported command received
break;
643,9 → 675,9
/************************************************************************/
/* Routine f�r die Serielle Ausgabe */
/************************************************************************/
int16_t uart_putchar(int8_t c) {
int uart_putchar(char c, FILE* fims) {
if (c == '\n')
uart_putchar('\r');
uart_putchar('\r', fims);
// wait until previous character was send
loop_until_bit_is_set(UCSR0A, UDRE0);
// send character
659,58 → 691,61
return;
 
if (request_verInfo && txd_complete) {
sendOutData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo));
sendData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo));
request_verInfo = FALSE;
}
 
if (request_display && txd_complete) {
LCD_printMenu();
sendOutData('H', FC_ADDRESS, 2, &displayLine, sizeof(displayLine),
&displayBuff[displayLine * 20], 20);
displayLine++;
if (displayLine >= 4)
displayLine = 0;
request_display = FALSE;
}
 
if (request_display1 && txd_complete) {
LCD_printMenu();
sendOutData('L', FC_ADDRESS, 3, &menuItem, sizeof(menuItem), &maxMenuItem,
sizeof(maxMenuItem), displayBuff, sizeof(displayBuff));
request_display1 = FALSE;
}
 
if (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten
uint8_t label[16]; // local sram buffer
memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer
sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel,
sizeof(requestedDebugLabel), label, 16);
requestedDebugLabel = 0xFF;
if (requestedAnalogLabel != 0xFF && txd_complete) {
char label[17]; // local sram buffer
memset(label, ' ', sizeof(label));
strcpy_P(label, (PGM_P)pgm_read_word(&(ANALOG_LABELS[requestedAnalogLabel]))); // read label from flash to sram buffer
sendData('A', FC_ADDRESS, 2, (uint8_t *) &requestedAnalogLabel, sizeof(requestedAnalogLabel), label, 16);
requestedAnalogLabel = 0xFF;
}
 
if (requestedProfilerLabel != 0xFF && txd_complete) {
char label[17]; // local sram buffer
memset(label, ' ', sizeof(label));
strcpy_P(label, (PGM_P)pgm_read_word(&(PROFILER_LABELS[requestedProfilerLabel]))); // read label from flash to sram buffer
sendData('F', FC_ADDRESS, 2, (uint8_t *) &requestedProfilerLabel, sizeof(requestedProfilerLabel), label, 16);
requestedProfilerLabel = 0xFF;
}
 
if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen
sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame));
sendData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame));
confirmFrame = 0;
}
 
if (((debugData_interval && checkDelay(debugData_timer)) || request_debugData)
&& txd_complete) {
sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut));
debugData_timer = setDelay(debugData_interval);
if (((debugDataInterval && checkDelay(debugDataTimer)) || request_debugData) && txd_complete) {
sendData('D', FC_ADDRESS, 1, (uint8_t *)&debugOut, sizeof(debugOut));
debugDataTimer = setDelay(debugDataInterval);
request_debugData = FALSE;
}
 
if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) && txd_complete) {
sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D));
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
data3D_timer = setDelay(data3D_interval);
request_data3D = FALSE;
if (((profilerDataInterval && checkDelay(profilerDataTimer)) || request_profilerData) && txd_complete) {
sendData('U', FC_ADDRESS, 2, (uint8_t *)&totalProfilerHits, sizeof(totalProfilerHits), (uint8_t *)&activitiesTimerHits, sizeof(activitiesTimerHits));
profilerDataTimer = setDelay(profilerDataInterval);
request_profilerData = FALSE;
}
 
if (request_DCM_matrix && txd_complete) {
/*
sendData('E', FC_ADDRESS, 1,
(uint8_t *) &dcmGyro, sizeof(dcmGyro));
*/
request_DCM_matrix = FALSE;
}
 
if (request_externalControl && txd_complete) {
sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
sendData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
sizeof(externalControl));
request_externalControl = FALSE;
}
722,7 → 757,7
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));
sendData('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;
toMk3MagTimer = setDelay(99);
729,19 → 764,19
}
#endif
 
if (request_motorTest && txd_complete) {
sendOutData('T', FC_ADDRESS, 0);
request_motorTest = FALSE;
if (request_outputTest && txd_complete) {
sendData('T', FC_ADDRESS, 0);
request_outputTest = FALSE;
}
 
if (request_PPMChannels && txd_complete) {
uint8_t length = MAX_CHANNELS;
sendOutData('P', FC_ADDRESS, 2, &length, 1, (uint8_t*)&PPM_in, sizeof(PPM_in));
uint8_t length = MAX_CONTROLCHANNELS;
sendData('P', FC_ADDRESS, 2, &length, 1, (uint8_t*)&PPM_in, sizeof(PPM_in));
request_PPMChannels = FALSE;
}
 
if (request_variables && txd_complete) {
sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
sendData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
request_variables = FALSE;
}
 
751,7 → 786,7
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
sendOutData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
sendData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
OSD_timer = setDelay(OSD_interval);
request_OSD = FALSE;
}