rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

AHRSComms: altitude sensor data is now sent to AHRS

Browse Source 
Altitude/pressure sensor data is sent to the AHRS whenever
the AltitudeActual object is updated.

Altitude, Pressure and Temperature are sent as floats.
Same as in the UAVObject that goes to the GCS.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1291 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
stac 2010-08-15 01:02:13 +00:00 committed by stac
parent d4c5dba234
commit 3125ecfc4e
5 changed files with 87 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
flight/AHRS/ahrs.c
View File

@ -74,10 +74,18 @@ struct attitude_solution {
} euler;
};
struct altitude_sensor {
float altitude;
float pressure;
float temperature;
};
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,
@ -261,6 +269,14 @@ void process_spi_request(void)
dump_spi_message(PIOS_COM_AUX, "I", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_ALTITUDE:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ALTITUDE);
altitude_data.altitude = user_rx_v1.payload.user.v.req.altitude.altitude;
altitude_data.pressure = user_rx_v1.payload.user.v.req.altitude.pressure;
altitude_data.temperature = user_rx_v1.payload.user.v.req.altitude.temperature;
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];

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

@ -56,6 +56,8 @@
#include "attituderaw.h"
#include "ahrsstatus.h"
#include "alarms.h"
#include "altitudeactual.h"
#include "stdbool.h"
#include "pios_opahrs.h" // library for OpenPilot AHRS access functions
#include "pios_opahrs_proto.h"
@ -71,16 +73,25 @@ static xTaskHandle taskHandle;
// Private functions
static void ahrscommsTask(void* parameters);
static void load_altitude_actual(struct opahrs_msg_v1_req_altitude * altitude);
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);
static bool AltitudeActualIsUpdatedFlag = false;
static void AltitudeActualUpdatedCb(UAVObjEvent * ev)
{
AltitudeActualIsUpdatedFlag = true;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsInitialize(void)
{
AltitudeActualConnectCallback(AltitudeActualUpdatedCb);
PIOS_OPAHRS_Init();
// Start main task
@ -137,13 +148,35 @@ static void ahrscommsTask(void* parameters)
/* Comms error */
break;
}
if (AltitudeActualIsUpdatedFlag) {
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 */
}
}
/* Wait for the next update interval */
vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
}
}
}
static void load_altitude_actual(struct opahrs_msg_v1_req_altitude * altitude)
{
AltitudeActualData data;
AltitudeActualGet(&data);
altitude->altitude = data.Altitude;
altitude->pressure = data.Pressure;
altitude->temperature = data.Temperature;
}
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_SetAltitudeActual(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_ALTITUDE);
/* 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_ALTITUDE, &rsp);
}
#endif /* PIOS_INCLUDE_OPAHRS */
/**

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

@ -43,6 +43,7 @@ extern enum opahrs_result PIOS_OPAHRS_Sync(struct opahrs_msg_v1 *rsp);
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_resync(void);
#endif /* PIOS_OPAHRS_H */

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

@ -184,17 +184,25 @@ struct opahrs_msg_v1_req_reset {
struct opahrs_msg_v1_req_serial {
} __attribute__((__packed__));
struct opahrs_msg_v1_req_altitude {
float altitude;
float pressure;
float temperature;
} __attribute__((__packed__));
struct opahrs_msg_v1_req_attituderaw {
} __attribute__((__packed__));
struct opahrs_msg_v1_req_attitude {
} __attribute__((__packed__));
union opahrs_msg_v1_req {
struct opahrs_msg_v1_req_nop nop;
struct opahrs_msg_v1_req_sync sync;
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_attituderaw attituderaw;
struct opahrs_msg_v1_req_attitude attitude;
} __attribute__((__packed__));
@ -212,6 +220,9 @@ struct opahrs_msg_v1_rsp_serial {
uint8_t serial_bcd[25];
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_altitude {
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_attituderaw {
struct {
int16_t x;
@ -249,6 +260,7 @@ struct opahrs_msg_v1_rsp_attitude {
union opahrs_msg_v1_rsp {
struct opahrs_msg_v1_rsp_sync sync;
struct opahrs_msg_v1_rsp_serial serial;
struct opahrs_msg_v1_rsp_altitude altitude;
struct opahrs_msg_v1_rsp_attituderaw attituderaw;
struct opahrs_msg_v1_rsp_attitude attitude;
} __attribute__((__packed__));
@ -258,11 +270,13 @@ enum opahrs_msg_v1_tag {
OPAHRS_MSG_V1_REQ_SYNC,
OPAHRS_MSG_V1_REQ_RESET,
OPAHRS_MSG_V1_REQ_SERIAL,
OPAHRS_MSG_V1_REQ_ALTITUDE,
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_ATTITUDERAW,
OPAHRS_MSG_V1_RSP_ATTITUDE,
};