mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-30 08:24:11 +01:00
17bea4cb91
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
268 lines
7.3 KiB
C
268 lines
7.3 KiB
C
/**
|
|
******************************************************************************
|
|
*
|
|
* @file pios_opahrs.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief IRQ Enable/Disable routines
|
|
* @see The GNU Public License (GPL) Version 3
|
|
* @defgroup PIOS_OPAHRS OPAHRS Functions
|
|
* @{
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* 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
|
|
*/
|
|
|
|
/* Project Includes */
|
|
#include "pios.h"
|
|
|
|
#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);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
static enum opahrs_result opahrs_msg_v1_send_req (const struct opahrs_msg_v1 * req)
|
|
{
|
|
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;
|
|
}
|
|
|
|
static enum opahrs_result opahrs_msg_v1_recv_rsp (enum opahrs_msg_v1_tag tag, struct opahrs_msg_v1 * rsp)
|
|
{
|
|
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 */
|