mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-29 14:52:12 +01:00
ahrscomms: connect the OP board to the AHRS board
The AHRS comms module now sync's with the AHRS and exchanges interesting data periodically. Whenever the link to the AHRS is down, the AHRSComms alarm is raised. This is fairly basic for now but provides the last piece of the infrastructure to move data back/forth between the OP and the AHRS. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1014 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
stac
stac
parent
05485e5b63
commit
17bea4cb91
@ -45,13 +45,17 @@
|
||||
*/
|
||||
|
||||
#include "ahrs_comms.h"
|
||||
#include "attitudeactual.h" // object that will be updated by the module
|
||||
#include "attitudesettings.h" // object holding module settings
|
||||
#include "attitudeactual.h"
|
||||
#include "attitudesettings.h"
|
||||
#include "headingactual.h"
|
||||
#include "ahrsstatus.h"
|
||||
#include "alarms.h"
|
||||
|
||||
#include "pios_opahrs.h" // library for OpenPilot AHRS access functions
|
||||
#include "pios_opahrs_proto.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE 200
|
||||
#define STACK_SIZE 400
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
|
||||
// Private types
|
||||
@ -61,6 +65,9 @@ static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void ahrscommsTask(void* parameters);
|
||||
static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude);
|
||||
static void update_heading_actual(struct opahrs_msg_v1_rsp_heading * heading);
|
||||
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
@ -68,10 +75,12 @@ static void ahrscommsTask(void* parameters);
|
||||
*/
|
||||
int32_t AHRSCommsInitialize(void)
|
||||
{
|
||||
// Start main task
|
||||
xTaskCreate(ahrscommsTask, (signed char*)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
PIOS_OPAHRS_Init();
|
||||
|
||||
return 0;
|
||||
// Start main task
|
||||
xTaskCreate(ahrscommsTask, (signed char*)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -79,42 +88,95 @@ int32_t AHRSCommsInitialize(void)
|
||||
*/
|
||||
static void ahrscommsTask(void* parameters)
|
||||
{
|
||||
AttitudeSettingsData settings;
|
||||
AttitudeActualData data;
|
||||
// Main task loop
|
||||
while (1) {
|
||||
struct opahrs_msg_v1 rsp;
|
||||
|
||||
// Main task loop
|
||||
while (1)
|
||||
{
|
||||
// Update settings with latest value
|
||||
AttitudeSettingsGet(&settings);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
|
||||
// Get the current object data
|
||||
AttitudeActualGet(&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);
|
||||
|
||||
// Query the latest attitude solution from the AHRS
|
||||
PIOS_OPAHRS_ReadAttitude();
|
||||
if (PIOS_OPAHRS_GetSerial(&rsp) == OPAHRS_RESULT_OK) {
|
||||
update_ahrs_status(&(rsp.payload.user.v.rsp.serial));
|
||||
} else {
|
||||
/* Comms error */
|
||||
continue;
|
||||
}
|
||||
|
||||
// Update the data
|
||||
data.q1 += 0.111;
|
||||
data.q2 += 1.1;
|
||||
data.q3 += 7.0;
|
||||
data.q4 -= 2.321;
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
|
||||
|
||||
data.Roll += 0.01;
|
||||
data.Pitch -= 0.03;
|
||||
data.Yaw += 0.05;
|
||||
/* We're in sync with the AHRS, spin here until an error occurs */
|
||||
while (1) {
|
||||
AttitudeSettingsData settings;
|
||||
|
||||
// Update the ExampleObject, after this function is called
|
||||
// notifications to any other modules listening to that object
|
||||
// will be sent and the GCS object will be updated through the
|
||||
// telemetry link. All operations will take place asynchronously
|
||||
// and the following call will return immediately.
|
||||
AttitudeActualSet(&data);
|
||||
/* Update settings with latest value */
|
||||
AttitudeSettingsGet(&settings);
|
||||
|
||||
if (PIOS_OPAHRS_GetAttitude(&rsp) == OPAHRS_RESULT_OK) {
|
||||
update_attitude_actual(&(rsp.payload.user.v.rsp.attitude));
|
||||
} else {
|
||||
/* Comms error */
|
||||
break;
|
||||
}
|
||||
|
||||
// Since this module executes at fixed time intervals, we need to
|
||||
// block the task until it is time for the next update.
|
||||
// The settings field is in ms, to convert to RTOS ticks we need
|
||||
// to divide by portTICK_RATE_MS.
|
||||
vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
|
||||
}
|
||||
if (PIOS_OPAHRS_GetHeading(&rsp) == OPAHRS_RESULT_OK) {
|
||||
update_heading_actual(&(rsp.payload.user.v.rsp.heading));
|
||||
} else {
|
||||
/* Comms error */
|
||||
break;
|
||||
}
|
||||
|
||||
/* Wait for the next update interval */
|
||||
vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude)
|
||||
{
|
||||
AttitudeActualData data;
|
||||
|
||||
data.q1 = attitude->quaternion.q1;
|
||||
data.q2 = attitude->quaternion.q2;
|
||||
data.q3 = attitude->quaternion.q3;
|
||||
data.q4 = attitude->quaternion.q4;
|
||||
|
||||
data.Roll = attitude->euler.roll;
|
||||
data.Pitch = attitude->euler.pitch;
|
||||
data.Yaw = attitude->euler.yaw;
|
||||
|
||||
AttitudeActualSet(&data);
|
||||
}
|
||||
|
||||
static void update_heading_actual(struct opahrs_msg_v1_rsp_heading * heading)
|
||||
{
|
||||
HeadingActualData data;
|
||||
|
||||
data.raw[HEADINGACTUAL_RAW_X] = heading->raw_mag.x;
|
||||
data.raw[HEADINGACTUAL_RAW_Y] = heading->raw_mag.y;
|
||||
data.raw[HEADINGACTUAL_RAW_Z] = heading->raw_mag.z;
|
||||
|
||||
data.heading = heading->heading;
|
||||
|
||||
HeadingActualSet(&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];
|
||||
}
|
||||
|
||||
AhrsStatusSet(&data);
|
||||
}
|
||||
|
||||
@ -30,25 +30,238 @@
|
||||
|
||||
#if defined(PIOS_INCLUDE_OPAHRS)
|
||||
|
||||
#include "pios_opahrs_proto.h"
|
||||
#include "pios_opahrs.h"
|
||||
|
||||
/**
|
||||
* Initialise the OpenPilot AHRS
|
||||
*/
|
||||
void PIOS_OPAHRS_Init(void)
|
||||
{
|
||||
PIOS_SPI_SetClockSpeed(PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_4);
|
||||
}
|
||||
|
||||
void PIOS_OPAHRS_ReadAttitude(void)
|
||||
static int32_t opahrs_msg_txrx (const uint8_t * tx, uint8_t * rx, uint32_t len)
|
||||
{
|
||||
int32_t rc;
|
||||
|
||||
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
|
||||
vTaskDelay(20 / portTICK_RATE_MS);
|
||||
rc = PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, tx, rx, len, NULL);
|
||||
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1);
|
||||
return (rc);
|
||||
}
|
||||
|
||||
int32_t PIOS_OPAHRS_Read(uint8_t address, uint8_t *buffer, uint8_t len)
|
||||
static enum opahrs_result opahrs_msg_v1_send_req (const struct opahrs_msg_v1 * req)
|
||||
{
|
||||
return 0;
|
||||
struct opahrs_msg_v1 link_rx;
|
||||
|
||||
for (uint8_t retries = 0; retries < 20; retries++) {
|
||||
struct opahrs_msg_v1 * rsp = &link_rx;
|
||||
|
||||
if (opahrs_msg_txrx((const uint8_t *)req, (uint8_t *)rsp, sizeof(*rsp)) < 0) {
|
||||
return OPAHRS_RESULT_FAILED;
|
||||
}
|
||||
|
||||
/* Make sure we got a sane response by checking the magic */
|
||||
if ((rsp->head.magic != OPAHRS_MSG_MAGIC_HEAD) ||
|
||||
(rsp->tail.magic != OPAHRS_MSG_MAGIC_TAIL)) {
|
||||
return OPAHRS_RESULT_FAILED;
|
||||
}
|
||||
|
||||
switch (rsp->head.type) {
|
||||
case OPAHRS_MSG_TYPE_LINK:
|
||||
switch (rsp->payload.link.state) {
|
||||
case OPAHRS_MSG_LINK_STATE_BUSY:
|
||||
case OPAHRS_MSG_LINK_STATE_INACTIVE:
|
||||
/* Wait for a small delay and retry */
|
||||
vTaskDelay(20 / portTICK_RATE_MS);
|
||||
continue;
|
||||
case OPAHRS_MSG_LINK_STATE_READY:
|
||||
/* Peer was ready when we Tx'd so they have now Rx'd our message */
|
||||
return OPAHRS_RESULT_OK;
|
||||
}
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_USER_V0:
|
||||
case OPAHRS_MSG_TYPE_USER_V1:
|
||||
/* Wait for a small delay and retry */
|
||||
vTaskDelay(20 / portTICK_RATE_MS);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
return OPAHRS_RESULT_TIMEOUT;
|
||||
}
|
||||
|
||||
int32_t PIOS_OPAHRS_Write(uint8_t address, uint8_t buffer)
|
||||
static enum opahrs_result opahrs_msg_v1_recv_rsp (enum opahrs_msg_v1_tag tag, struct opahrs_msg_v1 * rsp)
|
||||
{
|
||||
return 0;
|
||||
struct opahrs_msg_v1 link_tx;
|
||||
|
||||
opahrs_msg_v1_init_link_tx(&link_tx, OPAHRS_MSG_LINK_TAG_NOP);
|
||||
|
||||
for (uint8_t retries = 0; retries < 20; retries++) {
|
||||
if (opahrs_msg_txrx((const uint8_t *)&link_tx, (uint8_t *)rsp, sizeof(*rsp)) < 0) {
|
||||
return OPAHRS_RESULT_FAILED;
|
||||
}
|
||||
|
||||
/* Make sure we got a sane response by checking the magic */
|
||||
if ((rsp->head.magic != OPAHRS_MSG_MAGIC_HEAD) ||
|
||||
(rsp->tail.magic != OPAHRS_MSG_MAGIC_TAIL)) {
|
||||
return OPAHRS_RESULT_FAILED;
|
||||
}
|
||||
|
||||
switch (rsp->head.type) {
|
||||
case OPAHRS_MSG_TYPE_LINK:
|
||||
switch (rsp->payload.link.state) {
|
||||
case OPAHRS_MSG_LINK_STATE_BUSY:
|
||||
/* Wait for a small delay and retry */
|
||||
vTaskDelay(20 / portTICK_RATE_MS);
|
||||
continue;
|
||||
case OPAHRS_MSG_LINK_STATE_INACTIVE:
|
||||
case OPAHRS_MSG_LINK_STATE_READY:
|
||||
/* somehow, we've missed our response */
|
||||
return OPAHRS_RESULT_FAILED;
|
||||
}
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_USER_V0:
|
||||
/* This isn't the type we expected */
|
||||
return OPAHRS_RESULT_FAILED;
|
||||
break;
|
||||
case OPAHRS_MSG_TYPE_USER_V1:
|
||||
if (rsp->payload.user.t == tag) {
|
||||
return OPAHRS_RESULT_OK;
|
||||
} else {
|
||||
return OPAHRS_RESULT_FAILED;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return OPAHRS_RESULT_TIMEOUT;
|
||||
}
|
||||
|
||||
enum opahrs_result PIOS_OPAHRS_GetSerial(struct opahrs_msg_v1 *rsp)
|
||||
{
|
||||
struct opahrs_msg_v1 req;
|
||||
enum opahrs_result rc;
|
||||
|
||||
if (!rsp) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Make up a serial number request */
|
||||
opahrs_msg_v1_init_user_tx (&req, OPAHRS_MSG_V1_REQ_SERIAL);
|
||||
|
||||
/* 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_SERIAL, rsp);
|
||||
}
|
||||
|
||||
enum opahrs_result PIOS_OPAHRS_resync(void)
|
||||
{
|
||||
struct opahrs_msg_v1 req;
|
||||
struct opahrs_msg_v1 rsp;
|
||||
|
||||
enum opahrs_result rc = OPAHRS_RESULT_FAILED;
|
||||
|
||||
opahrs_msg_v1_init_link_tx(&req, OPAHRS_MSG_LINK_TAG_NOP);
|
||||
|
||||
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
|
||||
vTaskDelay(20 / portTICK_RATE_MS);
|
||||
|
||||
for (uint32_t i = 0; i < sizeof(req); i++) {
|
||||
/* Tx a shortened (by one byte) message to walk through all byte positions */
|
||||
opahrs_msg_v1_init_rx(&rsp);
|
||||
PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, (uint8_t *)&req, (uint8_t *)&rsp, sizeof(req)-1, NULL);
|
||||
|
||||
/* Good magic means we're sync'd */
|
||||
if ((rsp.head.magic == OPAHRS_MSG_MAGIC_HEAD) &&
|
||||
(rsp.tail.magic == OPAHRS_MSG_MAGIC_TAIL)) {
|
||||
/* We need to shift out one more byte to compensate for the short tx */
|
||||
PIOS_SPI_TransferByte(PIOS_OPAHRS_SPI, 0x00);
|
||||
rc = OPAHRS_RESULT_OK;
|
||||
break;
|
||||
}
|
||||
vTaskDelay(10 / portTICK_RATE_MS);
|
||||
}
|
||||
|
||||
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1);
|
||||
//vTaskDelay(5 / portTICK_RATE_MS);
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
extern enum opahrs_result PIOS_OPAHRS_Sync(struct opahrs_msg_v1 *rsp)
|
||||
{
|
||||
struct opahrs_msg_v1 req;
|
||||
enum opahrs_result rc;
|
||||
|
||||
if (!rsp) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Make up a sync request */
|
||||
opahrs_msg_v1_init_user_tx (&req, OPAHRS_MSG_V1_REQ_SYNC);
|
||||
req.payload.user.v.req.sync.cookie = 0xDEADBEEF;
|
||||
|
||||
/* 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_SYNC, rsp);
|
||||
}
|
||||
|
||||
|
||||
enum opahrs_result PIOS_OPAHRS_GetHeading(struct opahrs_msg_v1 *rsp)
|
||||
{
|
||||
struct opahrs_msg_v1 req;
|
||||
enum opahrs_result rc;
|
||||
|
||||
if (!rsp) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Make up a serial number request */
|
||||
opahrs_msg_v1_init_user_tx (&req, OPAHRS_MSG_V1_REQ_HEADING);
|
||||
|
||||
/* 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_HEADING, rsp);
|
||||
}
|
||||
|
||||
enum opahrs_result PIOS_OPAHRS_GetAttitude(struct opahrs_msg_v1 *rsp)
|
||||
{
|
||||
struct opahrs_msg_v1 req;
|
||||
enum opahrs_result rc;
|
||||
|
||||
if (!rsp) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Make up a serial number request */
|
||||
opahrs_msg_v1_init_user_tx (&req, OPAHRS_MSG_V1_REQ_ATTITUDE);
|
||||
|
||||
/* 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_ATTITUDE, rsp);
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_OPAHRS */
|
||||
|
||||
@ -26,19 +26,19 @@
|
||||
#ifndef PIOS_OPAHRS_H
|
||||
#define PIOS_OPAHRS_H
|
||||
|
||||
/* Local Types */
|
||||
#include "pios_opahrs_proto.h" /* opahrs message structs */
|
||||
|
||||
/* Global Variables */
|
||||
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||
extern xSemaphoreHandle PIOS_OPAHRS_EOT;
|
||||
#else
|
||||
extern int32_t PIOS_OPAHRS_EOT;
|
||||
#endif
|
||||
enum opahrs_result {
|
||||
OPAHRS_RESULT_OK = 0,
|
||||
OPAHRS_RESULT_TIMEOUT,
|
||||
OPAHRS_RESULT_FAILED,
|
||||
};
|
||||
|
||||
/* Public Functions */
|
||||
extern void PIOS_OPAHRS_Init(void);
|
||||
extern void PIOS_OPAHRS_ReadAttitude(void);
|
||||
extern int32_t PIOS_OPAHRS_Read(uint8_t address, uint8_t *buffer, uint8_t len);
|
||||
extern int32_t PIOS_OPAHRS_Write(uint8_t address, uint8_t buffer);
|
||||
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_GetHeading(struct opahrs_msg_v1 *rsp);
|
||||
extern enum opahrs_result PIOS_OPAHRS_resync(void);
|
||||
|
||||
#endif /* PIOS_OPAHRS_H */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user