1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-19 09:54:15 +01:00
stac 24a4c6b791 gps: Update GPSPosition instead of PositionActual
GPS module now updates GPSPosition UAVObject
rather than the PositionActual object.  The
GPSPosition object is intended to be consumed
only by the AHRS.  The AHRS will use this (and
other inputs) to compute a filtered version of
the position in the PositionActual object.

This commit will cause temporary breakage of the
GPS functionality in the GCS until the PositionActual
object is properly updated by the AHRS.  Most of the
GCS should continue to use PositionActual.  The only
exception to this might be any tool for specifically
visualizing the raw GPS state.

GPS.c is now only responsible for receiving a
complete NMEA sentence from the COM interface.

NMEA parsing is now factored out into NMEA.[ch]
which is where GPSPosition is now updated based
on the complete NMEA sentences obtained from the
GPS.

Latitude and Longitude are now encoded in a
fixed-point notation in units of degrees x 10^-7
to prevent truncation of precision due to encoding
into a float.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1431 ebee16cc-31ac-478f-84a7-5cbb03baadba
2010-08-27 02:15:00 +00:00

501 lines
16 KiB
C

/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AHRSCommsModule AHRSComms Module
* @brief Handles communication with AHRS and updating position
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file ahrs_comms.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input object: AttitudeSettings
* Output object: AttitudeActual
*
* This module will periodically update the value of latest attitude solution
* that is available from the AHRS.
* The module settings can configure how often AHRS is polled for a new solution.
*
* The module executes in its own thread.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "ahrs_comms.h"
#include "attitudeactual.h"
#include "ahrssettings.h"
#include "attituderaw.h"
#include "ahrsstatus.h"
#include "alarms.h"
#include "baroaltitude.h"
#include "stdbool.h"
#include "gpsposition.h"
#include "homelocation.h"
#include "ahrscalibration.h"
#include "CoordinateConversions.h"
#include "pios_opahrs.h" // library for OpenPilot AHRS access functions
#include "pios_opahrs_proto.h"
// Private constants
#define STACK_SIZE 400
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
// Private types
// Private variables
static xTaskHandle taskHandle;
// Private functions
static void ahrscommsTask(void* parameters);
static void load_baro_altitude(struct opahrs_msg_v1_req_update * update);
static void load_gps_position(struct opahrs_msg_v1_req_update * update);
static void load_magnetic_north(struct opahrs_msg_v1_req_north * north);
static void load_calibration(struct opahrs_msg_v1_req_calibration * calibration);
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw);
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial);
static void update_calibration(struct opahrs_msg_v1_rsp_calibration * calibration);
static void process_update(struct opahrs_msg_v1_rsp_update * update); // main information parser
static bool AHRSSettingsIsUpdatedFlag = false;
static void AHRSSettingsUpdatedCb(UAVObjEvent * ev)
{
AHRSSettingsIsUpdatedFlag = true;
}
static bool BaroAltitudeIsUpdatedFlag = false;
static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
{
BaroAltitudeIsUpdatedFlag = true;
}
static bool GPSPositionIsUpdatedFlag = false;
static void GPSPositionUpdatedCb(UAVObjEvent * ev)
{
GPSPositionIsUpdatedFlag = true;
}
static bool HomeLocationIsUpdatedFlag = false;
static void HomeLocationUpdatedCb(UAVObjEvent * ev)
{
HomeLocationIsUpdatedFlag = true;
}
static bool AHRSCalibrationIsUpdatedFlag = false;
static bool AHRSCalibrationIsLocallyUpdateFlag = false;
static void AHRSCalibrationUpdatedCb(UAVObjEvent * ev)
{
if(AHRSCalibrationIsLocallyUpdateFlag == true)
AHRSCalibrationIsLocallyUpdateFlag = false;
else
AHRSCalibrationIsUpdatedFlag = true;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsInitialize(void)
{
AHRSSettingsConnectCallback(AHRSSettingsUpdatedCb);
BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);
GPSPositionConnectCallback(GPSPositionUpdatedCb);
HomeLocationConnectCallback(HomeLocationUpdatedCb);
AHRSCalibrationConnectCallback(AHRSCalibrationUpdatedCb);
PIOS_OPAHRS_Init();
// Start main task
xTaskCreate(ahrscommsTask, (signed char*)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
return 0;
}
static uint16_t update_errors = 0;
static uint16_t attituderaw_errors = 0;
static uint16_t home_errors = 0;
static uint16_t calibration_errors = 0;
static uint16_t algorithm_errors = 0;
/**
* Module thread, should not return.
*/
static void ahrscommsTask(void* parameters)
{
enum opahrs_result result;
// Main task loop
while (1) {
struct opahrs_msg_v1 rsp;
AhrsStatusData data;
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
/* Whenever resyncing, assume AHRS doesn't reset and doesn't know home */
AhrsStatusGet(&data);
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
AhrsStatusSet(&data);
/* Spin here until we're in sync */
while (PIOS_OPAHRS_resync() != OPAHRS_RESULT_OK) {
vTaskDelay(100 / portTICK_RATE_MS);
}
/* Give the other side a chance to keep up */
//vTaskDelay(100 / portTICK_RATE_MS);
if (PIOS_OPAHRS_GetSerial(&rsp) == OPAHRS_RESULT_OK) {
update_ahrs_status(&(rsp.payload.user.v.rsp.serial));
} else {
/* Comms error */
continue;
}
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
/* We're in sync with the AHRS, spin here until an error occurs */
while (1) {
AHRSSettingsData settings;
/* Update settings with latest value */
AHRSSettingsGet(&settings);
// Update home coordinate if it hasn't been updated
AhrsStatusGet(&data);
if (HomeLocationIsUpdatedFlag || (data.HomeSet == AHRSSTATUS_HOMESET_FALSE)) {
struct opahrs_msg_v1 req;
load_magnetic_north(&(req.payload.user.v.req.north));
if ((result = PIOS_OPAHRS_SetMagNorth(&req)) == OPAHRS_RESULT_OK) {
HomeLocationIsUpdatedFlag = false;
data.HomeSet = AHRSSTATUS_HOMESET_TRUE;
AhrsStatusSet(&data);
} else {
/* Comms error */
home_errors++;
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
AhrsStatusSet(&data);
break;
}
}
// Update calibration if necessary
AhrsStatusGet(&data);
if (AHRSCalibrationIsUpdatedFlag || (data.CalibrationSet == AHRSSTATUS_CALIBRATIONSET_FALSE))
{
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 rsp;
load_calibration(&(req.payload.user.v.req.calibration));
if(( result = PIOS_OPAHRS_SetGetCalibration(&req,&rsp) ) == OPAHRS_RESULT_OK ) {
update_calibration(&(rsp.payload.user.v.rsp.calibration));
AHRSCalibrationIsUpdatedFlag = false;
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_TRUE;
AhrsStatusSet(&data);
} else {
/* Comms error */
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
AhrsStatusSet(&data);
calibration_errors++;
break;
}
}
// Update algorithm
if (AHRSSettingsIsUpdatedFlag || (data.AlgorithmSet == AHRSSTATUS_ALGORITHMSET_FALSE))
{
struct opahrs_msg_v1 req;
if(settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS)
req.payload.user.v.req.algorithm.algorithm = INSGPS_Algo;
if(settings.Algorithm == AHRSSETTINGS_ALGORITHM_SIMPLE)
req.payload.user.v.req.algorithm.algorithm = SIMPLE_Algo;
if(( result = PIOS_OPAHRS_SetAlgorithm(&req) ) == OPAHRS_RESULT_OK ) {
data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_TRUE;
AhrsStatusSet(&data);
} else {
/* Comms error */
data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_FALSE;
AhrsStatusSet(&data);
algorithm_errors++;
break;
}
}
// If settings indicate, grab the raw and filtered data instead of estimate
if (settings.UpdateRaw)
{
if( (result = PIOS_OPAHRS_GetAttitudeRaw(&rsp)) == OPAHRS_RESULT_OK) {
update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
} else {
/* Comms error */
attituderaw_errors++;
break;
}
}
if (settings.UpdateFiltered)
{
// Otherwise do standard technique
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 rsp;
// Load barometer if updated
if (BaroAltitudeIsUpdatedFlag)
load_baro_altitude(&(req.payload.user.v.req.update));
else
req.payload.user.v.req.update.barometer.updated = 0;
// Load GPS if updated
if (GPSPositionIsUpdatedFlag)
load_gps_position(&(req.payload.user.v.req.update));
else
req.payload.user.v.req.update.gps.updated = 0;
// Transfer packet and process returned attitude
if ((result = PIOS_OPAHRS_SetGetUpdate(&req,&rsp)) == OPAHRS_RESULT_OK) {
if (req.payload.user.v.req.update.barometer.updated)
BaroAltitudeIsUpdatedFlag = false;
if (req.payload.user.v.req.update.gps.updated)
GPSPositionIsUpdatedFlag = false;
process_update(&(rsp.payload.user.v.rsp.update));
} else {
/* Comms error */
update_errors++;
break;
}
}
/* Wait for the next update interval */
vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
}
}
}
static void load_calibration(struct opahrs_msg_v1_req_calibration * calibration)
{
AHRSCalibrationData data;
AHRSCalibrationGet(&data);
calibration->measure_var = data.measure_var;
calibration->accel_bias[0] = data.accel_bias[0];
calibration->accel_bias[1] = data.accel_bias[1];
calibration->accel_bias[2] = data.accel_bias[2];
calibration->accel_scale[0] = data.accel_scale[0];
calibration->accel_scale[1] = data.accel_scale[1];
calibration->accel_scale[2] = data.accel_scale[2];
calibration->accel_var[0] = data.accel_var[0];
calibration->accel_var[1] = data.accel_var[1];
calibration->accel_var[2] = data.accel_var[2];
calibration->gyro_bias[0] = data.gyro_bias[0];
calibration->gyro_bias[1] = data.gyro_bias[1];
calibration->gyro_bias[2] = data.gyro_bias[2];
calibration->gyro_scale[0] = data.gyro_scale[0];
calibration->gyro_scale[1] = data.gyro_scale[1];
calibration->gyro_scale[2] = data.gyro_scale[2];
calibration->gyro_var[0] = data.gyro_var[0];
calibration->gyro_var[1] = data.gyro_var[1];
calibration->gyro_var[2] = data.gyro_var[2];
calibration->mag_bias[0] = data.mag_bias[0];
calibration->mag_bias[1] = data.mag_bias[1];
calibration->mag_bias[2] = data.mag_bias[2];
calibration->mag_var[0] = data.mag_var[0];
calibration->mag_var[1] = data.mag_var[1];
calibration->mag_var[2] = data.mag_var[2];
}
static void update_calibration(struct opahrs_msg_v1_rsp_calibration * calibration)
{
AHRSCalibrationData data;
AHRSCalibrationGet(&data);
AHRSCalibrationIsLocallyUpdateFlag = true;
data.accel_var[0] = calibration->accel_var[0];
data.accel_var[1] = calibration->accel_var[1];
data.accel_var[2] = calibration->accel_var[2];
data.gyro_var[0] = calibration->gyro_var[0];
data.gyro_var[1] = calibration->gyro_var[1];
data.gyro_var[2] = calibration->gyro_var[2];
data.mag_var[0] = calibration->mag_var[0];
data.mag_var[1] = calibration->mag_var[1];
data.mag_var[2] = calibration->mag_var[2];
AHRSCalibrationSet(&data);
}
static void load_magnetic_north(struct opahrs_msg_v1_req_north * mag_north)
{
HomeLocationData data;
HomeLocationGet(&data);
mag_north->Be[0] = data.Be[0];
mag_north->Be[1] = data.Be[1];
mag_north->Be[2] = data.Be[2];
if(data.Be[0] == 0 && data.Be[1] == 0 && data.Be[2] == 0) {
// in case nothing has been set go to default to prevent NaN in attitude solution
mag_north->Be[0] = 1;
mag_north->Be[1] = 0;
mag_north->Be[2] = 0;
} else {
// normalize for unit length here
float len = sqrt(data.Be[0] * data.Be[0] + data.Be[1] * data.Be[1] + data.Be[2] * data.Be[2]);
mag_north->Be[0] = data.Be[0] / len;
mag_north->Be[1] = data.Be[1] / len;
mag_north->Be[2] = data.Be[2] / len;
}
}
static void load_baro_altitude(struct opahrs_msg_v1_req_update * update)
{
BaroAltitudeData data;
BaroAltitudeGet(&data);
update->barometer.altitude = data.Altitude;
update->barometer.updated = TRUE;
}
static void load_gps_position(struct opahrs_msg_v1_req_update * update)
{
GPSPositionData data;
GPSPositionGet(&data);
HomeLocationData home;
HomeLocationGet(&home);
update->gps.updated = TRUE;
if(home.RNE[0] == 0 && home.RNE[1] && home.RNE[2] == 0 && home.RNE[3] == 0) {
update->gps.NED[0] = 0;
update->gps.NED[1] = 0;
update->gps.NED[2] = 0;
update->gps.groundspeed = 0;
update->gps.heading = 0;
update->gps.quality = 0;
} else {
update->gps.groundspeed = data.Groundspeed;
update->gps.heading = data.Heading;
update->gps.quality = 0;
double LLA[3] = {(double) data.Latitude / 1e7, (double) data.Longitude / 1e7, (double) data.Altitude};
// convert from cm back to meters
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, update->gps.NED);
}
}
static void process_update(struct opahrs_msg_v1_rsp_update * update)
{
AttitudeActualData data;
data.q1 = update->quaternion.q1;
data.q2 = update->quaternion.q2;
data.q3 = update->quaternion.q3;
data.q4 = update->quaternion.q4;
float q[4] = {data.q1, data.q2, data.q3, data.q4};
float rpy[3];
Quaternion2RPY(q,rpy);
data.Roll = rpy[0];
data.Pitch = rpy[1];
data.Yaw = rpy[2];
if(data.Yaw < 0) data.Yaw += 360;
AttitudeActualSet(&data);
/*
* update->NED[]
* update->Vel[]
*/
}
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw)
{
AttitudeRawData data;
data.magnetometers[ATTITUDERAW_MAGNETOMETERS_X] = attituderaw->mags.x;
data.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y] = attituderaw->mags.y;
data.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z] = attituderaw->mags.z;
data.gyros[ATTITUDERAW_GYROS_X] = attituderaw->gyros.x;
data.gyros[ATTITUDERAW_GYROS_Y] = attituderaw->gyros.y;
data.gyros[ATTITUDERAW_GYROS_Z] = attituderaw->gyros.z;
data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = attituderaw->gyros_filtered.x;
data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = attituderaw->gyros_filtered.y;
data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = attituderaw->gyros_filtered.z;
data.gyrotemp[ATTITUDERAW_GYROTEMP_XY] = attituderaw->gyros.xy_temp;
data.gyrotemp[ATTITUDERAW_GYROTEMP_Z] = attituderaw->gyros.z_temp;
data.accels[ATTITUDERAW_ACCELS_X] = attituderaw->accels.x;
data.accels[ATTITUDERAW_ACCELS_Y] = attituderaw->accels.y;
data.accels[ATTITUDERAW_ACCELS_Z] = attituderaw->accels.z;
data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] = attituderaw->accels_filtered.x;
data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] = attituderaw->accels_filtered.y;
data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] = attituderaw->accels_filtered.z;
AttitudeRawSet(&data);
}
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial)
{
AhrsStatusData data;
// Get the current object data
AhrsStatusGet(&data);
for (uint8_t i = 0; i < sizeof(serial->serial_bcd); i++) {
data.SerialNumber[i] = serial->serial_bcd[i];
}
data.CommErrors[AHRSSTATUS_COMMERRORS_UPDATE] = update_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_ATTITUDERAW] = attituderaw_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_HOMELOCATION] = home_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_CALIBRATION] = calibration_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_ALGORITHM] = algorithm_errors;
AhrsStatusSet(&data);
}
/**
* @}
* @}
*/