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

AHRS: Send PositionActual and VelocityActual out

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1919 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-10-08 15:38:19 +00:00 committed by peabody124
parent 9ec694cc40
commit 0a9ab6ca5f
3 changed files with 39 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
flight/AHRS/ahrs.c
View File

@ -157,6 +157,8 @@ void calibrate_sensors(void);
void reset_values();
void send_calibration(void);
void send_attitude(void);
void send_velocity(void);
void send_position(void);
void homelocation_callback(AhrsObjHandle obj);
void altitude_callback(AhrsObjHandle obj);
void calibration_callback(AhrsObjHandle obj);
@ -358,6 +360,8 @@ for all data to be up to date before doing anything*/
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
send_attitude(); // get message out quickly
send_velocity();
send_position();
INSCovariancePrediction(1 / (float)EKF_RATE);
if (gps_data.updated && ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) {
@ -746,6 +750,32 @@ void send_attitude(void)
AttitudeActualSet(&attitude);
}
void send_velocity(void)
{
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
// convert into cm
velocityActual.North = Nav.Vel[0] * 100;
velocityActual.East = Nav.Vel[1] * 100;
velocityActual.Down = Nav.Vel[2] * 100;
VelocityActualSet(&velocityActual);
}
void send_position(void)
{
PositionActualData positionActual;
PositionActualGet(&positionActual);
// convert into cm
positionActual.North = Nav.Pos[0] * 100;
positionActual.East = Nav.Pos[1] * 100;
positionActual.Down = Nav.Pos[2] * 100;
PositionActualSet(&positionActual);
}
void send_calibration(void)
{
AHRSCalibrationData cal;

9
flight/Libraries/ahrs_comm_objects.c
View File

@ -34,6 +34,7 @@ static AhrsStatusData AhrsStatus;
static BaroAltitudeData BaroAltitude;
static GPSPositionData GPSPosition;
static PositionActualData PositionActual;
static VelocityActualData VelocityActual;
static HomeLocationData HomeLocation;
static AHRSSettingsData AHRSSettings;
@ -56,10 +57,11 @@ CREATEHANDLE(3, AhrsStatus);
CREATEHANDLE(4, BaroAltitude);
CREATEHANDLE(5, GPSPosition);
CREATEHANDLE(6, PositionActual);
CREATEHANDLE(7, HomeLocation);
CREATEHANDLE(8, AHRSSettings);
CREATEHANDLE(7, VelocityActual);
CREATEHANDLE(8, HomeLocation);
CREATEHANDLE(9, AHRSSettings);
#if 9 != MAX_AHRS_OBJECTS //sanity check
#if 10 != MAX_AHRS_OBJECTS //sanity check
#error We did not create the correct number of xxxHandle() functions
#endif
@ -96,6 +98,7 @@ void AhrsInitHandles(void)
ADDHANDLE(idx++, BaroAltitude);
ADDHANDLE(idx++, GPSPosition);
ADDHANDLE(idx++, PositionActual);
ADDHANDLE(idx++, VelocityActual);
ADDHANDLE(idx++, HomeLocation);
ADDHANDLE(idx++, AHRSSettings);
if (idx != MAX_AHRS_OBJECTS) {

4
flight/Libraries/inc/ahrs_comm_objects.h
View File

@ -33,6 +33,7 @@
#include "baroaltitude.h"
#include "gpsposition.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "homelocation.h"
#include "ahrscalibration.h"
#include "ahrssettings.h"
@ -48,13 +49,14 @@ typedef union {
BaroAltitudeData BaroAltitude;
GPSPositionData GPSPosition;
PositionActualData PositionActual;
VelocityActualData VelocityActual;
HomeLocationData HomeLocation;
AHRSSettingsData AHRSSettings;
} __attribute__ ((packed)) AhrsSharedData;
/** The number of UAVObjects we will be dealing with.
*/
#define MAX_AHRS_OBJECTS 9
#define MAX_AHRS_OBJECTS 10
/** Our own version of a UAVObject.
*/