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; |
} |