Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2038 → Rev 2039

/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