rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

AHRS: Added message to pass GPS information into AHRS from OP. This is then converted

Browse Source 
into NED reference frame and used in the INSGPS algorithm, although currently this
information isn't propagated back to OP.  Data structures related to the GPS position
into the algorithm and the position estimate out will likely be in flux.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1334 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-19 05:18:34 +00:00 committed by peabody124
parent 18110f9ac8
commit 3c4def13cc
6 changed files with 167 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
flight/AHRS/ahrs.c
View File

@ -149,23 +149,22 @@ struct altitude_sensor {
float temperature;
};
struct gps_sensor {
float latitude;
float longitude;
float altitude;
float heading;
float groundspeed;
float status;
};
static struct mag_sensor mag_data;
static struct accel_sensor accel_data;
static struct gyro_sensor gyro_data;
static struct altitude_sensor altitude_data;
static struct attitude_solution attitude_data = {
.quaternion = {
.q1 = 1.011,
.q2 = 2.022,
.q3 = 3.033,
.q4 = 0,
},
.euler = {
.roll = 4.044,
.pitch = 5.055,
.yaw = 6.066,
},
};
static struct gps_sensor gps_data;
static struct attitude_solution attitude_data;
/**
* @}
*/
@ -175,6 +174,7 @@ static struct attitude_solution attitude_data = {
void process_spi_request(void);
void downsample_data(void);
void calibrate_sensors(void);
void initialize_location();
/**
* @addtogroup AHRS_Global_Data AHRS Global Data
@ -203,6 +203,16 @@ int16_t * valid_data_buffer;
uint32_t ekf_too_slow = 0;
//! Total number of data blocks converted
uint32_t total_conversion_blocks = 0;
//! Flag to indicate new GPS data available
uint8_t gps_updated = FALSE;
//! Home location in ECEF coordinates
double BaseECEF[3] = {0, 0, 0};
//! Rotation matrix from LLA to Rne
float Rne[3][3];
//! Current location in NED coordinates relative to base
float NED[3];
//! Current location in NED coordinates relative to base
/**
* @}
*/
@ -215,7 +225,7 @@ int main()
{
float gyro[3], accel[3], mag[3];
float pos[3] = {0,0,0}, vel[3] = {0,0,0}, BaroAlt = 0;
uint32_t update_ctr = 0;
uint32_t loop_ctr = 0;
ahrs_algorithm = INSGPS_Algo;
/* Brings up System using CMSIS functions, enables the LEDs. */
@ -281,7 +291,7 @@ int main()
float scaled_mag_var[3] = {mag_var[0] / mag_length, mag_var[1] / mag_length, mag_var[2] / mag_length};
INSSetMagVar(scaled_mag_var);
/* // Initialize the algorithm assuming stationary
// Initialize the algorithm assuming stationary
float temp_var[3] = {10, 10, 10};
float temp_gyro[3] = {0, 0, 0};
@ -297,7 +307,7 @@ int main()
RPY2Quaternion(rpy,Nav.q);
INSPrediction( temp_gyro, accel_bias, 1 / (float) EKF_RATE );
FullCorrection(mag,pos,vel,BaroAlt);
} */
}
INSSetGyroBias(gyro_bias);
INSSetAccelVar(accel_var);
@ -321,6 +331,8 @@ int main()
// Alive signal
PIOS_LED_Toggle(LED1);
loop_ctr ++;
// Get magnetic readings
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
@ -364,11 +376,35 @@ int main()
mag[2] = -mag_data.raw.axis[2];
INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
update_ctr++;
if ( 0 )
MagCorrection(mag);
else if ( 1 );
FullCorrection(mag,pos,vel,BaroAlt);
if ( ((loop_ctr % 10000) == 0) && (gps_data.status > 0) )
{
// cheap logic instead of detecting the first update of gps data
WMM_GetMagVector(gps_data.latitude, gps_data.longitude, gps_data.altitude, 8, 17, 2010, MagNorth);
float MagNorthLen = sqrt(MagNorth[0] * MagNorth[0] + MagNorth[1] * MagNorth[1] + MagNorth[2] * MagNorth[2]);
float MagNorthScaled[3] = {MagNorth[0] / MagNorthLen, MagNorth[1] / MagNorthLen, MagNorth[2] / MagNorthLen};
INSSetMagNorth(MagNorthScaled);
}
if ( gps_updated )
{
if(BaseECEF[0] == 0 && BaseECEF[1] == 0 && BaseECEF[2] == 0)
initialize_location();
// Convert to NED frame. Probably this will be moved to OP mainboard
double LLA[3] = {gps_data.latitude, gps_data.longitude, gps_data.altitude};
LLA2Base(LLA, BaseECEF, Rne, NED);
// Compute velocity from Heading and groundspeed
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
vel[2] = 0;
FullCorrection(mag, NED, vel, altitude_data.altitude);
gps_updated = FALSE;
}
else
MagCorrection(mag);
Quaternion2RPY(Nav.q,rpy);
attitude_data.quaternion.q1 = Nav.q[0];
@ -407,6 +443,18 @@ int main()
return 0;
}
/**
* @brief Initialize a location as the home point and compute rotation matrix to
* get to NED coordinates
*/
void initialize_location()
{
double LLA[3] = {gps_data.latitude, gps_data.longitude, gps_data.altitude};
RneFromLLA(LLA, Rne);
LLA2ECEF(LLA ,BaseECEF);
}
/**
* @brief Downsample the analog data
* @return none
@ -629,6 +677,18 @@ void process_spi_request(void)
dump_spi_message(PIOS_COM_AUX, "V", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_GPS:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_GPS);
gps_updated = TRUE;
gps_data.latitude = user_rx_v1.payload.user.v.req.gps.latitude;
gps_data.longitude = user_rx_v1.payload.user.v.req.gps.longitude;
gps_data.altitude = user_rx_v1.payload.user.v.req.gps.altitude;
gps_data.heading = user_rx_v1.payload.user.v.req.gps.heading;
gps_data.groundspeed = user_rx_v1.payload.user.v.req.gps.groundspeed;
gps_data.status = user_rx_v1.payload.user.v.req.gps.status;
dump_spi_message(PIOS_COM_AUX, "V", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_ATTITUDERAW:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ATTITUDERAW);
user_tx_v1.payload.user.v.rsp.attituderaw.mags.x = mag_data.raw.axis[0];

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

@ -58,6 +58,7 @@
#include "alarms.h"
#include "altitudeactual.h"
#include "stdbool.h"
#include "positionactual.h"
#include "pios_opahrs.h" // library for OpenPilot AHRS access functions
#include "pios_opahrs_proto.h"
@ -74,6 +75,7 @@ static xTaskHandle taskHandle;
// Private functions
static void ahrscommsTask(void* parameters);
static void load_altitude_actual(struct opahrs_msg_v1_req_altitude * altitude);
static void load_position_actual(struct opahrs_msg_v1_req_gps * gps);
static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude);
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw);
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial);
@ -84,6 +86,12 @@ static void AltitudeActualUpdatedCb(UAVObjEvent * ev)
AltitudeActualIsUpdatedFlag = true;
}
static bool PositionActualIsUpdatedFlag = false;
static void PositionActualUpdatedCb(UAVObjEvent * ev)
{
PositionActualIsUpdatedFlag = true;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -91,6 +99,7 @@ static void AltitudeActualUpdatedCb(UAVObjEvent * ev)
int32_t AHRSCommsInitialize(void)
{
AltitudeActualConnectCallback(AltitudeActualUpdatedCb);
PositionActualConnectCallback(PositionActualUpdatedCb);
PIOS_OPAHRS_Init();
@ -136,30 +145,41 @@ static void ahrscommsTask(void* parameters)
AttitudeSettingsGet(&settings);
if (PIOS_OPAHRS_GetAttitude(&rsp) == OPAHRS_RESULT_OK) {
update_attitude_actual(&(rsp.payload.user.v.rsp.attitude));
update_attitude_actual(&(rsp.payload.user.v.rsp.attitude));
} else {
/* Comms error */
break;
/* Comms error */
break;
}
if (PIOS_OPAHRS_GetAttitudeRaw(&rsp) == OPAHRS_RESULT_OK) {
update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
} else {
/* Comms error */
break;
/* Comms error */
break;
}
if (AltitudeActualIsUpdatedFlag) {
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 req;
load_altitude_actual(&(req.payload.user.v.req.altitude));
if (PIOS_OPAHRS_SetAltitudeActual(&req) == OPAHRS_RESULT_OK) {
AltitudeActualIsUpdatedFlag = false;
} else {
/* Comms error */
}
load_altitude_actual(&(req.payload.user.v.req.altitude));
if (PIOS_OPAHRS_SetAltitudeActual(&req) == OPAHRS_RESULT_OK) {
AltitudeActualIsUpdatedFlag = false;
} else {
/* Comms error */
}
}
if (PositionActualIsUpdatedFlag) {
struct opahrs_msg_v1 req;
load_position_actual(&(req.payload.user.v.req.gps));
if (PIOS_OPAHRS_SetPositionActual(&req) == OPAHRS_RESULT_OK) {
PositionActualIsUpdatedFlag = false;
} else {
/* Comms error */
}
}
/* Wait for the next update interval */
vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
}
@ -177,6 +197,19 @@ static void load_altitude_actual(struct opahrs_msg_v1_req_altitude * altitude)
altitude->temperature = data.Temperature;
}
static void load_position_actual(struct opahrs_msg_v1_req_gps * gps)
{
PositionActualData data;
PositionActualGet(&data);
gps->latitude = data.Latitude;
gps->longitude = data.Longitude;
gps->altitude = data.GeoidSeparation;
gps->heading = data.Heading;
gps->groundspeed = data.Groundspeed;
gps->status = data.Status;
}
static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude)
{
AttitudeActualData data;

22
flight/PiOS/Common/pios_opahrs.c
View File

@ -266,6 +266,28 @@ enum opahrs_result PIOS_OPAHRS_GetAttitude(struct opahrs_msg_v1 *rsp)
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_ATTITUDE, rsp);
}
enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req)
{
struct opahrs_msg_v1 rsp;
enum opahrs_result rc;
if (!req) {
return -1;
}
/* Make up an attituderaw request */
opahrs_msg_v1_init_user_tx (req, OPAHRS_MSG_V1_REQ_GPS);
/* Send the message until it is received */
rc = opahrs_msg_v1_send_req (req);
if (rc != OPAHRS_RESULT_OK) {
/* Failed to send the request, bail out */
return rc;
}
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_GPS, &rsp);
}
enum opahrs_result PIOS_OPAHRS_SetAltitudeActual(struct opahrs_msg_v1 *req)
{
struct opahrs_msg_v1 rsp;

1
flight/PiOS/inc/pios_opahrs.h
View File

@ -44,6 +44,7 @@ extern enum opahrs_result PIOS_OPAHRS_GetSerial(struct opahrs_msg_v1 *rsp);
extern enum opahrs_result PIOS_OPAHRS_GetAttitude(struct opahrs_msg_v1 *rsp);
extern enum opahrs_result PIOS_OPAHRS_GetAttitudeRaw(struct opahrs_msg_v1 *rsp);
extern enum opahrs_result PIOS_OPAHRS_SetAltitudeActual(struct opahrs_msg_v1 *req);
extern enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req);
extern enum opahrs_result PIOS_OPAHRS_resync(void);
#endif /* PIOS_OPAHRS_H */

16
flight/PiOS/inc/pios_opahrs_proto.h
View File

@ -190,6 +190,15 @@ struct opahrs_msg_v1_req_altitude {
float temperature;
} __attribute__((__packed__));
struct opahrs_msg_v1_req_gps {
float latitude;
float longitude;
float altitude;
float heading;
float groundspeed;
uint8_t status;
} __attribute__((__packed__));
struct opahrs_msg_v1_req_attituderaw {
} __attribute__((__packed__));
@ -203,6 +212,7 @@ union opahrs_msg_v1_req {
struct opahrs_msg_v1_req_reset reset;
struct opahrs_msg_v1_req_serial serial;
struct opahrs_msg_v1_req_altitude altitude;
struct opahrs_msg_v1_req_gps gps;
struct opahrs_msg_v1_req_attituderaw attituderaw;
struct opahrs_msg_v1_req_attitude attitude;
} __attribute__((__packed__));
@ -223,6 +233,9 @@ struct opahrs_msg_v1_rsp_serial {
struct opahrs_msg_v1_rsp_altitude {
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_gps {
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_attituderaw {
struct {
int16_t x;
@ -273,6 +286,7 @@ union opahrs_msg_v1_rsp {
struct opahrs_msg_v1_rsp_altitude altitude;
struct opahrs_msg_v1_rsp_attituderaw attituderaw;
struct opahrs_msg_v1_rsp_attitude attitude;
struct opahrs_msg_v1_rsp_altitude gps;
} __attribute__((__packed__));
enum opahrs_msg_v1_tag {
@ -281,12 +295,14 @@ enum opahrs_msg_v1_tag {
OPAHRS_MSG_V1_REQ_RESET,
OPAHRS_MSG_V1_REQ_SERIAL,
OPAHRS_MSG_V1_REQ_ALTITUDE,
OPAHRS_MSG_V1_REQ_GPS,
OPAHRS_MSG_V1_REQ_ATTITUDERAW,
OPAHRS_MSG_V1_REQ_ATTITUDE,
OPAHRS_MSG_V1_RSP_SYNC,
OPAHRS_MSG_V1_RSP_SERIAL,
OPAHRS_MSG_V1_RSP_ALTITUDE,
OPAHRS_MSG_V1_RSP_GPS,
OPAHRS_MSG_V1_RSP_ATTITUDERAW,
OPAHRS_MSG_V1_RSP_ATTITUDE,
};

2
flight/Project/OpenOCD/floss-jtag.ahrs.osx.cfg
View File

@ -10,6 +10,6 @@ ft2232_vid_pid 0x0403 0x6010
ft2232_layout "usbjtag"
ft2232_latency 2
gdb_port 3333
gdb_port 3334
tcl_port 6666
telnet_port 4444