mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
3fda65c5d3
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2772 ebee16cc-31ac-478f-84a7-5cbb03baadba
143 lines
3.6 KiB
C
143 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;
|
|
}
|
|
}
|