mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
138 lines
3.6 KiB
C
138 lines
3.6 KiB
C
/**
|
|
******************************************************************************
|
|
*
|
|
* @file ahrs_spi_program_slave.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief AHRS programming over SPI link - slave(AHRS) end.
|
|
*
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* 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
|
|
*/
|
|
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include "pios_opahrs_proto.h"
|
|
#include "pios_spi.h"
|
|
#include "STM32103CB_AHRS.h"
|
|
|
|
#include "ahrs_bl.h"
|
|
#include "ahrs_spi_program_slave.h"
|
|
#include "ahrs_spi_program.h"
|
|
|
|
static AhrsProgramPacket txBuf;
|
|
static AhrsProgramPacket rxBuf;
|
|
static bool done = false;
|
|
|
|
static void ProcessPacket();
|
|
|
|
#define WAIT_IF_RECEIVING() while(!(GPIOB->IDR & GPIO_Pin_12)){}; //NSS must be high
|
|
//Number of crc failures to allow before giving up
|
|
#define PROGRAM_PACKET_TRIES 4
|
|
|
|
void AhrsProgramReceive(uint32_t spi_id) {
|
|
done = false;
|
|
memset(&txBuf, 0, sizeof(AhrsProgramPacket));
|
|
//wait for a program request
|
|
int count = PROGRAM_PACKET_TRIES;
|
|
while (1) {
|
|
WAIT_IF_RECEIVING();
|
|
while ((PIOS_SPI_Busy(spi_id) != 0)) {
|
|
};
|
|
memset(&rxBuf, 'a', sizeof(AhrsProgramPacket));
|
|
int32_t res = PIOS_SPI_TransferBlock(spi_id, NULL, (uint8_t*) &rxBuf,
|
|
SPI_PROGRAM_REQUEST_LENGTH + 1, NULL);
|
|
|
|
if (res == 0 && memcmp(&rxBuf, SPI_PROGRAM_REQUEST,
|
|
SPI_PROGRAM_REQUEST_LENGTH) == 0) {
|
|
break;
|
|
}
|
|
if (count-- == 0) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (!StartProgramming()) {
|
|
//Couldn't erase FLASH. Nothing we can do.
|
|
return;
|
|
}
|
|
|
|
//send ack
|
|
memcpy(&txBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH);
|
|
WAIT_IF_RECEIVING();
|
|
while (0 != PIOS_SPI_TransferBlock(spi_id, (uint8_t*) &txBuf, NULL,
|
|
SPI_PROGRAM_REQUEST_LENGTH + 1, NULL)) {
|
|
};
|
|
|
|
txBuf.type = PROGRAM_NULL;
|
|
|
|
while (!done) {
|
|
WAIT_IF_RECEIVING();
|
|
if (0 == PIOS_SPI_TransferBlock(spi_id, (uint8_t*) &txBuf,
|
|
(uint8_t*) &rxBuf, sizeof(AhrsProgramPacket), NULL)) {
|
|
|
|
uint32_t crc = GenerateCRC(&rxBuf);
|
|
if (crc != rxBuf.crc || txBuf.packetId == rxBuf.packetId) {
|
|
continue;
|
|
}
|
|
ProcessPacket();
|
|
txBuf.packetId = rxBuf.packetId;
|
|
txBuf.crc = GenerateCRC(&txBuf);
|
|
}
|
|
}
|
|
}
|
|
|
|
void ProcessPacket() {
|
|
switch (rxBuf.type) {
|
|
case PROGRAM_NULL:
|
|
txBuf.type = PROGRAM_NULL;
|
|
break;
|
|
|
|
case PROGRAM_WRITE:
|
|
if (WriteData(rxBuf.address, rxBuf.data, rxBuf.size)) {
|
|
txBuf.type = PROGRAM_ACK;
|
|
txBuf.size = rxBuf.size;
|
|
} else {
|
|
txBuf.type = PROGRAM_ERR;
|
|
}
|
|
break;
|
|
|
|
case PROGRAM_READ:
|
|
if (ReadData(rxBuf.address, txBuf.data, rxBuf.size)) {
|
|
txBuf.type = PROGRAM_ACK;
|
|
txBuf.size = rxBuf.size;
|
|
} else {
|
|
txBuf.type = PROGRAM_ERR;
|
|
}
|
|
break;
|
|
|
|
case PROGRAM_REBOOT:
|
|
if (0 == memcmp(rxBuf.data, REBOOT_CONFIRMATION,
|
|
REBOOT_CONFIRMATION_LENGTH)) {
|
|
done = true;
|
|
txBuf.type = PROGRAM_ACK;
|
|
} else {
|
|
txBuf.type = PROGRAM_ERR;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
txBuf.type = PROGRAM_ERR;
|
|
}
|
|
}
|