rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

OP-119 AHRS: Removed WMM computation for now because it was triggering a reset (probably not watchdog) which is weird - it worked on AHRS. Maybe running out of memory? Also added the break on comm error in ahrs_comms to resync with it. I think the message to send the home location is also not working right now. Fixed chip reset.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1340 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-20 14:48:02 +00:00 committed by peabody124
parent adc6385880
commit 86766ecc14
2 changed files with 23 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
View File

@ -100,6 +100,7 @@ static void HomeLocationUpdatedCb(UAVObjEvent * ev)
HomeLocationIsUpdatedFlag = true;
}
static bool AHRSKnowsHome = FALSE;
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -129,6 +130,9 @@ static void ahrscommsTask(void* parameters)
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
/* Whenever resyncing, assume AHRS doesn't reset and doesn't know home */
AHRSKnowsHome = FALSE;
/* Spin here until we're in sync */
while (PIOS_OPAHRS_resync() != OPAHRS_RESULT_OK) {
vTaskDelay(100 / portTICK_RATE_MS);
@ -175,6 +179,7 @@ static void ahrscommsTask(void* parameters)
AltitudeActualIsUpdatedFlag = false;
} else {
/* Comms error */
break;
}
}
@ -186,19 +191,22 @@ static void ahrscommsTask(void* parameters)
PositionActualIsUpdatedFlag = false;
} else {
/* Comms error */
break;
}
}
if (HomeLocationIsUpdatedFlag) {
if (HomeLocationIsUpdatedFlag || !AHRSKnowsHome) {
struct opahrs_msg_v1 req;
load_magnetic_north(&(req.payload.user.v.req.north));
if (PIOS_OPAHRS_SetMagNorth(&req) == OPAHRS_RESULT_OK) {
HomeLocationIsUpdatedFlag = false;
AHRSKnowsHome = TRUE;
} else {
/* Comms error */
break;
}
}
}
/* Wait for the next update interval */
vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );

25
flight/OpenPilot/Modules/GPS/GPS.c
View File

@ -65,7 +65,7 @@
// Private functions
static void gpsTask(void* parameters);
static void setHomeLocation(PositionActualData gpsData);
static void setHomeLocation(PositionActualData * gpsData);
// functions
char* nmeaGetPacketBuffer(void);
@ -120,6 +120,7 @@ int32_t GPSInitialize(void)
return 0;
}
static bool homeLocationSet = 0;
/**
* gps task. Processes input buffer. It does not return.
@ -131,9 +132,9 @@ static void gpsTask(void* parameters)
portTickType xDelay = 100 / portTICK_RATE_MS;
PositionActualData GpsData;
uint32_t timeNowMs;
uint8_t homeLocationSet = 0;
homeLocationSet = 0;
// Loop forever
while(1)
{
@ -161,9 +162,9 @@ static void gpsTask(void* parameters)
else {
// Had an update
PositionActualGet(&GpsData);
if(GpsData.Status == POSITIONACTUAL_STATUS_FIX3D && !homeLocationSet ) {
setHomeLocation(GpsData);
homeLocationSet = 1;
if(GpsData.Status == POSITIONACTUAL_STATUS_FIX3D && homeLocationSet == FALSE) {
setHomeLocation(&GpsData);
homeLocationSet = TRUE;
}
}
@ -172,15 +173,15 @@ static void gpsTask(void* parameters)
}
}
static void setHomeLocation(PositionActualData gpsData)
static void setHomeLocation(PositionActualData * gpsData)
{
HomeLocationData home;
HomeLocationGet(&home);
// Store LLA
home.Latitude = (int32_t) gpsData.Latitude * 10e6;
home.Longitude = (int32_t) gpsData.Longitude * 10e6;
home.Altitude = gpsData.GeoidSeparation;
home.Latitude = (int32_t) (gpsData->Latitude * 10e6);
home.Longitude = (int32_t) (gpsData->Longitude * 10e6);
home.Altitude = gpsData->GeoidSeparation;
// Compute home ECEF coordinates and the rotation matrix into NED
double LLA[3] = {(double) home.Latitude / 10e6, (double) home.Longitude / 10e6, (double) home.Altitude};
@ -197,9 +198,9 @@ static void setHomeLocation(PositionActualData gpsData)
memcpy(&home.RNE[0], &RNE[0][0], 9 * sizeof(RNE[0][0]));
// Compute magnetic flux direction at home location
WMM_Initialize(); // Set default values and constants
//WMM_Initialize(); // Set default values and constants
// TODO: Extract time/date from GPS to seed this
WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, home.Be);
//WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, home.Be);
HomeLocationSet(&home);
}