Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2160 → Rev 2189

/branches/dongfang_FC_rewrite/directGPSNaviControl.c
9,7 → 9,6
#include "output.h"
#include "isqrt.h"
#include "attitude.h"
#include "dongfangMath.h"
#include "attitude.h"
 
typedef enum {
17,7 → 16,7
NAVI_FLIGHT_MODE_FREE,
NAVI_FLIGHT_MODE_AID,
NAVI_FLIGHT_MODE_HOME,
} FlightMode_t;
} NaviMode_t;
 
/*
* This is not read from internally. It only serves to let a pilot/debugger
53,7 → 52,7
// GPS coordinates for home position
GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
// the current flight mode
FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;
NaviMode_t naviMode = NAVI_FLIGHT_MODE_UNDEF;
int16_t naviSticks[2] = {0,0};
 
int8_t naviStatus;
60,22 → 59,22
 
// ---------------------------------------------------------------------------------
void navi_updateFlightMode(void) {
static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF;
static NaviMode_t naviModeOld = NAVI_FLIGHT_MODE_UNDEF;
 
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
flightMode = NAVI_FLIGHT_MODE_FREE;
naviMode = NAVI_FLIGHT_MODE_FREE;
} else {
if (dynamicParams.naviMode < 50)
flightMode = NAVI_FLIGHT_MODE_FREE;
naviMode = NAVI_FLIGHT_MODE_FREE;
else if (dynamicParams.naviMode < 180)
flightMode = NAVI_FLIGHT_MODE_AID;
naviMode = NAVI_FLIGHT_MODE_AID;
else
flightMode = NAVI_FLIGHT_MODE_HOME;
naviMode = NAVI_FLIGHT_MODE_HOME;
}
 
if (flightMode != flightModeOld) {
if (naviMode != naviModeOld) {
beep(100);
flightModeOld = flightMode;
naviModeOld = naviMode;
}
}
 
107,11 → 106,9
}
 
// checks nick and roll sticks for manual control
uint8_t navi_isManuallyControlled(int16_t* PRTY) {
debugOut.analog[26] = PRTY[CONTROL_PITCH];
debugOut.analog[27] = PRTY[CONTROL_ROLL];
if (abs(PRTY[CONTROL_PITCH]) < staticParams.naviStickThreshold
&& abs(PRTY[CONTROL_ROLL]) < staticParams.naviStickThreshold)
uint8_t navi_isManuallyControlled(int16_t* RPTY) {
if (abs(RPTY[CONTROL_PITCH]) < staticParams.naviStickThreshold
&& abs(RPTY[CONTROL_ROLL]) < staticParams.naviStickThreshold)
return 0;
else
return 1;
268,7 → 265,7
}
}
 
void navigation_periodicTaskAndPRTY(int16_t* PRTY) {
void navigation_periodicTaskAndRPTY(int16_t* RPTY) {
static uint8_t GPS_P_Delay = 0;
static uint16_t beep_rythm = 0;
static uint16_t navi_testOscPrescaler = 0;
280,7 → 277,7
return;
}
 
navi_updateFlightMode();
navi_updateNaviMode();
 
navi_testOscPrescaler++;
if (navi_testOscPrescaler == 488) {
314,7 → 311,7
case INVALID: // invalid gps data
navi_setNeutral();
naviStatus = NAVI_STATUS_INVALID_GPS;
if (flightMode != NAVI_FLIGHT_MODE_FREE) {
if (naviMode != NAVI_FLIGHT_MODE_FREE) {
beep(1); // beep if signal is neccesary
}
break;
334,7 → 331,7
// if the gps data quality is good
beep_rythm++;
if (navi_isGPSSignalOK()) {
switch (flightMode) { // check what's to do
switch (naviMode) { // check what's to do
case NAVI_FLIGHT_MODE_FREE:
// update hold position to current gps position
navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
345,7 → 342,7
 
case NAVI_FLIGHT_MODE_AID:
if (holdPosition.status != INVALID) {
if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
if (navi_isManuallyControlled(RPTY)) { // MK controlled by user
// update hold point to current gps position
navi_writeCurrPositionTo(&holdPosition);
// disable gps control
377,7 → 374,7
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
navi_writeCurrPositionTo(&holdPosition);
if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
if (navi_isManuallyControlled(RPTY)) { // MK controlled by user
navi_setNeutral();
naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
} else {// GPS control active
389,7 → 386,7
beep(50); // signal invalid home position
// try to hold at least the position as a fallback option
if (holdPosition.status != INVALID) {
if (navi_isManuallyControlled(PRTY)) {
if (navi_isManuallyControlled(RPTY)) {
// MK controlled by user
navi_setNeutral();
naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
414,7 → 411,7
// disable gps control
navi_setNeutral();
naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL;
if (flightMode != NAVI_FLIGHT_MODE_FREE) {
if (naviMode != NAVI_FLIGHT_MODE_FREE) {
// beep if signal is not sufficient
if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))
beep(100);
428,14 → 425,6
break;
} // eof GPSInfo.status
 
PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
 
//debugOut.analog[16] = flightMode;
//debugOut.analog[17] = naviStatus;
 
/*
debugOut.analog[18] = naviSticks[CONTROL_PITCH];
debugOut.analog[19] = naviSticks[CONTROL_ROLL];
*/
RPTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
RPTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
}