/branches/dongfang_FC_rewrite/configuration.c |
---|
58,7 → 58,7 |
#include "uart0.h" |
int16_t variables[VARIABLE_COUNT]; |
paramset_t staticParams; |
ParamSet_t staticParams; |
channelMap_t channelMap; |
mixerMatrix_t mixerMatrix; |
volatile dynamicParam_t dynamicParams; |
116,11 → 116,11 |
} |
const XLATION XLATIONS[] = { |
{offsetof(paramset_t, heightSetting), offsetof(dynamicParam_t, heightSetting)}, |
{offsetof(ParamSet_t, heightSetting), offsetof(dynamicParam_t, heightSetting)}, |
}; |
const MMXLATION MMXLATIONS[] = { |
{offsetof(paramset_t, heightD), offsetof(dynamicParam_t, heightD),0,100}, |
{offsetof(ParamSet_t, heightD), offsetof(dynamicParam_t, heightD),0,100}, |
}; |
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
152,7 → 152,7 |
} |
for (i=0; i<sizeof(staticParams.userParams); i++) { |
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, userParams) + i)); |
src = *((uint8_t*)(&staticParams + offsetof(ParamSet_t, userParams) + i)); |
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, userParams) + i); |
if (src < 255) { |
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
/branches/dongfang_FC_rewrite/configuration.h |
---|
57,13 → 57,18 |
uint8_t attitudeControl; |
// The rest... |
// Output and servo |
/*PMM*/uint8_t output0Timing; |
/*PMM*/uint8_t output1Timing; |
uint8_t servoManualControl[2]; |
// Correction |
uint8_t levelCorrection[2]; |
// Simple direct navigation |
uint8_t directGPSMode; |
/* P */uint8_t userParams[8]; |
} dynamicParam_t; |
179,14 → 184,28 |
uint8_t outputDebugMask; |
uint8_t outputFlags; |
// Simple direct navigation |
uint8_t directNaviMode; |
// NaviCtrl navigation |
// Shared for both modes of navigation |
uint8_t naviStickThreshold; |
uint8_t GPSMininumSatellites; |
uint8_t naviP; |
uint8_t naviI; |
uint8_t naviD; |
// User params |
uint8_t userParams[8]; |
// Name |
char name[12]; |
} paramset_t; |
extern paramset_t staticParams; |
} ParamSet_t; |
extern ParamSet_t staticParams; |
// MKFlags |
#define MKFLAG_MOTOR_RUN (1<<0) |
#define MKFLAG_FLY (1<<1) |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
55,6 → 55,7 |
#include "heightControl.h" |
#include "attitudeControl.h" |
#include "externalControl.h" |
#include "naviControl.h" |
#include "configuration.h" |
#include "attitude.h" |
#include "commands.h" |
167,25 → 168,29 |
/* |
* Update the variables indicating stick position from the sum of R/C, GPS and external control. |
*/ |
void controlMixer_update(void) { |
void controlMixer_periodicTask(void) { |
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
// TODO: If no signal --> zero. |
int16_t tempThrottle; |
// takes almost no time... |
RC_update(); |
RC_periodicTask(); |
// takes almost no time... |
EC_update(); |
EC_periodicTask(); |
// takes about 80 usec. |
HC_update(); |
HC_periodicTask(); |
int16_t* RC_PRTY = RC_getPRTY(); |
int16_t* EC_PRTY = EC_getPRTY(); |
updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]); |
updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]); |
int16_t naviSticks[] = {RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH], RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]}; |
navigation_periodicTask(&naviSticks); |
updateControlAndMeasureControlActivity(CONTROL_PITCH, naviSticks[CONTROL_PITCH]); |
updateControlAndMeasureControlActivity(CONTROL_ROLL, naviSticks[CONTROL_ROLL]); |
updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
dampenControlActivity(); |
/branches/dongfang_FC_rewrite/controlMixer.h |
---|
83,7 → 83,7 |
/* |
* Update the exported variables. Called at every flight control cycle. |
*/ |
void controlMixer_update(void); |
void controlMixer_periodicTask(void); |
/* |
* Get the current command. See the COMMAND_.... define's |
/branches/dongfang_FC_rewrite/directGPSNaviControl.c |
---|
0,0 → 1,364 |
// Navigation with a GPS directly attached to the FC's UART1. |
#include <inttypes.h> |
#include <stdlib.h> |
#include <stddef.h> |
//#include "mymath.h" |
//#include "timer0.h" |
//#include "uart1.h" |
//#include "rc.h" |
//#include "eeprom.h" |
#include "ubx.h" |
#include "configuration.h" |
#include "controlMixer.h" |
#include "output.h" |
#include "isqrt.h" |
#include "attitude.h" |
#include "dongfangMath.h" |
typedef enum { |
GPS_FLIGHT_MODE_UNDEF, |
GPS_FLIGHT_MODE_FREE, |
GPS_FLIGHT_MODE_AID, |
GPS_FLIGHT_MODE_HOME, |
} FlightMode_t; |
#define GPS_POSINTEGRAL_LIMIT 32000 |
#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
#define GPS_P_LIMIT 25 |
typedef struct { |
int32_t longitude; |
int32_t latitude; |
int32_t altitude; |
Status_t status; |
} GPS_Pos_t; |
// GPS coordinates for hold position |
GPS_Pos_t holdPosition = { 0, 0, 0, INVALID }; |
// GPS coordinates for home position |
GPS_Pos_t homePosition = { 0, 0, 0, INVALID }; |
// the current flight mode |
FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF; |
// --------------------------------------------------------------------------------- |
void GPS_updateFlightMode(void) { |
static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF; |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD |
|| MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
flightMode = GPS_FLIGHT_MODE_FREE; |
} else { |
if (dynamicParams.directGPSMode < 50) |
flightMode = GPS_FLIGHT_MODE_AID; |
else if (dynamicParams.directGPSMode < 180) |
flightMode = GPS_FLIGHT_MODE_FREE; |
else |
flightMode = GPS_FLIGHT_MODE_HOME; |
} |
if (flightMode != flightModeOld) { |
beep(100); |
flightModeOld = flightMode; |
} |
} |
// --------------------------------------------------------------------------------- |
// This function defines a good GPS signal condition |
uint8_t GPS_isSignalOK(void) { |
static uint8_t GPSFix = 0; |
if ((GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D) |
&& (GPSInfo.flags & FLAG_GPSFIXOK) |
&& ((GPSInfo.satnum >= staticParams.GPSMininumSatellites) || GPSFix)) { |
GPSFix = 1; |
return 1; |
} else |
return (0); |
} |
// --------------------------------------------------------------------------------- |
// rescale xy-vector length to limit |
uint8_t GPS_limitXY(int32_t *x, int32_t *y, int32_t limit) { |
int32_t len; |
len = isqrt32(*x * *x + *y * *y); |
if (len > limit) { |
// normalize control vector components to the limit |
*x = (*x * limit) / len; |
*y = (*y * limit) / len; |
return 1; |
} |
return 0; |
} |
// checks nick and roll sticks for manual control |
uint8_t GPS_isManuallyControlled(int16_t* naviSticks) { |
if (naviSticks[CONTROL_PITCH] < staticParams.naviStickThreshold |
&& naviSticks[CONTROL_ROLL] < staticParams.naviStickThreshold) |
return 0; |
else |
return 1; |
} |
// set given position to current gps position |
uint8_t GPS_setCurrPosition(GPS_Pos_t * pGPSPos) { |
uint8_t retval = 0; |
if (pGPSPos == NULL) |
return (retval); // bad pointer |
if (GPS_isSignalOK()) { // is GPS signal condition is fine |
pGPSPos->longitude = GPSInfo.longitude; |
pGPSPos->latitude = GPSInfo.latitude; |
pGPSPos->altitude = GPSInfo.altitude; |
pGPSPos->status = NEWDATA; |
retval = 1; |
} else { // bad GPS signal condition |
pGPSPos->status = INVALID; |
retval = 0; |
} |
return (retval); |
} |
// clear position |
uint8_t GPS_clearPosition(GPS_Pos_t * pGPSPos) { |
uint8_t retval = 0; |
if (pGPSPos == NULL) |
return retval; // bad pointer |
else { |
pGPSPos->longitude = 0; |
pGPSPos->latitude = 0; |
pGPSPos->altitude = 0; |
pGPSPos->status = INVALID; |
retval = 1; |
} |
return (retval); |
} |
// calculates the GPS control stick values from the deviation to target position |
// if the pointer to the target positin is NULL or is the target position invalid |
// then the P part of the controller is deactivated. |
void GPS_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) { |
static int32_t PID_Nick, PID_Roll; |
int32_t coscompass, sincompass; |
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, |
I_East = 0; |
int32_t PID_North = 0, PID_East = 0; |
static int32_t cos_target_latitude = 1; |
static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0; |
static GPS_Pos_t *pLastTargetPos = 0; |
// if GPS data and Compass are ok |
if (GPS_isSignalOK() && (compassHeading >= 0)) { |
if (pTargetPos != NULL) // if there is a target position |
{ |
if (pTargetPos->status != INVALID) // and the position data are valid |
{ |
// if the target data are updated or the target pointer has changed |
if ((pTargetPos->status != PROCESSED) |
|| (pTargetPos != pLastTargetPos)) { |
// reset error integral |
GPSPosDevIntegral_North = 0; |
GPSPosDevIntegral_East = 0; |
// recalculate latitude projection |
cos_target_latitude = int_cos((int16_t) (pTargetPos->latitude / 10000000L)); |
// remember last target pointer |
pLastTargetPos = pTargetPos; |
// mark data as processed |
pTargetPos->status = PROCESSED; |
} |
// calculate position deviation from latitude and longitude differences |
GPSPosDev_North = (GPSInfo.latitude - pTargetPos->latitude); // to calculate real cm we would need *111/100 additionally |
GPSPosDev_East = (GPSInfo.longitude - pTargetPos->longitude); // to calculate real cm we would need *111/100 additionally |
// calculate latitude projection |
GPSPosDev_East *= cos_target_latitude; |
GPSPosDev_East >>= MATH_UNIT_FACTOR_LOG; |
} else { // no valid target position available |
// reset error |
GPSPosDev_North = 0; |
GPSPosDev_East = 0; |
// reset error integral |
GPSPosDevIntegral_North = 0; |
GPSPosDevIntegral_East = 0; |
} |
} else { // no target position available |
// reset error |
GPSPosDev_North = 0; |
GPSPosDev_East = 0; |
// reset error integral |
GPSPosDevIntegral_North = 0; |
GPSPosDevIntegral_East = 0; |
} |
//Calculate PID-components of the controller |
// D-Part |
D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) / 512; |
D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) / 512; |
// P-Part |
P_North = ((int32_t) staticParams.naviP * GPSPosDev_North) / 2048; |
P_East = ((int32_t) staticParams.naviP * GPSPosDev_East) / 2048; |
// I-Part |
I_North = ((int32_t) staticParams.naviI * GPSPosDevIntegral_North) |
/ 8192; |
I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) / 8192; |
// combine P & I |
PID_North = P_North + I_North; |
PID_East = P_East + I_East; |
if (!GPS_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) { |
GPSPosDevIntegral_North += GPSPosDev_North / 16; |
GPSPosDevIntegral_East += GPSPosDev_East / 16; |
GPS_limitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, |
GPS_POSINTEGRAL_LIMIT); |
} |
// combine PI- and D-Part |
PID_North += D_North; |
PID_East += D_East; |
// scale combination with gain. |
// dongfang: Lets not do that. P I and D can be scaled instead. |
// PID_North = (PID_North * (int32_t) staticParams.NaviGpsGain) / 100; |
// PID_East = (PID_East * (int32_t) staticParams.NaviGpsGain) / 100; |
// GPS to nick and roll settings |
// A positive nick angle moves head downwards (flying forward). |
// A positive roll angle tilts left side downwards (flying left). |
// If compass heading is 0 the head of the copter is in north direction. |
// A positive nick angle will fly to north and a positive roll angle will fly to west. |
// In case of a positive north deviation/velocity the |
// copter should fly to south (negative nick). |
// In case of a positive east position deviation and a positive east velocity the |
// copter should fly to west (positive roll). |
// The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values |
// in the flight.c. Therefore a positive north deviation/velocity should result in a positive |
// GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
coscompass = int_cos(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
sincompass = int_sin(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
// limit resulting GPS control vector |
GPS_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
sticks[CONTROL_PITCH] += (int16_t) PID_Nick; |
sticks[CONTROL_ROLL] += (int16_t) PID_Roll; |
} else { // invalid GPS data or bad compass reading |
// reset error integral |
GPSPosDevIntegral_North = 0; |
GPSPosDevIntegral_East = 0; |
} |
} |
void navigation_periodicTask(int16_t* sticks) { |
static uint8_t GPS_P_Delay = 0; |
static uint16_t beep_rythm = 0; |
GPS_updateFlightMode(); |
// store home position if start of flight flag is set |
if (MKFlags & MKFLAG_CALIBRATE) { |
if (GPS_setCurrPosition(&homePosition)) |
beep(700); |
} |
switch (GPSInfo.status) { |
case INVALID: // invalid gps data |
if (flightMode != GPS_FLIGHT_MODE_FREE) { |
beep(100); // beep if signal is neccesary |
} |
break; |
case PROCESSED: // if gps data are already processed do nothing |
// downcount timeout |
if (GPSTimeout) |
GPSTimeout--; |
// if no new data arrived within timeout set current data invalid |
// and therefore disable GPS |
else { |
GPSInfo.status = INVALID; |
} |
break; |
case NEWDATA: // new valid data from gps device |
// if the gps data quality is good |
beep_rythm++; |
if (GPS_isSignalOK()) { |
switch (flightMode) { // check what's to do |
case GPS_FLIGHT_MODE_FREE: |
// update hold position to current gps position |
GPS_setCurrPosition(&holdPosition); // can get invalid if gps signal is bad |
// disable gps control |
break; |
case GPS_FLIGHT_MODE_AID: |
if (holdPosition.status != INVALID) { |
if (GPS_isManuallyControlled(sticks)) { // MK controlled by user |
// update hold point to current gps position |
GPS_setCurrPosition(&holdPosition); |
// disable gps control |
GPS_P_Delay = 0; |
} else { // GPS control active |
if (GPS_P_Delay < 7) { |
// delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
GPS_P_Delay++; |
GPS_setCurrPosition(&holdPosition); // update hold point to current gps position |
GPS_PIDController(NULL, sticks); // activates only the D-Part |
} else |
GPS_PIDController(&holdPosition, sticks); // activates the P&D-Part |
} |
} else // invalid Hold Position |
{ // try to catch a valid hold position from gps data input |
GPS_setCurrPosition(&holdPosition); |
} |
break; |
case GPS_FLIGHT_MODE_HOME: |
if (homePosition.status != INVALID) { |
// update hold point to current gps position |
// to avoid a flight back if home comming is deactivated |
GPS_setCurrPosition(&holdPosition); |
if (GPS_isManuallyControlled(sticks)) // MK controlled by user |
{ |
} else {// GPS control active |
GPS_PIDController(&homePosition, sticks); |
} |
} else { |
// bad home position |
beep(50); // signal invalid home position |
// try to hold at least the position as a fallback option |
if (holdPosition.status != INVALID) { |
if (GPS_isManuallyControlled(sticks)) { |
// MK controlled by user |
} else { |
// GPS control active |
GPS_PIDController(&holdPosition, sticks); |
} |
} else { // try to catch a valid hold position |
GPS_setCurrPosition(&holdPosition); |
} |
} |
break; // eof TSK_HOME |
default: // unhandled task |
break; // eof default |
} // eof switch GPS_Task |
} // eof gps data quality is good |
else // gps data quality is bad |
{ // disable gps control |
if (flightMode != GPS_FLIGHT_MODE_FREE) { |
// beep if signal is not sufficient |
if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) |
beep(100); |
else if (GPSInfo.satnum < staticParams.GPSMininumSatellites |
&& !(beep_rythm % 5)) |
beep(10); |
} |
} |
// set current data as processed to avoid further calculations on the same gps data |
GPSInfo.status = PROCESSED; |
break; |
} // eof GPSInfo.status |
} |
/branches/dongfang_FC_rewrite/eeprom.c |
---|
139,9 → 139,9 |
/***************************************************/ |
// setnumber [1..5] |
uint8_t paramSet_readFromEEProm(uint8_t setnumber) { |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(paramset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
output_init(); // what's that doing here?? |
return readChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(paramset_t)); |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(ParamSet_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
// output_init(); // what's that doing here?? |
return readChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(ParamSet_t)); |
} |
/***************************************************/ |
148,11 → 148,11 |
/* Write Parameter Set to EEPROM */ |
/***************************************************/ |
void paramSet_writeToEEProm(uint8_t setnumber) { |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(paramset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
writeChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(paramset_t)); |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(ParamSet_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
writeChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(ParamSet_t)); |
// set this parameter set to active set |
setActiveParamSet(setnumber); |
output_init(); // what's that doing here?? |
// output_init(); // what's that doing here?? |
} |
void paramSet_readOrDefault() { |
/branches/dongfang_FC_rewrite/externalControl.c |
---|
37,7 → 37,7 |
return 0; |
} |
void EC_update() { |
void EC_periodicTask() { |
if (externalControlActive) { |
externalControlActive--; |
EC_PRTY[CONTROL_PITCH] = externalControl.pitch * (int16_t) staticParams.stickP; |
/branches/dongfang_FC_rewrite/externalControl.h |
---|
20,7 → 20,7 |
extern ExternalControl_t externalControl; |
extern uint8_t externalControlActive; |
void EC_update(void); |
void EC_periodicTask(void); |
int16_t* EC_getPRTY(void); |
uint8_t EC_getArgument(void); |
uint8_t EC_getCommand(void); |
/branches/dongfang_FC_rewrite/flight.c |
---|
61,9 → 61,6 |
#include "attitude.h" |
#include "controlMixer.h" |
#include "commands.h" |
#ifdef USE_MK3MAG |
#include "gps.h" |
#endif |
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
159,10 → 156,6 |
uint8_t i, axis; |
// Fire the main flight attitude calculation, including integration of angles. |
// We want that to kick as early as possible, not to delay new AD sampling further. |
calculateFlightAttitude(); |
controlMixer_update(); |
throttleTerm = controls[CONTROL_THROTTLE]; |
// This check removed. Is done on a per-motor basis, after output matrix multiplication. |
/branches/dongfang_FC_rewrite/heightControl.c |
---|
29,7 → 29,7 |
iHeight = 0; |
} |
void HC_update(void) { |
void HC_periodicTask(void) { |
int32_t height = analog_getHeight(); |
static uint8_t setHeightLatch = 0; |
/branches/dongfang_FC_rewrite/heightControl.h |
---|
1,3 → 1,3 |
void HC_update(void); |
void HC_periodicTask(void); |
uint16_t HC_getThrottle(uint16_t throttle); |
void HC_setGround(void); |
/branches/dongfang_FC_rewrite/isqrt.S |
---|
0,0 → 1,203 |
;-----------------------------------------------------------------------------; |
; Fast integer squareroot routines for avr-gcc project (C)ChaN, 2008 |
; http://elm-chan.org/docs/avrlib/sqrt32.S |
;-----------------------------------------------------------------------------; |
; uint16_t isqrt32 (uint32_t n); |
; uint8_t isqrt16 (uint16_t n); |
; uint16_t ihypot (int16_t x, int16_t y); |
;-----------------------------------------------------------------------------: |
; 32bit integer squareroot |
;-----------------------------------------------------------------------------; |
; uint16_t isqrt32 ( |
; uint32_t n |
; ); |
; |
; Return Value: |
; Squareroot of n. |
; |
; Size = 53 words |
; Clock = 532..548 cycles |
; Stack = 0 byte |
.global isqrt32 |
.func isqrt32 |
isqrt32: |
clr r0 |
clr r18 |
clr r19 |
clr r20 |
ldi r21, 1 |
clr r27 |
clr r30 |
clr r31 |
ldi r26, 16 |
1: lsl r22 |
rol r23 |
rol r24 |
rol r25 |
rol r0 |
rol r18 |
rol r19 |
rol r20 |
lsl r22 |
rol r23 |
rol r24 |
rol r25 |
rol r0 |
rol r18 |
rol r19 |
rol r20 |
brpl 2f |
add r0, r21 |
adc r18, r27 |
adc r19, r30 |
adc r20, r31 |
rjmp 3f |
2: sub r0, r21 |
sbc r18, r27 |
sbc r19, r30 |
sbc r20, r31 |
3: lsl r21 |
rol r27 |
rol r30 |
andi r21, 0b11111000 |
ori r21, 0b00000101 |
sbrc r20, 7 |
subi r21, 2 |
dec r26 |
brne 1b |
lsr r30 |
ror r27 |
ror r21 |
lsr r30 |
ror r27 |
ror r21 |
mov r24, r21 |
mov r25, r27 |
ret |
.endfunc |
;-----------------------------------------------------------------------------: |
; 16bit integer squareroot |
;-----------------------------------------------------------------------------; |
; uint8_t isqrt16 ( |
; uint16_t n |
; ); |
; |
; Return Value: |
; Squareroot of n. |
; |
; Size = 33 words |
; Clock = 181..189 cycles |
; Stack = 0 byte |
.global isqrt16 |
.func isqrt16 |
isqrt16: |
clr r18 |
clr r19 |
ldi r20, 1 |
clr r21 |
ldi r22, 8 |
1: lsl r24 |
rol r25 |
rol r18 |
rol r19 |
lsl r24 |
rol r25 |
rol r18 |
rol r19 |
brpl 2f |
add r18, r20 |
adc r19, r21 |
rjmp 3f |
2: sub r18, r20 |
sbc r19, r21 |
3: lsl r20 |
rol r21 |
andi r20, 0b11111000 |
ori r20, 0b00000101 |
sbrc r19, 7 |
subi r20, 2 |
dec r22 |
brne 1b |
lsr r21 |
ror r20 |
lsr r21 |
ror r20 |
mov r24, r20 |
ret |
.endfunc |
;-----------------------------------------------------------------------------: |
; 16bit integer hypot (megaAVR is required) |
;-----------------------------------------------------------------------------; |
; uint16_t ihypot ( |
; int16_t x, |
; int16_t y |
; ); |
; |
; Return Value: |
; Squareroot of (x*x + y*y) |
; |
; Size = 42 words |
; Clock = 581..597 cycles |
; Stack = 0 byte |
.global ihypot |
.func ihypot |
ihypot: |
clr r26 |
sbrs r25, 7 |
rjmp 1f |
com r24 |
com r25 |
adc r24, r26 |
adc r25, r26 |
1: sbrs r23, 7 |
rjmp 2f |
com r22 |
com r23 |
adc r22, r26 |
adc r23, r26 |
2: mul r22, r22 |
movw r18, r0 |
mul r23, r23 |
movw r20, r0 |
mul r22, r23 |
add r19, r0 |
adc r20, r1 |
adc r21, r26 |
add r19, r0 |
adc r20, r1 |
adc r21, r26 |
mul r24, r24 |
movw r30, r0 |
mul r25, r25 |
add r18, r30 |
adc r19, r31 |
adc r20, r0 |
adc r21, r1 |
mul r24, r25 |
add r19, r0 |
adc r20, r1 |
adc r21, r26 |
add r19, r0 |
adc r20, r1 |
adc r21, r26 |
movw r24, r20 |
movw r22, r18 |
clr r1 |
rjmp isqrt32 |
.endfunc |
/branches/dongfang_FC_rewrite/isqrt.h |
---|
0,0 → 1,11 |
#ifndef _ISQRT_H |
#define _ISQRT_H |
#include <inttypes.h> |
// coded in assembler file |
extern uint16_t isqrt32(uint32_t n); |
extern uint8_t isqrt16(uint16_t n); |
extern uint16_t ihypot(int16_t x, int16_t y); |
#endif // _ISQRT_H |
/branches/dongfang_FC_rewrite/main.c |
---|
72,7 → 72,7 |
#ifdef USE_NAVICTRL |
#include "spi.h" |
#endif |
#ifdef USE_MK3MAG |
#ifdef USE_DIRECT_GPS |
#include "mk3mag.h" |
#endif |
#include "eeprom.h" |
108,8 → 108,9 |
#ifdef USE_NAVICTRL |
SPI_MasterInit(); |
#endif |
#ifdef USE_MK3MAG |
MK3MAG_Init(); |
#ifdef USE_DIRECT_GPS |
MK3MAG_init(); |
usart1_init(); |
#endif |
// Parameter Set handling |
162,23 → 163,12 |
#ifdef USE_NAVICTRL |
printf("\n\rSupport for NaviCtrl"); |
#ifdef USE_RC_DSL |
printf("\r\nSupport for DSL RC at 2nd UART"); |
#endif |
#ifdef USE_RC_SPECTRUM |
printf("\r\nSupport for SPECTRUM RC at 2nd UART"); |
#endif |
#endif |
#ifdef USE_MK3MAG |
printf("\n\rSupport for MK3MAG Compass"); |
#ifdef USE_DIRECT_GPS |
printf("\n\rDirect (no NaviCtrl) navigation"); |
#endif |
#if (defined (USE_MK3MAG)) |
if(CPUType == ATMEGA644P) printf("\n\r2 UART"); |
else printf("\n\r1 UART"); |
#endif |
controlMixer_setNeutral(); |
// Cal. attitude sensors and reset integrals. |
210,6 → 200,8 |
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
J4HIGH; |
calculateFlightAttitude(); |
controlMixer_periodicTask(); |
flight_control(); |
J4LOW; |
/branches/dongfang_FC_rewrite/makefile |
---|
16,13 → 16,9 |
# Use one of the extensions for a gps solution |
#EXT = NAVICTRL |
#EXT = MK3MAG |
EXT = |
EXT = DIRECT_GPS |
#EXT = |
# Use optional one the RCs if EXT = NAVICTRL has been used |
#RC = DSL |
#RC = SPECTRUM |
#GYRO=ENC-03_FC1.3 |
#GYRO_HW_NAME=ENC |
#GYRO_HW_FACTOR=1.304f |
48,19 → 44,15 |
ifeq ($(MCU), atmega644) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
HEX_NAME = MEGA644_$(EXT)_$(RC)_$(GYRO) |
HEX_NAME = MEGA644_$(EXT)_$(GYRO) |
endif |
ifeq ($(MCU), atmega644p) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
HEX_NAME = MEGA644p_$(EXT)_$(RC)_$(GYRO) |
HEX_NAME = MEGA644p_$(EXT)_$(GYRO) |
endif |
ifeq ($(F_CPU), 16000000) |
QUARZ = 16MHZ |
endif |
ifeq ($(F_CPU), 20000000) |
QUARZ = 20MHZ |
endif |
137,22 → 129,16 |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c output.c controlMixer.c |
SRC += externalControl.c GPSControl.c dongfangMath.c twimaster.c rc.c attitude.c flight.c |
SRC += eeprom.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c |
SRC += externalControl.c dongfangMath.c twimaster.c rc.c attitude.c flight.c |
SRC += eeprom.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c |
ifeq ($(EXT), MK3MAG) |
SRC += mk3mag.c |
#mymath.c |
ifeq ($(EXT), DIRECT_GPS) |
SRC += mk3mag.c directGPSNaviControl.c uart1.c ubx.c |
endif |
ifeq ($(EXT), NAVICTRL) |
SRC += spi.c |
ifeq ($(RC), DSL) |
SRC += dsl.c |
endif |
ifeq ($(RC), SPECTRUM) |
SRC += spectrum.c |
endif |
endif |
########################################################################################################## |
# List Assembler source files here. |
162,7 → 148,7 |
# Even though the DOS/Win* filesystem matches both .s and .S the same, |
# it will preserve the spelling of the filenames, and gcc itself does |
# care about how the name is spelled on its command-line. |
ASRC = |
ASRC = isqrt.S |
# List any extra directories to look for include files here. |
# Each directory must be seperated by a space. |
194,18 → 180,12 |
CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) -DGYRO_HW_NAME=${GYRO_HW_NAME} -DGYRO_HW_FACTOR=${GYRO_HW_FACTOR} -DGYRO_PITCHROLL_CORRECTION=${GYRO_PITCHROLL_CORRECTION} -DGYRO_YAW_CORRECTION=${GYRO_YAW_CORRECTION} |
ifeq ($(EXT), MK3MAG) |
CFLAGS += -DUSE_MK3MAG |
ifeq ($(EXT), DIRECT_GPS) |
CFLAGS += -DUSE_DIRECT_GPS |
endif |
ifeq ($(EXT), NAVICTRL) |
CFLAGS += -DUSE_NAVICTRL |
ifeq ($(RC), DSL) |
CFLAGS += -DUSE_RC_DSL |
endif |
ifeq ($(RC), SPECTRUM) |
CFLAGS += -DUSE_RC_SPECTRUM |
endif |
endif |
ifeq ($(SETUP), QUADRO) |
CFLAGS += -DUSE_QUADRO |
407,16 → 387,10 |
@echo $(MSG_EXTENDED_COFF) $(TARGET).cof |
$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof |
# Program the device. |
program: $(TARGET).hex $(TARGET).eep |
$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) |
# Create final output files (.hex, .eep) from ELF output file. |
%.hex: %.elf |
@echo |
/branches/dongfang_FC_rewrite/mk3mag.c |
---|
65,21 → 65,19 |
/*********************************************/ |
/* Initialize Interface to MK3MAG Compass */ |
/*********************************************/ |
void MK3MAG_Init(void) { |
void MK3MAG_init(void) { |
// Port PC4 connected to PWM output from compass module |
DDRC &= ~(1 << DDC4); // set as input |
PORTC |= (1 << PORTC4); // pull up to increase PWM counter also if nothing is connected |
PWMTimeout = 0; |
toMk3Mag.CalState = 0; |
toMk3Mag.Orientation = 1; |
toMk3Mag.calState = 0; |
toMk3Mag.orientation = 1; |
} |
/*********************************************/ |
/* Get PWM from MK3MAG */ |
/*********************************************/ |
void MK3MAG_Update(void) {// called every 102.4 us by timer 0 ISR |
void MK3MAG_periodicTask(void) {// called every 102.4 us by timer 0 ISR |
static uint16_t PWMCount = 0; |
static uint16_t beepDelay = 0; |
static uint16_t debugCounter = 0; |
/branches/dongfang_FC_rewrite/mk3mag.h |
---|
2,18 → 2,18 |
#define _MK3MAG_H |
typedef struct { |
int16_t Attitude[2]; |
uint8_t UserParam[2]; |
uint8_t CalState; |
uint8_t Orientation; |
int16_t attitude[2]; |
uint8_t userParam[2]; |
uint8_t calState; |
uint8_t orientation; |
} ToMk3Mag_t; |
extern ToMk3Mag_t toMk3Mag; |
// Initialization |
void MK3MAG_Init(void); |
void MK3MAG_init(void); |
// should be called cyclic to get actual compass heading |
void MK3MAG_Update(void); |
void MK3MAG_periodicTask(void); |
#endif //_MK3MAG_H |
/branches/dongfang_FC_rewrite/naviControl.h |
---|
1,8 → 1,9 |
//void NC_update(void); |
//int16_t* NC_getPRTY(void); |
//uint8_t NC_getArgument(void); |
//uint8_t NC_getCommand(void); |
//int16_t NC_getVariable(uint8_t varNum); |
//void NC_calibrate(void); |
//uint8_t NC_getSignalQuality (void); |
//void NC_setNeutral(void); |
#ifndef _NAVICONTROL_H |
#define _NAVICONTROL_H |
#include<inttypes.h> |
void navi_setNeutral(void); |
void navigation_periodicTask(int16_t* naviSticks); |
#endif |
/branches/dongfang_FC_rewrite/rc.c |
---|
239,7 → 239,7 |
/* |
* This must be called (as the only thing) for each control loop cycle (488 Hz). |
*/ |
void RC_update() { |
void RC_periodicTask() { |
int16_t tmp1, tmp2; |
if (RCQuality) { |
RCQuality--; |
/branches/dongfang_FC_rewrite/rc.h |
---|
30,7 → 30,7 |
uint8_t RC_hasNewRCData (void); |
*/ |
void RC_update(void); |
void RC_periodicTask(void); |
int16_t* RC_getPRTY(void); |
uint8_t RC_getArgument(void); |
uint8_t RC_getCommand(void); |
/branches/dongfang_FC_rewrite/timer0.c |
---|
60,7 → 60,7 |
#include "uart0.h" |
#include "output.h" |
#ifdef USE_MK3MAG |
#ifdef USE_DIRECT_GPS |
#include "mk3mag.h" |
#endif |
187,8 → 187,8 |
#ifndef USE_NAVICTRL |
// update compass value if this option is enabled in the settings |
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
#ifdef USE_MK3MAG |
MK3MAG_Update(); // read out mk3mag pwm |
#ifdef USE_DIRECT_GPS |
MK3MAG_periodicTask(); // read out mk3mag pwm |
#endif |
} |
#endif |
/branches/dongfang_FC_rewrite/timer2.c |
---|
55,9 → 55,9 |
#include "rc.h" |
#include "attitude.h" |
#define SLOW 1 |
#define COARSERESOLUTION 1 |
#ifdef SLOW |
#ifdef COARSERESOLUTION |
#define NEUTRAL_PULSELENGTH 938 |
#define SERVOLIMIT 500 |
#define SCALE_FACTOR 4 |
/branches/dongfang_FC_rewrite/uart0.c |
---|
14,7 → 14,7 |
#include "output.h" |
#include "attitude.h" |
#ifdef USE_MK3MAG |
#ifdef USE_DIRECT_GPS |
#include "mk3mag.h" |
#endif |
51,7 → 51,7 |
uint8_t confirmFrame; |
typedef struct { |
int16_t Heading; |
int16_t heading; |
}__attribute__((packed)) Heading_t; |
DebugOut_t debugOut; |
62,8 → 62,8 |
uint16_t debugData_interval = 0; // in 1ms |
uint16_t data3D_interval = 0; // in 1ms |
#ifdef USE_MK3MAG |
int16_t compass_timer; |
#ifdef USE_DIRECT_GPS |
int16_t toMk3MagTimer; |
#endif |
// keep lables in flash to save 512 bytes of sram space |
170,8 → 170,8 |
// no bytes to send |
txd_complete = TRUE; |
#ifdef USE_MK3MAG |
compass_timer = setDelay(220); |
#ifdef USE_DIRECT_GPS |
toMk3MagTimer = setDelay(220); |
#endif |
versionInfo.SWMajor = VERSION_MAJOR; |
446,9 → 446,9 |
case FC_ADDRESS: |
switch (rxd_buffer[2]) { |
#ifdef USE_MK3MAG |
#ifdef USE_DIRECT_GPS |
case 'K':// compass value |
compassHeading = ((Heading_t *)pRxData)->Heading; |
compassHeading = ((Heading_t *)pRxData)->heading; |
// compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
break; |
#endif |
667,17 → 667,17 |
request_externalControl = FALSE; |
} |
#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; |
#ifdef USE_DIRECT_GPS |
if((checkDelay(toMk3MagTimer)) && 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)); |
// 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); |
toMk3MagTimer = setDelay(99); |
} |
#endif |
/branches/dongfang_FC_rewrite/uart1.c |
---|
1,96 → 1,28 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "uart1.h" |
#include "configuration.h" |
#if defined (USE_MK3MAG) |
#else |
#ifdef USE_RC_DSL |
#include "dsl.h" |
#endif |
#ifdef USE_RC_SPECTRUM |
#include "spectrum.h" |
#endif |
#endif |
#include "ubx.h" |
#ifndef USART1_BAUD |
#define USART1_BAUD 57600 |
#endif |
/****************************************************************/ |
/* Initialization of the USART1 */ |
/****************************************************************/ |
void usart1_Init(void) { |
void usart1_init(void) { |
// USART1 Control and Status Register A, B, C and baud rate register |
uint8_t sreg = SREG; |
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART1_BAUD) - 1); |
uint16_t ubrr = (uint16_t) ((uint32_t) F_CPU / (8 * USART1_BAUD) - 1); |
// disable all interrupts before reconfiguration |
cli(); |
// disable RX-Interrupt |
UCSR1B &= ~(1 << RXCIE1); |
// disable TX-Interrupt |
UCSR1B &= ~(1 << TXCIE1); |
// disable DRE-Interrupt |
UCSR1B &= ~(1 << UDRIE1); |
// disable RX-Interrupt, disable TX-Interrupt, disable DRE-Interrupt |
UCSR1B &= ~ ((1 << RXCIE1) | (1 << TXCIE1) | (1 << UDRIE1)); |
// set direction of RXD1 and TXD1 pins |
// set RXD1 (PD2) as an input pin |
PORTD |= (1 << PORTD2); |
// set RXD1 (PD2) as an input pin, set TXD1 (PD3) as an output pin |
PORTD |= (1 << PORTD2) | (1 << PORTD3); |
DDRD &= ~(1 << DDD2); |
// set TXD1 (PD3) as an output pin |
PORTD |= (1 << PORTD3); |
DDRD |= (1 << DDD3); |
// USART0 Baud Rate Register |
102,18 → 34,12 |
UCSR1A |= (1 << U2X1); |
// enable receiver and transmitter |
UCSR1B = (1 << TXEN1) | (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); |
// set asynchronous mode, no parity, 1 stop bit |
UCSR1C &= ~((1 << UMSEL11) | (1 << UMSEL10) | (1 << UPM11) | (1 << UPM10) | (1 << USBS1)); |
// 8-bit |
UCSR1B &= ~(1 << UCSZ12); |
UCSR1C |= (1 << UCSZ11); |
UCSR1C |= (1 << UCSZ10); |
UCSR1C |= (1 << UCSZ11) | (1 << UCSZ10); |
// flush receive buffer explicit |
while (UCSR1A & (1 << RXC1)) |
147,14 → 73,6 |
/****************************************************************/ |
/* USART1 receiver ISR */ |
/****************************************************************/ |
ISR(USART1_RX_vect) |
{ |
uint8_t c; |
c = UDR1; // get data byte |
#ifdef USE_RC_DSL |
dsl_parser(c); // parse dsl data stream |
#endif |
#ifdef USE_RC_SPECTRUM |
spectrum_parser(c); // parse spectrum data stream |
#endif |
ISR(USART1_RX_vect) { |
ubx_parser(UDR1); // get data byte and put it into the ubx protocol parser |
} |
/branches/dongfang_FC_rewrite/uart1.h |
---|
1,6 → 1,6 |
#ifndef _UART1_H |
#define _UART1_H |
extern void usart1_Init(void); |
extern void usart1_init(void); |
#endif //_UART1_H |
/branches/dongfang_FC_rewrite/ubx.c |
---|
0,0 → 1,230 |
#include <inttypes.h> |
#include "ubx.h" |
//#include "main.h" |
#include <avr/io.h> |
// ubx protocol parser state machine |
#define UBXSTATE_IDLE 0 |
#define UBXSTATE_SYNC1 1 |
#define UBXSTATE_SYNC2 2 |
#define UBXSTATE_CLASS 3 |
#define UBXSTATE_LEN1 4 |
#define UBXSTATE_LEN2 5 |
#define UBXSTATE_DATA 6 |
#define UBXSTATE_CKA 7 |
#define UBXSTATE_CKB 8 |
// ublox protocoll identifier |
#define UBX_CLASS_NAV 0x01 |
#define UBX_ID_POSLLH 0x02 |
#define UBX_ID_SOL 0x06 |
#define UBX_ID_VELNED 0x12 |
#define UBX_SYNC1_CHAR 0xB5 |
#define UBX_SYNC2_CHAR 0x62 |
typedef struct { |
uint32_t ITOW; // ms GPS Millisecond Time of Week |
int32_t Frac; // ns remainder of rounded ms above |
int16_t week; // GPS week |
uint8_t GPSfix; // GPSfix Type, range 0..6 |
uint8_t Flags; // Navigation Status Flags |
int32_t ECEF_X; // cm ECEF X coordinate |
int32_t ECEF_Y; // cm ECEF Y coordinate |
int32_t ECEF_Z; // cm ECEF Z coordinate |
uint32_t PAcc; // cm 3D Position Accuracy Estimate |
int32_t ECEFVX; // cm/s ECEF X velocity |
int32_t ECEFVY; // cm/s ECEF Y velocity |
int32_t ECEFVZ; // cm/s ECEF Z velocity |
uint32_t SAcc; // cm/s Speed Accuracy Estimate |
uint16_t PDOP; // 0.01 Position DOP |
uint8_t res1; // reserved |
uint8_t numSV; // Number of SVs used in navigation solution |
uint32_t res2; // reserved |
Status_t Status; |
} UBX_SOL_t; |
typedef struct { |
uint32_t ITOW; // ms GPS Millisecond Time of Week |
int32_t LON; // 1e-07 deg Longitude |
int32_t LAT; // 1e-07 deg Latitude |
int32_t HEIGHT; // mm Height above Ellipsoid |
int32_t HMSL; // mm Height above mean sea level |
uint32_t Hacc; // mm Horizontal Accuracy Estimate |
uint32_t Vacc; // mm Vertical Accuracy Estimate |
Status_t Status; |
} UBX_POSLLH_t; |
typedef struct { |
uint32_t ITOW; // ms GPS Millisecond Time of Week |
int32_t VEL_N; // cm/s NED north velocity |
int32_t VEL_E; // cm/s NED east velocity |
int32_t VEL_D; // cm/s NED down velocity |
uint32_t Speed; // cm/s Speed (3-D) |
uint32_t GSpeed; // cm/s Ground Speed (2-D) |
int32_t Heading; // 1e-05 deg Heading 2-D |
uint32_t SAcc; // cm/s Speed Accuracy Estimate |
uint32_t CAcc; // deg Course / Heading Accuracy Estimate |
Status_t Status; |
} UBX_VELNED_t; |
UBX_SOL_t UbxSol = |
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID }; |
UBX_POSLLH_t UbxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID }; |
UBX_VELNED_t UbxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID }; |
GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID }; |
volatile uint8_t GPSTimeout = 0; |
void updateGPSInfo(void) { |
if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) |
&& (UbxVelNed.Status == NEWDATA)) { |
//RED_FLASH; |
// DebugOut.Digital ....blah... |
if (GPSInfo.status != NEWDATA) { |
GPSInfo.status = INVALID; |
// NAV SOL |
GPSInfo.flags = UbxSol.Flags; |
GPSInfo.satfix = UbxSol.GPSfix; |
GPSInfo.satnum = UbxSol.numSV; |
GPSInfo.PAcc = UbxSol.PAcc; |
GPSInfo.VAcc = UbxSol.SAcc; |
// NAV POSLLH |
GPSInfo.longitude = UbxPosLlh.LON; |
GPSInfo.latitude = UbxPosLlh.LAT; |
GPSInfo.altitude = UbxPosLlh.HEIGHT; |
GPSInfo.veleast = UbxVelNed.VEL_E; |
GPSInfo.velnorth = UbxVelNed.VEL_N; |
GPSInfo.veltop = -UbxVelNed.VEL_D; |
GPSInfo.velground = UbxVelNed.GSpeed; |
GPSInfo.status = NEWDATA; |
} |
// set state to collect new data |
UbxSol.Status = PROCESSED; // never update old data |
UbxPosLlh.Status = PROCESSED; // never update old data |
UbxVelNed.Status = PROCESSED; // never update old data |
} |
} |
// this function should be called within the UART RX ISR |
void ubx_parser(uint8_t c) { |
static uint8_t ubxstate = UBXSTATE_IDLE; |
static uint8_t cka, ckb; |
static uint16_t msglen; |
static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered |
switch (ubxstate) { |
case UBXSTATE_IDLE: // check 1st sync byte |
if (c == UBX_SYNC1_CHAR) |
ubxstate = UBXSTATE_SYNC1; |
else |
ubxstate = UBXSTATE_IDLE; // out of synchronization |
break; |
case UBXSTATE_SYNC1: // check 2nd sync byte |
if (c == UBX_SYNC2_CHAR) |
ubxstate = UBXSTATE_SYNC2; |
else |
ubxstate = UBXSTATE_IDLE; // out of synchronization |
break; |
case UBXSTATE_SYNC2: // check msg class to be NAV |
if (c == UBX_CLASS_NAV) |
ubxstate = UBXSTATE_CLASS; |
else |
ubxstate = UBXSTATE_IDLE; // unsupported message class |
break; |
case UBXSTATE_CLASS: // check message identifier |
switch (c) { |
case UBX_ID_POSLLH: // geodetic position |
ubxP = (int8_t *) &UbxPosLlh; // data start pointer |
ubxEp = (int8_t *) (&UbxPosLlh + 1); // data end pointer |
ubxSp = (int8_t *) &UbxPosLlh.Status; // status pointer |
break; |
case UBX_ID_SOL: // navigation solution |
ubxP = (int8_t *) &UbxSol; // data start pointer |
ubxEp = (int8_t *) (&UbxSol + 1); // data end pointer |
ubxSp = (int8_t *) &UbxSol.Status; // status pointer |
break; |
case UBX_ID_VELNED: // velocity vector in tangent plane |
ubxP = (int8_t *) &UbxVelNed; // data start pointer |
ubxEp = (int8_t *) (&UbxVelNed + 1); // data end pointer |
ubxSp = (int8_t *) &UbxVelNed.Status; // status pointer |
break; |
default: // unsupported identifier |
ubxstate = UBXSTATE_IDLE; |
break; |
} |
if (ubxstate != UBXSTATE_IDLE) { |
ubxstate = UBXSTATE_LEN1; |
cka = UBX_CLASS_NAV + c; |
ckb = UBX_CLASS_NAV + cka; |
} |
break; |
case UBXSTATE_LEN1: // 1st message length byte |
msglen = c; |
cka += c; |
ckb += cka; |
ubxstate = UBXSTATE_LEN2; |
break; |
case UBXSTATE_LEN2: // 2nd message length byte |
msglen += ((uint16_t) c) << 8; |
cka += c; |
ckb += cka; |
// if the old data are not processed so far then break parsing now |
// to avoid writing new data in ISR during reading by another function |
if (*ubxSp == NEWDATA) { |
updateGPSInfo(); //update GPS info respectively |
ubxstate = UBXSTATE_IDLE; |
} else // data invalid or allready processd |
{ |
*ubxSp = INVALID; |
ubxstate = UBXSTATE_DATA; |
} |
break; |
case UBXSTATE_DATA: |
if (ubxP < ubxEp) |
*ubxP++ = c; // copy curent data byte if any space is left |
cka += c; |
ckb += cka; |
if (--msglen == 0) |
ubxstate = UBXSTATE_CKA; // switch to next state if all data was read |
break; |
case UBXSTATE_CKA: |
if (c == cka) |
ubxstate = UBXSTATE_CKB; |
else { |
*ubxSp = INVALID; |
ubxstate = UBXSTATE_IDLE; |
} |
break; |
case UBXSTATE_CKB: |
if (c == ckb) { |
*ubxSp = NEWDATA; // new data are valid |
updateGPSInfo(); //update GPS info respectively |
GPSTimeout = 255; |
} else { // if checksum not match then set data invalid |
*ubxSp = INVALID; |
} |
ubxstate = UBXSTATE_IDLE; // ready to parse new data |
break; |
default: // unknown ubx state |
ubxstate = UBXSTATE_IDLE; |
break; |
} |
} |
/branches/dongfang_FC_rewrite/ubx.h |
---|
0,0 → 1,53 |
#ifndef _UBX_H |
#define _UBX_H |
#include <inttypes.h> |
typedef enum { |
INVALID, NEWDATA, PROCESSED |
} Status_t; |
// Satfix types for GPSData.satfix |
#define SATFIX_NONE 0x00 |
#define SATFIX_DEADRECKOING 0x01 |
#define SATFIX_2D 0x02 |
#define SATFIX_3D 0x03 |
#define SATFIX_GPS_DEADRECKOING 0x04 |
#define SATFIX_TIMEONLY 0x05 |
// Flags for interpretation of the GPSData.flags |
#define FLAG_GPSFIXOK 0x01 // (i.e. within DOP & ACC Masks) |
#define FLAG_DIFFSOLN 0x02 // (is DGPS used) |
#define FLAG_WKNSET 0x04 // (is Week Number valid) |
#define FLAG_TOWSET 0x08 // (is Time of Week valid) |
/* enable the UBX protocol at the gps receiver with the following messages enabled |
01-02 NAV - POSLLH |
01-06 Nav - SOL |
01-12 NAV - VELNED */ |
typedef struct { |
uint8_t flags; // flags |
uint8_t satnum; // number of satelites |
uint8_t satfix; // type of satfix |
int32_t longitude; // in 1e-07 deg |
int32_t latitude; // in 1e-07 deg |
int32_t altitude; // in mm |
uint32_t PAcc; // in cm 3d position accuracy |
int32_t velnorth; // in cm/s |
int32_t veleast; // in cm/s |
int32_t veltop; // in cm/s |
uint32_t velground; // 2D ground speed in cm/s |
uint32_t VAcc; // in cm/s 3d velocity accuracy |
Status_t status; // status of data: invalid | valid |
} GPS_INFO_t; |
//here you will find the current gps info |
extern GPS_INFO_t GPSInfo; // measured position (last gps record) |
// this variable should be decremted by the application |
extern volatile uint8_t GPSTimeout; // is reset to 255 if a new UBX msg was received |
#define USART1_BAUD 57600 |
// this function should be called within the UART RX ISR |
extern void ubx_parser(uint8_t c); |
#endif //_UBX_H |