//----------------------------------------------------------------------------
void
spiEepromWritePage(spiEepromDriver * sedp, uint16_t page, const uint8_t * buf)
{
uint32_t addr = page << SPIEEPROM_PAGE_SIZE_SHIFT;
spiStart(sedp->spip, &sedp->cfgp->spicfg);
spiSelect(sedp->spip);
spiPolledExchange(sedp->spip, OP_WRITE);
#ifdef SPIEEPROM_24BIT_ADDRESS
spiPolledExchange(sedp->spip, (addr>>16)&0xff);
#endif
spiPolledExchange(sedp->spip, (addr>>8)&0xff);
spiPolledExchange(sedp->spip, addr&0xff);
for ( uint16_t idx = 0 ; idx < SPIEEPROM_PAGE_SIZE ; ++idx )
spiPolledExchange(sedp->spip, *buf++);
spiUnselect(sedp->spip);
}
/**
* @brief Stops a sequential read gracefully.
*
* @param[in] mmcp pointer to the @p MMCDriver object
*
* @return The operation status.
* @retval CH_SUCCESS the operation succeeded.
* @retval CH_FAILED the operation failed.
*
* @api
*/
bool_t mmcStopSequentialRead(MMCDriver *mmcp) {
static const uint8_t stopcmd[] = {0x40 | MMCSD_CMD_STOP_TRANSMISSION,
0, 0, 0, 0, 1, 0xFF};
chDbgCheck(mmcp != NULL, "mmcStopSequentialRead");
if (mmcp->state != BLK_READING)
return CH_FAILED;
spiSend(mmcp->config->spip, sizeof(stopcmd), stopcmd);
/* result = recvr1(mmcp) != 0x00;*/
/* Note, ignored r1 response, it can be not zero, unknown issue.*/
(void) recvr1(mmcp);
/* Read operation finished.*/
spiUnselect(mmcp->config->spip);
mmcp->state = BLK_READY;
return CH_SUCCESS;
}
msg_t Mtd25::spi_read(uint8_t *rxbuf, size_t len,
uint8_t *writebuf, size_t preamble_len) {
#if SPI_USE_MUTUAL_EXCLUSION
spiAcquireBus(this->spip);
#endif
osalDbgCheck((nullptr != rxbuf) && (0 != len));
spiSelect(spip);
spiSend(spip, preamble_len, writebuf);
spiReceive(spip, len, rxbuf);
spiUnselect(spip);
#if SPI_USE_MUTUAL_EXCLUSION
spiReleaseBus(this->spip);
#endif
return MSG_OK;
}
static int cjReadRegister(unsigned char regAddr) {
#if ! EFI_UNIT_TEST
spiSelect(driver);
tx_buff[0] = regAddr;
spiSend(driver, 1, tx_buff);
// safety?
chThdSleepMilliseconds(10);
rx_buff[0] = 0;
spiReceive(driver, 1, rx_buff);
spiUnselect(driver);
#ifdef CJ125_DEBUG_SPI
scheduleMsg(logger, "cjReadRegister: addr=%d answer=%d", regAddr, rx_buff[0]);
#endif /* CJ125_DEBUG_SPI */
return rx_buff[0];
#else /* EFI_UNIT_TEST */
return 0;
#endif /* EFI_UNIT_TEST */
}
void spiInit(void)
{
#warning spi stuff is currently disabled
#if 0
/* set SS, SCLK and MOSI to output mode, and MISO to input mode */
writeIO(&DDR_SPI, SPI_ALL, SPI_ALL ^ SPI_MISO);
writeIO(&PORT_SPI, SPI_SS_NULL, SPI_SS_NULL);
/* ensure that all slave select pins are high */
spiUnselect();
/* disable power saving for SPI, and enable hardware SPI with interrupts
* disabled and master mode enabled */
writeIO(&PRR0, _BV(PRSPI), 0);
writeIO(&SPCR, _BV(SPE) | _BV(MSTR), _BV(SPE) | _BV(MSTR));
/* read the status and data registers to clear the state */
readIO(&SPSR, 0xff);
readIO(&SPDR, 0xff);
#endif
}
static THD_FUNCTION(Thread1, arg) {
static uint8_t txbuf[5];
static uint8_t rxbuf[5];
(void)arg;
chRegSetThreadName("Blinker");
while (TRUE) {
palSetPad(GPIOB, GPIOB_LED);
/* Send the Manufacturer and Device ID Read command */
txbuf[0] = 0x9F;
spiSelect(&SPID1);
spiExchange(&SPID1, sizeof(txbuf), txbuf, rxbuf);
spiUnselect(&SPID1);
chThdSleepMilliseconds(1000);
}
return 0;
}
/**
* @brief Writes a value into a register.
* @pre The SPI interface must be initialized and the driver started.
*
* @param[in] spip pointer to the SPI initerface
* @param[in] reg register number
* @param[in] value the value to be written
*/
void lis302dlWriteRegister(SPIDriver *spip, uint8_t reg, uint8_t value) {
switch (reg) {
default:
/* Reserved register must not be written, according to the datasheet
this could permanently damage the device.*/
osalDbgAssert(FALSE, "reserved register");
case LIS302DL_WHO_AM_I:
case LIS302DL_HP_FILTER_RESET:
case LIS302DL_STATUS_REG:
case LIS302DL_OUTX:
case LIS302DL_OUTY:
case LIS302DL_OUTZ:
case LIS302DL_FF_WU_SRC1:
case LIS302DL_FF_WU_SRC2:
case LIS302DL_CLICK_SRC:
/* Read only registers cannot be written, the command is ignored.*/
return;
case LIS302DL_CTRL_REG1:
case LIS302DL_CTRL_REG2:
case LIS302DL_CTRL_REG3:
case LIS302DL_FF_WU_CFG1:
case LIS302DL_FF_WU_THS1:
case LIS302DL_FF_WU_DURATION1:
case LIS302DL_FF_WU_CFG2:
case LIS302DL_FF_WU_THS2:
case LIS302DL_FF_WU_DURATION2:
case LIS302DL_CLICK_CFG:
case LIS302DL_CLICK_THSY_X:
case LIS302DL_CLICK_THSZ:
case LIS302DL_CLICK_TIMELIMIT:
case LIS302DL_CLICK_LATENCY:
case LIS302DL_CLICK_WINDOW:
spiSelect(spip);
txbuf[0] = reg;
txbuf[1] = value;
spiSend(spip, 2, txbuf);
spiUnselect(spip);
}
}
/*
* Application entry point.
*/
void main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* OS initialization.
*/
chSysInit();
/*
* Activates the SPI driver 1 using the driver default configuration.
*/
spiStart(&SPID1, &spicfg);
/*
* Normal main() thread activity.
*/
while (TRUE) {
volatile uint8_t b;
chThdSleepMilliseconds(1000);
/* Exchanging data, if the pins MISO and MOSI are connected then the
transmitted data is received back into the buffer. On the
STM8S-Discovery board the pins are CN2-9 and CN2-10.*/
spiSelect(&SPID1);
spiExchange(&SPID1, sizeof(digits), digits, buffer);
/* Polled transfers test.*/
b = spiPolledExchange(&SPID1, 0x55);
b = spiPolledExchange(&SPID1, 0xAA);
spiUnselect(&SPID1);
}
}
msg_t Mtd25::spi_write(const uint8_t *txdata, size_t len,
uint8_t *writebuf, size_t preamble_len) {
memcpy(&writebuf[preamble_len], txdata, len);
if (MSG_OK != spi_write_enable())
return MSG_RESET;
#if SPI_USE_MUTUAL_EXCLUSION
spiAcquireBus(this->spip);
#endif
spiSelect(spip);
spiSend(spip, preamble_len + len, writebuf);
spiUnselect(spip);
#if SPI_USE_MUTUAL_EXCLUSION
spiReleaseBus(this->spip);
#endif
return wait_op_complete(cfg.programtime);
}
/**
* @brief Reads a block within a sequential read operation.
*
* @param[in] mmcp pointer to the @p MMCDriver object
* @param[out] buffer pointer to the read buffer
*
* @return The operation status.
* @retval CH_SUCCESS the operation succeeded.
* @retval CH_FAILED the operation failed.
*
* @api
*/
bool_t mmcSequentialRead(MMCDriver *mmcp, uint8_t *buffer) {
int i;
chDbgCheck((mmcp != NULL) && (buffer != NULL), "mmcSequentialRead");
if (mmcp->state != BLK_READING)
return CH_FAILED;
for (i = 0; i < MMC_WAIT_DATA; i++) {
spiReceive(mmcp->config->spip, 1, buffer);
if (buffer[0] == 0xFE) {
spiReceive(mmcp->config->spip, MMCSD_BLOCK_SIZE, buffer);
/* CRC ignored. */
spiIgnore(mmcp->config->spip, 2);
return CH_SUCCESS;
}
}
/* Timeout.*/
spiUnselect(mmcp->config->spip);
spiStop(mmcp->config->spip);
return CH_FAILED;
}
msg_t Mtd25::wait_op_complete(systime_t timeout) {
systime_t start = chVTGetSystemTimeX();
systime_t end = start + timeout;
osalThreadSleepMilliseconds(2);
while (chVTIsSystemTimeWithinX(start, end)) {
uint8_t tmp;
#if SPI_USE_MUTUAL_EXCLUSION
spiAcquireBus(this->spip);
#endif
spiSelect(spip);
spiPolledExchange(spip, S25_CMD_RDSR1);
tmp = spiPolledExchange(spip, 0);
spiUnselect(spip);
#if SPI_USE_MUTUAL_EXCLUSION
spiReleaseBus(this->spip);
#endif
if (tmp & S25_SR1_WIP) {
continue;
}
else {
if (tmp & (S25_SR1_PERR | S25_SR1_EERR))
return MSG_RESET;
else
return MSG_OK;
}
osalThreadSleepMilliseconds(10);
}
/* time is out */
return MSG_RESET;
}
void sc_spi_exchange(uint8_t spin, uint8_t *tx, uint8_t *rx, size_t bytes)
{
SPIConfig spi_cfg;
chDbgAssert(spin < SC_SPI_MAX_CLIENTS, "SPI n outside range", "#3");
chDbgAssert(spi_conf[spin].spip != NULL, "SPI n not initialized", "#2");
/* spiStart always to set CS for every call according to SPI client. */
spi_cfg.end_cb = NULL;
spi_cfg.ssport = spi_conf[spin].cs_port;
spi_cfg.sspad = spi_conf[spin].cs_pin;
spi_cfg.cr1 = spi_conf[spin].cr1;
spiStart(spi_conf[spin].spip, &spi_cfg);
spiAcquireBus(spi_conf[spin].spip);
spiSelect(spi_conf[spin].spip);
spiExchange(spi_conf[spin].spip, bytes, tx, rx);
spiUnselect(spi_conf[spin].spip);
spiReleaseBus(spi_conf[spin].spip);
spiStop(spi_conf[spin].spip);
}
/**
* @brief Writes a block within a sequential write operation.
*
* @param[in] mmcp pointer to the @p MMCDriver object
* @param[out] buffer pointer to the write buffer
*
* @return The operation status.
* @retval CH_SUCCESS the operation succeeded.
* @retval CH_FAILED the operation failed.
*
* @api
*/
bool_t mmcSequentialWrite(MMCDriver *mmcp, const uint8_t *buffer) {
static const uint8_t start[] = {0xFF, 0xFC};
uint8_t b[1];
chDbgCheck((mmcp != NULL) && (buffer != NULL), "mmcSequentialWrite");
if (mmcp->state != BLK_WRITING)
return CH_FAILED;
spiSend(mmcp->config->spip, sizeof(start), start); /* Data prologue. */
spiSend(mmcp->config->spip, MMCSD_BLOCK_SIZE, buffer);/* Data. */
spiIgnore(mmcp->config->spip, 2); /* CRC ignored. */
spiReceive(mmcp->config->spip, 1, b);
if ((b[0] & 0x1F) == 0x05) {
wait(mmcp);
return CH_SUCCESS;
}
/* Error.*/
spiUnselect(mmcp->config->spip);
spiStop(mmcp->config->spip);
return CH_FAILED;
}
void sc_spi_send(uint8_t spin, uint8_t *data, size_t bytes)
{
SPIConfig spi_cfg;
chDbgAssert(spin < SC_SPI_MAX_CLIENTS, "SPI n outside range", "#4");
chDbgAssert(spi_conf[spin].spip != NULL, "SPI n not initialized", "#3");
/* spiStart always to set CS per SPI client. */
spi_cfg.end_cb = NULL;
spi_cfg.ssport = spi_conf[spin].cs_port;
spi_cfg.sspad = spi_conf[spin].cs_pin;
spi_cfg.cr1 = spi_conf[spin].cr1;
spiStart(spi_conf[spin].spip, &spi_cfg);
spiAcquireBus(spi_conf[spin].spip);
spiSelect(spi_conf[spin].spip);
spiSend(spi_conf[spin].spip, bytes, data);
spiUnselect(spi_conf[spin].spip);
spiReleaseBus(spi_conf[spin].spip);
spiStop(spi_conf[spin].spip);
}
/**
* @brief Reads a block within a sequential read operation.
*
* @param[in] mmcp pointer to the @p MMCDriver object
* @param[out] buffer pointer to the read buffer
*
* @return The operation status.
* @retval HAL_SUCCESS the operation succeeded.
* @retval HAL_FAILED the operation failed.
*
* @api
*/
bool mmcSequentialRead(MMCDriver *mmcp, uint8_t *buffer) {
unsigned i;
osalDbgCheck((mmcp != NULL) && (buffer != NULL));
if (mmcp->state != BLK_READING) {
return HAL_FAILED;
}
for (i = 0; i < MMC_WAIT_DATA; i++) {
spiReceive(mmcp->config->spip, 1, buffer);
if (buffer[0] == 0xFEU) {
spiReceive(mmcp->config->spip, MMCSD_BLOCK_SIZE, buffer);
/* CRC ignored. */
spiIgnore(mmcp->config->spip, 2);
return HAL_SUCCESS;
}
}
/* Timeout.*/
spiUnselect(mmcp->config->spip);
spiStop(mmcp->config->spip);
mmcp->state = BLK_READY;
return HAL_FAILED;
}
/**
* @brief Stops a sequential write gracefully.
*
* @param[in] mmcp pointer to the @p MMCDriver object
* @return The operation status.
* @retval FALSE the operation succeeded.
* @retval TRUE the operation failed.
*
* @api
*/
bool_t mmcStopSequentialWrite(MMCDriver *mmcp) {
static const uint8_t stop[] = {0xFD, 0xFF};
chDbgCheck(mmcp != NULL, "mmcStopSequentialWrite");
chSysLock();
if (mmcp->state != MMC_WRITING) {
chSysUnlock();
return TRUE;
}
chSysUnlock();
spiSend(mmcp->spip, sizeof(stop), stop);
spiUnselect(mmcp->spip);
chSysLock();
if (mmcp->state == MMC_WRITING) {
mmcp->state = MMC_READY;
chSysUnlock();
return FALSE;
}
chSysUnlock();
return TRUE;
}
LLDSPEC bool_t gdisp_lld_init(GDisplay *g)
{
unsigned char j;
// No private area for this controller
g->priv = 0;
// Initialise the board interface
init_board(g);
// Hardware reset
spiUnselect(&DISP_SPI_PORT);
CLR_RS;
setpin_reset(g, TRUE);
delayms(10);
setpin_reset(g, FALSE);
delayms(10);
SET_RS;
spiSelect(&DISP_SPI_PORT);
for (j = 0; j < 134; j++) {
write_cmd(g, initData[j]);
}
delayms(10);
spiUnselect(&DISP_SPI_PORT);
delayms(10);
spiSelect(&DISP_SPI_PORT);
write_cmd(g, 0xf0);
write_cmd(g, 0x81);
write_cmd(g, 0xf4);
write_cmd(g, 0xb3);
write_cmd(g, 0xa0);
write_cmd(g, 0xf0);
write_cmd(g, 0x06);
write_cmd(g, 0x10);
write_cmd(g, 0x20);
write_cmd(g, 0x30);
write_cmd(g, 0xf5);
write_cmd(g, 0x0f);
write_cmd(g, 0x1c);
write_cmd(g, 0x2f);
write_cmd(g, 0x34);
write_cmd(g, 0xf4);
write_cmd(g, 0x94);
write_cmd(g, 0xb3);
write_cmd(g, 0xa2);
write_cmd(g, 0xd0);
spiUnselect(&DISP_SPI_PORT);
CLR_RS;
spiStop(&DISP_SPI_PORT);
spiStart(&DISP_SPI_PORT, &spi2cfg2);
/* Initialise the GDISP structure to match */
g->g.Width = GDISP_SCREEN_WIDTH;
g->g.Height = GDISP_SCREEN_HEIGHT;
g->g.Orientation = GDISP_ROTATE_0;
g->g.Powermode = powerOn;
g->g.Backlight = GDISP_INITIAL_BACKLIGHT;
g->g.Contrast = GDISP_INITIAL_CONTRAST;
return TRUE;
}
void spiEnd(void)
{
spiUnselect();
}
static THD_FUNCTION(Thread1, arg) {
static int8_t xbuf[4], ybuf[4]; /* Last accelerometer data.*/
systime_t time; /* Next deadline.*/
(void)arg;
chRegSetThreadName("reader");
/* LIS302DL initialization.*/
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG1, 0x43);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG2, 0x00);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG3, 0x00);
/* Reader thread loop.*/
time = chVTGetSystemTime();
while (TRUE) {
int32_t x, y;
unsigned i;
/* Keeping an history of the latest four accelerometer readings.*/
for (i = 3; i > 0; i--) {
xbuf[i] = xbuf[i - 1];
ybuf[i] = ybuf[i - 1];
}
/* Reading MEMS accelerometer X and Y registers.*/
xbuf[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTX);
ybuf[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTY);
/* Transmitting accelerometer the data over SPI2.*/
spiSelect(&SPID2);
spiSend(&SPID2, 4, xbuf);
spiSend(&SPID2, 4, ybuf);
spiUnselect(&SPID2);
/* Calculating average of the latest four accelerometer readings.*/
x = ((int32_t)xbuf[0] + (int32_t)xbuf[1] +
(int32_t)xbuf[2] + (int32_t)xbuf[3]) / 4;
y = ((int32_t)ybuf[0] + (int32_t)ybuf[1] +
(int32_t)ybuf[2] + (int32_t)ybuf[3]) / 4;
/* Reprogramming the four PWM channels using the accelerometer data.*/
if (y < 0) {
pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)-y);
pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)0);
}
else {
pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)y);
pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)0);
}
if (x < 0) {
pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)-x);
pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)0);
}
else {
pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)x);
pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)0);
}
/* Waiting until the next 250 milliseconds time interval.*/
chThdSleepUntil(time += MS2ST(100));
}
}
void frontend_configure(void)
{
spiStart(&FRONTEND_SPI, &spi_config);
spiAcquireBus(&FRONTEND_SPI);
spiSelect(&FRONTEND_SPI);
for (u8 i = 0; i < 2; ++i) {
spi_write(2, 0x03);
spi_write(3, 0x01);
spi_write(4, 0x03);
spi_write(5, 0x00);
spi_write(6, 0x1F);
spi_write(9, 0x00);
spi_write(11, 0x08);
spi_write(12, 0x1C);
spi_write(13, 0x03);
spi_write(14, 0x69);
spi_write(15, 0x2B);
spi_write(16, 0x74);
spi_write(17, 0xF1);
spi_write(18, 0xEA);
spi_write(19, 0x0B);
spi_write(20, 0x01);
spi_write(21, 0x28);
spi_write(22, 0x2B);
spi_write(23, 0x74);
spi_write(24, 0xF1);
spi_write(25, 0xEA);
spi_write(26, 0x0B);
spi_write(27, 0x01);
spi_write(28, 0x28);
spi_write(29, 0x2B);
spi_write(30, 0x74);
spi_write(31, 0xF1);
spi_write(32, 0xEA);
spi_write(33, 0x0B);
spi_write(34, 0x03);
spi_write(35, 0x7F);
spi_write(36, 0x2B);
spi_write(37, 0x74);
spi_write(38, 0xF1);
spi_write(39, 0xEA);
spi_write(40, 0x0B);
spi_write(41, 0x03);
spi_write(42, 0x4F);
spi_write(43, 0x89);
spi_write(45, 0x01);
spi_write(46, 0x7B);
spi_write(47, 0x91);
spi_write(49, 0xBD);
spi_write(50, 0x0C);
spi_write(51, 0x64);
spi_write(52, 0xA8);
spi_write(53, 0x5B);
spi_write(54, 0xE5);
spi_write(55, 0x57);
spi_write(56, 0xE4);
spi_write(57, 0xDD);
spi_write(58, 0x0C);
spi_write(59, 0x64);
spi_write(60, 0xA8);
spi_write(61, 0xC5);
spi_write(62, 0x6D);
spi_write(63, 0x57);
spi_write(64, 0x71);
spi_write(65, 0x7F);
spi_write(66, 0x00);
spi_write(67, 0xC9);
spi_write(68, 0x86);
spi_write(69, 0x7E);
spi_write(70, 0x0F);
spi_write(71, 0x11);
spi_write(72, 0x00);
spi_write(73, 0x00);
spi_write(74, 0xFF);
spi_write(75, 0x1C);
spi_write(76, 0xCD);
spi_write(77, 0x99);
spi_write(78, 0x0B);
spi_write(79, 0x40);
spi_write(80, 0x08);
spi_write(81, 0x30);
spi_write(82, 0xFF);
spi_write(83, 0x1C);
spi_write(84, 0xCD);
spi_write(85, 0x99);
spi_write(86, 0x0B);
spi_write(87, 0x40);
spi_write(88, 0x08);
spi_write(89, 0x30);
spi_write(90, 0xFF);
spi_write(91, 0x0C);
spi_write(92, 0xCD);
spi_write(93, 0x99);
spi_write(94, 0x0B);
spi_write(95, 0x40);
spi_write(96, 0x08);
spi_write(97, 0x30);
spi_write(98, 0xFF);
spi_write(99, 0x0C);
spi_write(100, 0xCD);
spi_write(101, 0x99);
spi_write(102, 0x0B);
spi_write(103, 0x40);
spi_write(104, 0x08);
spi_write(105, 0x30);
}
spi_write(15, 0x0B);
spi_write(22, 0x0B);
spi_write(29, 0x0B);
spi_write(36, 0x0B);
spiUnselect(&FRONTEND_SPI);
spiReleaseBus(&FRONTEND_SPI);
}
__attribute__((noreturn)) msg_t InstrumentThread(void *arg) {
size_t rval;
#ifndef RELEASE
size_t wval;
#endif
// uint16_t aCheckSum = 0;
uint16_t aCheckSum = 2056; //Ryan checksum check
#ifdef _TEST_BBI2C
uint8_t status;
static uint8_t txbuf[32];
static uint8_t rxbuf[32];
#endif // _TEST_BBI2C
ChipDriverStatus_t chipStatus = SUCCESS;
// volatile int32_t dly = 0, dmmy = 0;
// int j,k;
// uint8_t bit, pldata;
(void)arg;
chRegSetThreadName("Instrument");
/* Reader thread loop.*/
while (!USBconfigured) chThdSleep(100); //Wait here until USB hw is configured
RED_OFF;
while (TRUE) {
bzero(&pktInBuf,sizeof(usb_packet_t));//Zeros out the mem location to store pktInBuf
rval=readPacket(&pktInBuf,0);//Reads in a new packet
// Command Dispatcher Requirements:
// See usbcmdio.h for structures
// defining each kind of message
// and payload
//
// Recognize 9 commands: ACK, NAK, RESET, ID, WRITE, READ, ECHO, SHADOW, ADC
// DB1_HI;
switch (pktInBuf.type) {
case CMD_ACK:
pktInBuf.length = 4;
// aCheckSum = compute_fletch(&pktInBuf);
pktInBuf.checksum = aCheckSum; // TODO: compute FLETCH
#ifdef RELEASE
writePacket(&pktInBuf,0);
#else
wval=writePacket(&pktInBuf,0);
dprintf("ACK sent %u \r\n",wval);
#endif
break;
case CMD_NAK:
pktInBuf.length = 4;
pktInBuf.type = CMD_ACK; // all packet's ACK unless error
// aCheckSum = compute_fletch(&pktInBuf);
pktInBuf.checksum = aCheckSum; // TODO: compute FLETCH
#ifdef RELEASE
writePacket(&pktInBuf,0);
#else
wval=writePacket(&pktInBuf,0);
dprintf("NAK sent %u\r\n",wval);
#endif
break;
case CMD_RESET:
pktInBuf.length = 4;
if (chipStatus == SUCCESS)
pktInBuf.type = CMD_ACK; // all packet's ACK unless error
else
pktInBuf.type = CMD_NAK;
// aCheckSum = compute_fletch(&pktInBuf);
pktInBuf.checksum = aCheckSum; // TODO: compute FLETCH
#ifdef RELEASE
writePacket(&pktInBuf,0);
#else
wval=writePacket(&pktInBuf,0);
dprintf("RESET sent %u\r\n",wval);
#endif
break;
case CMD_ID:
dprintf("ID \r\n");
get_instrument_ID(&pktInBuf); // my_id;
// pktInBuf.length=4+sizeof(payload_id_response_t);
if (chipStatus == SUCCESS)
pktInBuf.type = CMD_ACK; // all packet's ACK unless error
else
pktInBuf.type = CMD_NAK;
// aCheckSum = compute_fletch(&pktInBuf);
pktInBuf.checksum = aCheckSum; // TODO: compute FLETCH
#ifdef RELEASE
writePacket(&pktInBuf,0);
#else
wval=writePacket(&pktInBuf,0);
dprintf("ID sent %u \r\n",wval);
#endif
break;
case CMD_ECHO:
dprintf("ECHO \r\n");
pktInBuf.type = CMD_ACK; // all packet's ACK unless error
// aCheckSum = compute_fletch(&pktInBuf);
pktInBuf.checksum = aCheckSum; // TODO: compute FLETCH
if (rval > 0) { // echo the incoming packet, if non-zero
#ifdef RELEASE
writePacket(&pktInBuf,0);
#else
wval=writePacket(&pktInBuf,0);
dprintf("ECHO sent %d\r\n",wval);
#endif
}
break;
case CMD_SSN:
dprintf("SSN \r\n");
get_instrument_SSN(&pktInBuf);
if (chipStatus == SUCCESS)
pktInBuf.type = CMD_ACK;
else
pktInBuf.type = CMD_NAK;
// aCheckSum = compute_fletch(&pktInBuf);
pktInBuf.checksum = aCheckSum; // TODO: compute FLETCH
writePacket(&pktInBuf,0);
break;
case CMD_UID:
dprintf("UID \r\n");
get_instrument_UID(&pktInBuf);
if (chipStatus == SUCCESS)
pktInBuf.type = CMD_ACK;
else
pktInBuf.type = CMD_NAK;
// aCheckSum = compute_fletch(&pktInBuf);
pktInBuf.checksum = aCheckSum; // TODO: compute FLETCH
writePacket(&pktInBuf,0);
break;
case CMD_ADC:
dprintf("ADC \r\n");
palTogglePad(GPIOD, 12); //Debug check: Green LED
get_instrument_ADC(&pktInBuf);
if (chipStatus == SUCCESS)
pktInBuf.type = CMD_ACK;
else
pktInBuf.type = CMD_NAK;
// aCheckSum = compute_fletch(&pktInBuf);
pktInBuf.checksum = aCheckSum; // TODO: compute FLETCH
writePacket(&pktInBuf,0);
break;
case CMD_ADC_ALL:
// dprintf("ADC ALL \r\n");
palTogglePad(GPIOD, 12); //Debug check: Green LED
if(!cake.empty) {
// while(!cake.empty){ //seems to run into a race condition
int k=0;
while(k<22){
get_instrument_ADC_ALL(&pktInBuf);
pktInBuf.type = CMD_ACK;
pktInBuf.checksum = aCheckSum;
writePacket(&pktInBuf,0);
k++;
}
} else {
//TODO:fill a packet with zero and send it
pktInBuf.type = CMD_NAK;
pktInBuf.length = 9;
pktInBuf.checksum = aCheckSum;
pktInBuf.payload.adc_resp.current =0;
pktInBuf.payload.adc_resp.voltage =0;
pktInBuf.payload.adc_resp.DUTstate =0;
writePacket(&pktInBuf,0);
}
break;
case CMD_ADC_TEST2:
// dprintf("ADC ALL \r\n");
palTogglePad(GPIOD, 12); //Debug check: Green LED
if(!cake.empty) {
get_instrument_ADC_TEST(&pktInBuf);//Put 2 readings in a packet
if (chipStatus == SUCCESS)
pktInBuf.type = CMD_ACK;
else
pktInBuf.type = CMD_NAK;
pktInBuf.checksum = aCheckSum;
writePacket(&pktInBuf,0);
} else {
pktInBuf.length = 4;
pktInBuf.type = CMD_NAK; // packet's NAK on empty
pktInBuf.checksum = aCheckSum;
writePacket(&pktInBuf,0); //send empty packet NAK
}
break;
default:
dprintf("ERROR: unrecognized command: %u\r\n",pktInBuf.type);
pktInBuf.length = 4;
pktInBuf.type = CMD_NAK; // packet's NAK on error
// aCheckSum = compute_fletch(&pktInBuf);
pktInBuf.checksum = aCheckSum; // TODO: compute FLETCH
writePacket(&pktInBuf,0);
break;
}
#ifdef _SPI_TEST
spiSelect(&SPID2);
spiSend(&SPID2, 8, txbuf);
spiUnselect(&SPID2);
#endif // _SPI_TEST
#ifdef _TEST_BBI2C
#ifdef _BBI2C_INCLUDED
bzero(txbuf,32);
bzero(rxbuf,32);
status = bbI2C_bufio(i2c_addr<<1, 0x80, txbuf, 0, rxbuf, 1);
dprintf("Global reg 0x%x status: %d\r\n",rxbuf[0],status);
status = bbI2C_bufio(i2c_addr<<1, 0x0, txbuf, 12, rxbuf, 0);
dprintf("Clear reg A chan 0 status: %d\r\n",status);
status = bbI2C_bufio(i2c_addr<<1, 0x20, txbuf, 12, rxbuf, 0);
dprintf("Clear reg B chan 0 status: %d\r\n",status);
status = bbI2C_bufio(i2c_addr<<1, 0x40, txbuf, 12, rxbuf, 0);
dprintf("Clear reg A chan 1 status: %d\r\n",status);
status = bbI2C_bufio(i2c_addr<<1, 0x60, txbuf, 12, rxbuf, 0);
dprintf("Clear reg B chan 1 status: %d\r\n",status);
#endif // _BBI2C_INCLUDED
#endif // _TEST_BBI2C
//Blink the LED
RED_ON;
//Here's how to printf out the USB debug shell port
//chprintf(shell_cfg1.sc_channel,".");
}
}
void eeprom_unselect(void){
spiUnselect(&EEPROM_SPI_BUS); /* Slave Select de-assertion.*/
spiReleaseBus(&EEPROM_SPI_BUS); /* Ownership release.*/
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
uint32_t tempR, tempP, tempY;
float Roll,Pitch,Yaw;
halInit();
chSysInit();
i2c_setup();
palSetPadMode(IOPORT1, 8, PAL_MODE_INPUT); // PA8 - RADIO INPUT
palSetPadMode(IOPORT2, 13, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); /* SCK. */
palSetPadMode(IOPORT2, 14, PAL_MODE_STM32_ALTERNATE_PUSHPULL); /* MISO.*/
palSetPadMode(IOPORT2, 15, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); /* MOSI.*/
palSetPadMode(IOPORT2, 12, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
palSetPadMode(IOPORT3, 11, PAL_MODE_OUTPUT_PUSHPULL);
chMtxInit(&mtx_imu);
chMtxInit(&mutex_motors);
palClearPad(IOPORT3,10);
//palClearPad(IOPORT3,11);
/*
* Activates the USB driver and then the USB bus pull-up on D+.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
usbConnectBus(serusbcfg.usbp);
//palClearPad(GPIOC, GPIOC_USB_DISC);
icuStart(&ICUD1, &icucfg);
icuEnable(&ICUD1);
chThdSleepSeconds(1);
chThdCreateStatic(waThread1, sizeof(waThread1), HIGHPRIO, Thread1, NULL);
//chThdCreateStatic(waMotorsThread, sizeof(waMotorsThread), NORMALPRIO, MotorsThread, NULL);
spiAcquireBus(&SPID2); /* Acquire ownership of the bus. */
spiStart(&SPID2, &ls_spicfg); /* Setup transfer parameters. */
spiSelect(&SPID2);
KP = 1;
KI = 1;
KD = 0;
while (TRUE) {
while(!(SPID2.spi->SR & SPI_SR_RXNE));
spiReceive(&SPID2,24,rxbuf_16bit);
// Floating point calculation of Roll/Pitch/Yaw inside the microcontroller. Decomment next 6 lines if you want to implement
// the control Law.
tempR = (uint32_t)((rxbuf_16bit[0] << 31) | (rxbuf_16bit[1] << 23) | (rxbuf_16bit[2] << 11) | rxbuf_16bit[3]);
tempP = (uint32_t)((rxbuf_16bit[4] << 31) | (rxbuf_16bit[5] << 23) | (rxbuf_16bit[6] << 11) | rxbuf_16bit[7]);
tempY = (uint32_t)((rxbuf_16bit[8] << 31) | (rxbuf_16bit[9] << 23) | (rxbuf_16bit[10] << 11) | rxbuf_16bit[11]);
Roll = (*(float*)&tempR);
Pitch = (*(float*)&tempP);
Yaw = (*(float*)&tempY);
// CONTROL LAW HERE
//++ (ROLL,PITCH,YAW ERRORS) -----> [CONTROLLER] -----> (MOTORS INPUT)
// PID Control Law
roll_error = 0 - Roll;
pitch_error = 0 - Pitch;
yaw_error = 0 - Yaw;
roll_I = roll_I + roll_error*0.01;
pitch_I = pitch_I + pitch_error*0.01;
roll_D = (roll_error - roll_prev_error)/0.01;
pitch_D = (pitch_error - pitch_prev_error)/0.01;
roll_controller_output = (int8_t)(KP*roll_error + KI*roll_I + KD*roll_D);
pitch_controller_output = (int8_t)(KP*pitch_error + KI*pitch_I + KD*pitch_D);
//roll_controller_output = (int8_t)roll_error;
//pitch_controller_output = (int8_t)pitch_error;
roll_prev_error = roll_error;
pitch_prev_error = pitch_error;
//-------------------------------------------------------------------
blctrl20_set_velocity();
palSetPad(IOPORT3,11);
/* SPI FLOATING POINT TEST PACKET (sending 5.6F)
rxbuf_16bit[0] = 0;
rxbuf_16bit[1] = 129;
rxbuf_16bit[2] = 1638;
rxbuf_16bit[3] = 819;
rxbuf_16bit[4] = 0;
rxbuf_16bit[5] = 129;
rxbuf_16bit[6] = 1638;
rxbuf_16bit[7] = 819;
rxbuf_16bit[8] = 1;
rxbuf_16bit[9] = 129;
rxbuf_16bit[10] = 1638;
rxbuf_16bit[11] = 819;
*/
if(SDU1.config->usbp->state==USB_ACTIVE)
{
// chprintf((BaseChannel *)&SDU1,"S:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:E\r\n",rxbuf_16bit[0],rxbuf_16bit[1],rxbuf_16bit[2],rxbuf_16bit[3],rxbuf_16bit[4],rxbuf_16bit[5],rxbuf_16bit[6],rxbuf_16bit[7],rxbuf_16bit[8],rxbuf_16bit[9],rxbuf_16bit[10],rxbuf_16bit[11]);
chprintf((BaseChannel *)&SDU1, "S:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:E\r\n",rxbuf_16bit[0],rxbuf_16bit[1],rxbuf_16bit[2],rxbuf_16bit[3],rxbuf_16bit[4],rxbuf_16bit[5],rxbuf_16bit[6],rxbuf_16bit[7],rxbuf_16bit[8],rxbuf_16bit[9],rxbuf_16bit[10],rxbuf_16bit[11],(int8_t)Roll,(int8_t)Pitch,(int8_t)Yaw,icu_ch[3],icu_ch[4],roll_controller_output,pitch_controller_output,yaw_controller_output);
}
chThdSleepMilliseconds(10);
}
spiUnselect(&SPID2);
spiReleaseBus(&SPID2); /* Ownership release. */
}
/// called every 10ms from clock.c - check all temp sensors that are ready for checking
void temp_sensor_tick() {
temp_sensor_t i = 0;
for (; i < NUM_TEMP_SENSORS; i++) {
if (temp_sensors_runtime[i].next_read_time) {
temp_sensors_runtime[i].next_read_time--;
}
else {
uint16_t temp = 0;
#ifdef TEMP_MAX31855
uint32_t value = 0;
struct max31855_plat_data *max31855;
#endif
//time to deal with this temp sensor
switch(temp_sensors[i].temp_type) {
#ifdef TEMP_MAX31855
case TT_MAX31855:
#ifdef __arm__
max31855 = (struct max31855_plat_data*)temp_sensors[i].plat_data;
spiAcquireBus(max31855->bus);
spiStart(max31855->bus, max31855->config);
spiSelect(max31855->bus);
spiReceive(max31855->bus, 4, &value);
spiUnselect(max31855->bus);
spiReleaseBus(max31855->bus);
value = SWAP_UINT32(value);
temp_sensors_runtime[i].temp_flags = 0;
if (value & (1 << 16)) {
#ifdef HALT_ON_TERMOCOUPLE_FAILURE
emergency_stop();
#endif
}
else {
temp = value >> 18;
if (temp & (1 << 13))
temp = 0;
}
#else
temp = 0;
#endif
break;
#endif
#ifdef TEMP_MAX6675
case TT_MAX6675:
#ifdef PRR
PRR &= ~MASK(PRSPI);
#elif defined PRR0
PRR0 &= ~MASK(PRSPI);
#endif
SPCR = MASK(MSTR) | MASK(SPE) | MASK(SPR0);
// enable TT_MAX6675
WRITE(SS, 0);
// No delay required, see
// https://github.com/triffid/Teacup_Firmware/issues/22
// read MSB
SPDR = 0;
for (;(SPSR & MASK(SPIF)) == 0;);
temp = SPDR;
temp <<= 8;
// read LSB
SPDR = 0;
for (;(SPSR & MASK(SPIF)) == 0;);
temp |= SPDR;
// disable TT_MAX6675
WRITE(SS, 1);
temp_sensors_runtime[i].temp_flags = 0;
if ((temp & 0x8002) == 0) {
// got "device id"
temp_sensors_runtime[i].temp_flags |= PRESENT;
if (temp & 4) {
// thermocouple open
temp_sensors_runtime[i].temp_flags |= TCOPEN;
}
else {
temp = temp >> 3;
}
}
// this number depends on how frequently temp_sensor_tick is called. the MAX6675 can give a reading every 0.22s, so set this to about 250ms
temp_sensors_runtime[i].next_read_time = 25;
break;
#endif /* TEMP_MAX6675 */
#ifdef TEMP_THERMISTOR
case TT_THERMISTOR:
do {
uint8_t j, table_num;
//Read current temperature
temp = analog_read(i);
// for thermistors the thermistor table number is in the additional field
table_num = temp_sensors[i].additional;
//Calculate real temperature based on lookup table
for (j = 1; j < NUMTEMPS; j++) {
if (pgm_read_word(&(temptable[table_num][j][0])) > temp) {
// Thermistor table is already in 14.2 fixed point
#ifndef EXTRUDER
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR("pin:%d Raw ADC:%d table entry: %d"),temp_sensors[i].temp_pin,temp,j);
#endif
// Linear interpolating temperature value
// y = ((x - x₀)y₁ + (x₁-x)y₀ ) / (x₁ - x₀)
// y = temp
// x = ADC reading
// x₀= temptable[j-1][0]
// x₁= temptable[j][0]
// y₀= temptable[j-1][1]
// y₁= temptable[j][1]
// y =
// Wikipedia's example linear interpolation formula.
temp = (
// ((x - x₀)y₁
((uint32_t)temp - pgm_read_word(&(temptable[table_num][j-1][0]))) * pgm_read_word(&(temptable[table_num][j][1]))
// +
+
// (x₁-x)
(pgm_read_word(&(temptable[table_num][j][0])) - (uint32_t)temp)
// y₀ )
* pgm_read_word(&(temptable[table_num][j-1][1])))
// /
/
// (x₁ - x₀)
(pgm_read_word(&(temptable[table_num][j][0])) - pgm_read_word(&(temptable[table_num][j-1][0])));
#ifndef EXTRUDER
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR(" temp:%d.%d"),temp/4,(temp%4)*25);
#endif
break;
}
}
#ifndef EXTRUDER
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR(" Sensor:%d\n"),i);
#endif
//Clamp for overflows
if (j == NUMTEMPS)
temp = temptable[table_num][NUMTEMPS-1][1];
temp_sensors_runtime[i].next_read_time = 0;
} while (0);
break;
#endif /* TEMP_THERMISTOR */
#ifdef TEMP_AD595
case TT_AD595:
temp = analog_read(i);
// convert
// >>8 instead of >>10 because internal temp is stored as 14.2 fixed point
temp = (temp * 500L) >> 8;
temp_sensors_runtime[i].next_read_time = 0;
break;
#endif /* TEMP_AD595 */
#ifdef TEMP_PT100
case TT_PT100:
#warning TODO: PT100 code
break
#endif /* TEMP_PT100 */
#ifdef TEMP_INTERCOM
case TT_INTERCOM:
temp = read_temperature(temp_sensors[i].temp_pin);
temp_sensors_runtime[i].next_read_time = 25;
break;
#endif /* TEMP_INTERCOM */
#ifdef TEMP_DUMMY
case TT_DUMMY:
temp = temp_sensors_runtime[i].last_read_temp;
if (temp_sensors_runtime[i].target_temp > temp)
temp++;
else if (temp_sensors_runtime[i].target_temp < temp)
temp--;
temp_sensors_runtime[i].next_read_time = 0;
break;
#endif /* TEMP_DUMMY */
default: /* prevent compiler warning */
break;
}
/* Exponentially Weighted Moving Average alpha constant for smoothing
noisy sensors. Instrument Engineer's Handbook, 4th ed, Vol 2 p126
says values of 0.05 to 0.1 for TEMP_EWMA are typical. */
#ifndef TEMP_EWMA
#define TEMP_EWMA 1.0
#endif
#define EWMA_SCALE 1024L
#define EWMA_ALPHA ((long) (TEMP_EWMA * EWMA_SCALE))
temp_sensors_runtime[i].last_read_temp = (uint16_t) ((EWMA_ALPHA * temp +
(EWMA_SCALE-EWMA_ALPHA) * temp_sensors_runtime[i].last_read_temp
) / EWMA_SCALE);
}
if (labs((int16_t)(temp_sensors_runtime[i].last_read_temp - temp_sensors_runtime[i].target_temp)) < (TEMP_HYSTERESIS*4)) {
if (temp_sensors_runtime[i].temp_residency < (TEMP_RESIDENCY_TIME*120))
temp_sensors_runtime[i].temp_residency++;
}
else {
// Deal with flakey sensors which occasionally report a wrong value
// by setting residency back, but not entirely to zero.
if (temp_sensors_runtime[i].temp_residency > 10)
temp_sensors_runtime[i].temp_residency -= 10;
else
temp_sensors_runtime[i].temp_residency = 0;
}
if (temp_sensors[i].heater < NUM_HEATERS) {
heater_tick(temp_sensors[i].heater, temp_sensors[i].temp_type, temp_sensors_runtime[i].last_read_temp, temp_sensors_runtime[i].target_temp);
}
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR("DU temp: {%d %d %d.%d}"), i,
temp_sensors_runtime[i].last_read_temp,
temp_sensors_runtime[i].last_read_temp / 4,
(temp_sensors_runtime[i].last_read_temp & 0x03) * 25);
}
//==============================================================================
// SdSpiCard member functions
//------------------------------------------------------------------------------
bool SdSpiCard::begin(SdSpiDriver* spi, uint8_t csPin, SPISettings settings) {
m_spiActive = false;
m_errorCode = SD_CARD_ERROR_NONE;
m_type = 0;
m_spiDriver = spi;
uint16_t t0 = curTimeMS();
uint32_t arg;
m_spiDriver->begin(csPin);
m_spiDriver->setSpiSettings(SD_SCK_HZ(250000));
spiStart();
// must supply min of 74 clock cycles with CS high.
spiUnselect();
for (uint8_t i = 0; i < 10; i++) {
spiSend(0XFF);
}
spiSelect();
// command to go idle in SPI mode
while (cardCommand(CMD0, 0) != R1_IDLE_STATE) {
if (isTimedOut(t0, SD_INIT_TIMEOUT)) {
error(SD_CARD_ERROR_CMD0);
goto fail;
}
}
#if USE_SD_CRC
if (cardCommand(CMD59, 1) != R1_IDLE_STATE) {
error(SD_CARD_ERROR_CMD59);
goto fail;
}
#endif // USE_SD_CRC
// check SD version
while (1) {
if (cardCommand(CMD8, 0x1AA) == (R1_ILLEGAL_COMMAND | R1_IDLE_STATE)) {
type(SD_CARD_TYPE_SD1);
break;
}
for (uint8_t i = 0; i < 4; i++) {
m_status = spiReceive();
}
if (m_status == 0XAA) {
type(SD_CARD_TYPE_SD2);
break;
}
if (isTimedOut(t0, SD_INIT_TIMEOUT)) {
error(SD_CARD_ERROR_CMD8);
goto fail;
}
}
// initialize card and send host supports SDHC if SD2
arg = type() == SD_CARD_TYPE_SD2 ? 0X40000000 : 0;
while (cardAcmd(ACMD41, arg) != R1_READY_STATE) {
// check for timeout
if (isTimedOut(t0, SD_INIT_TIMEOUT)) {
error(SD_CARD_ERROR_ACMD41);
goto fail;
}
}
// if SD2 read OCR register to check for SDHC card
if (type() == SD_CARD_TYPE_SD2) {
if (cardCommand(CMD58, 0)) {
error(SD_CARD_ERROR_CMD58);
goto fail;
}
if ((spiReceive() & 0XC0) == 0XC0) {
type(SD_CARD_TYPE_SDHC);
}
// Discard rest of ocr - contains allowed voltage range.
for (uint8_t i = 0; i < 3; i++) {
spiReceive();
}
}
spiStop();
m_spiDriver->setSpiSettings(settings);
return true;
fail:
spiStop();
return false;
}
static flash_error_t program(void *instance, flash_address_t addr,
const uint8_t *pp, size_t n) {
N25Q128Driver *devp = (N25Q128Driver *)instance;
SPIDriver *spip = devp->config->spip;
flash_error_t err;
osalDbgAssert(devp->state == FLASH_READY, "invalid state");
#if N25Q128_SHARED_SPI == TRUE
spiAcquireBus(spip);
spiStart(spip, devp->config->spicfg);
#endif
devp->state = FLASH_ACTIVE;
while (n > 0U) {
uint8_t sts;
/* Data size that can be written in a single program page operation.*/
size_t chunk = (size_t)(((addr | PAGE_MASK) + 1U) - addr);
if (chunk > n) {
chunk = n;
}
/* Enabling write operation.*/
spiSelect(spip);
spi_send_cmd(devp, N25Q128_CMD_WRITE_ENABLE);
spiUnselect(spip);
(void) spiPolledExchange(spip, 0xFF); /* One frame delay.*/
/* Page program command.*/
spiSelect(spip);
spi_send_cmd_addr(devp, N25Q128_CMD_PAGE_PROGRAM, addr);
spiSend(spip, chunk, pp);
spiUnselect(spip);
(void) spiPolledExchange(spip, 0xFF); /* One frame delay.*/
/* Operation end waiting.*/
do {
#if N25Q128_NICE_WAITING == TRUE
osalThreadSleepMilliseconds(1);
#endif
/* Read status command.*/
spiSelect(spip);
spi_send_cmd(devp, N25Q128_CMD_READ_STATUS_REGISTER);
spiReceive(spip, 1, &sts);
spiUnselect(spip);
} while ((sts & N25Q128_STS_BUSY) != 0U);
/* Checking for errors.*/
if ((sts & N25Q128_STS_ALL_ERRORS) != 0U) {
/* Clearing status register.*/
(void) spiPolledExchange(spip, 0xFF); /* One frame delay.*/
spiSelect(spip);
spi_send_cmd(devp, N25Q128_CMD_CLEAR_FLAG_STATUS_REGISTER);
spiUnselect(spip);
/* Program operation failed.*/
err = FLASH_PROGRAM_FAILURE;
goto exit_error;
}
/* Next page.*/
addr += chunk;
pp += chunk;
n -= chunk;
}
/* Program operation succeeded.*/
err = FLASH_NO_ERROR;
/* Common exit path for this function.*/
exit_error:
devp->state = FLASH_READY;
#if N25Q128_SHARED_SPI == TRUE
spiReleaseBus(spip);
#endif
return err;
}
/**
* @brief End a SPI transaction.
*/
static int end_spi(void)
{
spiUnselect(SPI_SX);
spiReleaseBus(SPI_SX);
return 0;
}
void Rf24ChibiosIo::unselect() {
spiUnselect(driver);
}
