diff --git a/conf/airframes/examples/quadrotor_navstik.xml b/conf/airframes/examples/quadrotor_navstik.xml
index 15ccd17aa1..b81dc54287 100644
--- a/conf/airframes/examples/quadrotor_navstik.xml
+++ b/conf/airframes/examples/quadrotor_navstik.xml
@@ -45,6 +45,7 @@
+
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index b13eab8dda..8d47b151a0 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -136,7 +136,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/modules/direct_memory_logger.xml"
gui_color="#710080"
/>
+
+
+
+
+ Directly log values to memory for flash chips.
+
+
+
+
+
+
+
+
+
+
+
+
+
+ DM_LOG_UART ?= uart1
+ DM_LOG_UART_LOWER=$(shell echo $(DM_LOG_UART) | tr A-Z a-z)
+ DM_LOG_UART_UPPER=$(shell echo $(DM_LOG_UART) | tr a-z A-Z)
+
+ DM_LOG_SPI_DEV ?= spi2
+ DM_LOG_SPI_DEV_LOWER=$(shell echo $(DM_LOG_SPI_DEV) | tr A-Z a-z)
+ DM_LOG_SPI_DEV_UPPER=$(shell echo $(DM_LOG_SPI_DEV) | tr a-z A-Z)
+
+ DM_LOG_SPI_SLAVE_IDX ?= spi_slave1
+ DM_LOG_SPI_SLAVE_IDX_LOWER=$(shell echo $(DM_LOG_SPI_SLAVE_IDX) | tr A-Z a-z)
+ DM_LOG_SPI_SLAVE_IDX_UPPER=$(shell echo $(DM_LOG_SPI_SLAVE_IDX) | tr a-z A-Z)
+
+ include $(CFG_SHARED)/spi_master.makefile
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/modules/direct_memory_logger.xml b/conf/settings/modules/direct_memory_logger.xml
new file mode 100644
index 0000000000..ae9140aa0f
--- /dev/null
+++ b/conf/settings/modules/direct_memory_logger.xml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/boards/navstik_1.0.h b/sw/airborne/boards/navstik_1.0.h
index db0fd1d04f..786040b46d 100644
--- a/sw/airborne/boards/navstik_1.0.h
+++ b/sw/airborne/boards/navstik_1.0.h
@@ -124,39 +124,19 @@
#define PPM_GPIO_AF GPIO_AF8
/* SPI */
-#define SPI1_GPIO_AF GPIO_AF5
-#define SPI1_GPIO_PORT_MISO GPIOA
-#define SPI1_GPIO_MISO GPIO6
-#define SPI1_GPIO_PORT_MOSI GPIOA
-#define SPI1_GPIO_MOSI GPIO7
-#define SPI1_GPIO_PORT_SCK GPIOA
-#define SPI1_GPIO_SCK GPIO5
-
#define SPI2_GPIO_AF GPIO_AF5
-#define SPI2_GPIO_PORT_MISO GPIOB
-#define SPI2_GPIO_MISO GPIO14
-#define SPI2_GPIO_PORT_MOSI GPIOB
-#define SPI2_GPIO_MOSI GPIO15
+#define SPI2_GPIO_PORT_MISO GPIOC
+#define SPI2_GPIO_MISO GPIO2
+#define SPI2_GPIO_PORT_MOSI GPIOC
+#define SPI2_GPIO_MOSI GPIO3
#define SPI2_GPIO_PORT_SCK GPIOB
-#define SPI2_GPIO_SCK GPIO13
+#define SPI2_GPIO_SCK GPIO10
#define SPI_SELECT_SLAVE0_PORT GPIOA
-#define SPI_SELECT_SLAVE0_PIN GPIO15
+#define SPI_SELECT_SLAVE0_PIN GPIO0
#define SPI_SELECT_SLAVE1_PORT GPIOA
-#define SPI_SELECT_SLAVE1_PIN GPIO4
-
-#define SPI_SELECT_SLAVE2_PORT GPIOB
-#define SPI_SELECT_SLAVE2_PIN GPIO12
-
-#define SPI_SELECT_SLAVE3_PORT GPIOC
-#define SPI_SELECT_SLAVE3_PIN GPIO13
-
-#define SPI_SELECT_SLAVE4_PORT GPIOC
-#define SPI_SELECT_SLAVE4_PIN GPIO12
-
-#define SPI_SELECT_SLAVE5_PORT GPIOC
-#define SPI_SELECT_SLAVE5_PIN GPIO4
+#define SPI_SELECT_SLAVE1_PIN GPIO5
/* I2C mapping */
diff --git a/sw/airborne/modules/loggers/direct_memory_logger.c b/sw/airborne/modules/loggers/direct_memory_logger.c
new file mode 100644
index 0000000000..e2104caab1
--- /dev/null
+++ b/sw/airborne/modules/loggers/direct_memory_logger.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright (C) 2014 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file modules/loggers/direct_memory_logger.c
+ * @brief Write logs directly to flash memory chips
+ */
+
+#include "direct_memory_logger.h"
+#include "mcu_periph/uart.h"
+#include "subsystems/imu.h"
+#include "stabilization.h"
+
+struct DirectMemoryLogger dml;
+static void direct_memory_spi_cb(struct spi_transaction* trans);
+static int32_t seq_in_array(uint8_t* array, uint16_t array_size, uint8_t* sequence, uint16_t sequence_size);
+
+// Different sequences
+static uint8_t start_log_sequence[6] = {0xAA, 0x55, 0xFF, 0x00, 0x55, 0xAA};
+static uint8_t stop_log_sequence[6] = {0xFF, 0x00, 0x55, 0xAA, 0x10, 0xFF};
+
+// The logging output connection
+#define __DMLoggerLink(dev, _x) dev##_x
+#define _DMLoggerLink(dev, _x) __DMLoggerLink(dev, _x)
+#define DMLoggerLink(_x) _DMLoggerLink(DM_LOG_UART, _x)
+
+// Logging struct
+struct LogStruct {
+ uint32_t counter;
+ int32_t accel_z;
+ int32_t gyro_p;
+ int32_t gyro_q;
+ int32_t gyro_r;
+ int32_t thrust;
+} __attribute__((packed));
+static struct LogStruct log_struct;
+static uint32_t dm_counter = 0;
+
+static int32_t seq_in_array(uint8_t* array, uint16_t array_size, uint8_t* sequence, uint16_t sequence_size)
+{
+ uint16_t i;
+ static uint16_t current_sequence_id = 0;
+ static uint16_t count_ff = 0;
+
+ for (i = 0; i < array_size; i++) {
+
+ // Detect stop sequence
+ if (array[i] == sequence[current_sequence_id]) {
+ current_sequence_id++;
+ if (current_sequence_id >= sequence_size) {
+ count_ff = 0;
+ current_sequence_id = 0;
+ return i;
+ }
+ } else {
+ current_sequence_id = 0;
+ }
+
+ // Detect ff sequence
+ if (array[i] == 0xFF) {
+ count_ff++;
+
+ if (count_ff >= 1000) {
+ count_ff = 0;
+ current_sequence_id = 0;
+ return i;
+ }
+ } else {
+ count_ff = 0;
+ }
+ }
+
+ return -1;
+}
+
+void direct_memory_logger_init(void)
+{
+ dml.status = DML_INIT;
+
+ // Initialize the sst chip
+ sst25vfxxxx_init(&dml.sst, &(DM_LOG_SPI_DEV), DM_LOG_SPI_SLAVE_IDX, &direct_memory_spi_cb);
+}
+
+void direct_memory_logger_periodic(void)
+{
+ int32_t seq_idx;
+ uint16_t i, end_idx;
+
+ // Switch the different statusses
+ switch (dml.status) {
+ case DML_IDLE:
+ // Do nothing
+ break;
+
+ // Stopping
+ case DML_STOP:
+ if (dml.sst.status != SST25VFXXXX_IDLE) {
+ break;
+ }
+
+ dml.status = DML_IDLE;
+ dml.write_addr = dml.sst.flash_addr + 6;
+ sst25vfxxxx_write(&dml.sst, stop_log_sequence, 6);
+ break;
+
+ // Logging
+ case DML_START:
+ dm_counter = 0;
+ dml.status = DML_LOGGING;
+ case DML_LOGGING:
+ // Check if too slow TODO fix error
+ dm_counter++;
+ if (dml.sst.status != SST25VFXXXX_IDLE) {
+ break;
+ }
+
+ // Set the log values
+ log_struct.counter++;
+ log_struct.accel_z = imu.accel.z;
+ log_struct.gyro_p = imu.gyro.p;
+ log_struct.gyro_q = imu.gyro.q;
+ log_struct.gyro_r = imu.gyro.r;
+ log_struct.thrust = stabilization_cmd[COMMAND_THRUST];
+
+ sst25vfxxxx_write(&dml.sst, (uint8_t*) &log_struct, sizeof(struct LogStruct));
+ break;
+
+ // Reading
+ case DML_READ:
+ dml.status = DML_READING;
+ case DML_READING:
+
+ if (DMLoggerLink(TxRunning) || dml.sst.status != SST25VFXXXX_IDLE) {
+ break;
+ }
+
+ // Detect end sequence
+ seq_idx = seq_in_array(&(dml.buffer[5]), DML_BUF_SIZE - 5, stop_log_sequence, 6);
+ if (seq_idx < 0) {
+ end_idx = DML_BUF_SIZE;
+ } else {
+ end_idx = seq_idx + 5;
+ dml.status = DML_IDLE;
+ }
+
+ for (i = 5; i < end_idx; i++) {
+ DMLoggerLink(Transmit(dml.buffer[i]));
+ }
+
+ // Read next bytes
+ dml.sst.flash_addr += end_idx - 5;
+ if (seq_idx < 0) {
+ sst25vfxxxx_read(&dml.sst, dml.buffer, DML_BUF_SIZE - 5);
+ }
+ break;
+
+ default:
+ if (dml.sst.status == SST25VFXXXX_IDLE) {
+ dml.status = DML_IDLE;
+ }
+ break;
+ }
+}
+
+void direct_memory_logger_set(uint8_t val)
+{
+ // First handle stopping while logging
+ if (dml.status == DML_LOGGING && val == DML_STOP) {
+ dml.status = DML_STOP;
+ return;
+ }
+
+ // Handle only while idle
+ if (dml.status != DML_IDLE) {
+ return;
+ }
+
+ // Handle all the statuses
+ dml.status = val;
+ switch (dml.status) {
+ case DML_ERASE:
+ dml.sst.flash_addr = 0x0;
+ dml.write_addr = 0x0;
+ sst25vfxxxx_chip_erase(&dml.sst);
+ break;
+ case DML_START:
+ dml.sst.flash_addr = dml.write_addr;
+ sst25vfxxxx_write(&dml.sst, start_log_sequence, 6);
+ break;
+ case DML_READ:
+ dml.sst.flash_addr = 0x0;
+ sst25vfxxxx_read(&dml.sst, dml.buffer, DML_BUF_SIZE - 5);
+ break;
+ default:
+ break;
+ }
+}
+
+static void direct_memory_spi_cb(__attribute__((unused)) struct spi_transaction* trans)
+{
+ sst25vfxxxx_after_cb(&dml.sst);
+}
diff --git a/sw/airborne/modules/loggers/direct_memory_logger.h b/sw/airborne/modules/loggers/direct_memory_logger.h
new file mode 100644
index 0000000000..f1ab26be08
--- /dev/null
+++ b/sw/airborne/modules/loggers/direct_memory_logger.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2014 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file modules/loggers/direct_memory_logger.h
+ * @brief Write logs directly to flash memory chips
+ */
+
+#ifndef DIRECT_MEMORY_LOGGER_H_
+#define DIRECT_MEMORY_LOGGER_H_
+
+#include "peripherals/sst25vfxxxx.h"
+
+#define DML_BUF_SIZE 128 /**< The read buffer size */
+
+/* The different statusses the Direct Memory Logger is able to be in */
+enum DMLStatus {
+ DML_INIT, /**< The DML is initializing */
+ DML_IDLE, /**< The DML is idle */
+ DML_ERASE, /**< The DML is busy erasing itself */
+ DML_START, /**< The DML is starting the logger */
+ DML_LOGGING, /**< The DML is busy logging */
+ DML_STOP, /**< The DML is busy stopping */
+ DML_READ, /**< The DML is busy starting read */
+ DML_READING, /**< The DML is busy reading */
+};
+
+/* Contains all the direct memory information */
+struct DirectMemoryLogger {
+ struct SST25VFxxxx sst; /**< The memory chip */
+ volatile enum DMLStatus status; /**< The status of the Direct Memory Logger */
+ uint8_t buffer[DML_BUF_SIZE]; /**< The buffer for writing and reading */
+ uint32_t write_addr;
+};
+
+extern struct DirectMemoryLogger dml;
+void direct_memory_logger_init(void);
+void direct_memory_logger_periodic(void);
+void direct_memory_logger_set(uint8_t val);
+
+#endif /* DIRECT_MEMORY_LOGGER_H_ */
diff --git a/sw/airborne/peripherals/sst25vfxxxx.c b/sw/airborne/peripherals/sst25vfxxxx.c
new file mode 100644
index 0000000000..60b4d05e39
--- /dev/null
+++ b/sw/airborne/peripherals/sst25vfxxxx.c
@@ -0,0 +1,318 @@
+/*
+ * Copyright (C) 2014 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/sst25vfxxxx.c
+ * Driver for the SST25Vxxxx flash chips
+ */
+
+#include "sst25vfxxxx.h"
+
+/* Static function defines */
+
+/**
+ * Initializing the sst25vfxxxx chip
+ */
+void sst25vfxxxx_init(struct SST25VFxxxx* sst, struct spi_periph* spi_p, const uint8_t slave_idx, SPICallback spi_cb)
+{
+ /* Set spi_peripheral and start flash address */
+ sst->spi_p = spi_p;
+ sst->flash_addr = 0x0;
+
+ /* Set the spi transaction */
+ sst->spi_t.cpol = SPICpolIdleLow;
+ sst->spi_t.cpha = SPICphaEdge1;
+ sst->spi_t.dss = SPIDss8bit;
+ sst->spi_t.bitorder = SPIMSBFirst;
+ sst->spi_t.cdiv = SPIDiv32;
+
+ sst->spi_t.input_length = 0;
+ sst->spi_t.output_length = 0;
+ sst->spi_t.input_buf = sst->input_buf;
+ sst->spi_t.output_buf = sst->output_buf;
+ sst->spi_t.slave_idx = slave_idx;
+ sst->spi_t.select = SPISelectUnselect;
+ sst->spi_t.status = SPITransDone;
+ sst->spi_t.after_cb = spi_cb;
+
+ /* Update the status and start with enabling writing */
+ sst->status = SST25VFXXXX_IDLE;
+ sst25vfxxxx_block_write_en(sst);
+}
+
+/**
+ * Callback of the SPI after going one level higher for gathering the sst pointer
+ */
+void sst25vfxxxx_after_cb(struct SST25VFxxxx* sst)
+{
+ switch (sst->status) {
+ // Enabling writing to blocks
+ case SST25VFXXXX_WRITE_EN:
+ // When last transmit is done
+ if (sst->status_idx >= 1) {
+ sst->status = SST25VFXXXX_IDLE;
+ break;
+ }
+
+ // Write to the status register
+ sst->status_idx = 1;
+ sst->output_buf[0] = SST25VFXXXX_WRSR;
+ sst->output_buf[1] = 0x0;
+ sst->spi_t.output_length = 2;
+ sst->spi_t.input_length = 0;
+ spi_submit(sst->spi_p, &sst->spi_t);
+ break;
+
+ // Erase the full chip
+ case SST25VFXXXX_CHIP_ERASE:
+ switch (sst->status_idx) {
+ // Execute full erase command
+ case 0:
+ sst->status_idx = 1;
+ sst->output_buf[0] = SST25VFXXXX_ERASE_CHIP;
+ sst->spi_t.output_length = 1;
+ sst->spi_t.input_length = 0;
+ spi_submit(sst->spi_p, &sst->spi_t);
+ break;
+
+ // Wait for chip to finish
+ case 1:
+ // Disable writing when finished erasing
+ if (sst->spi_t.input_length == 2 && !(sst->input_buf[1] & 0x1)) {
+ sst->status_idx = 2;
+ sst->output_buf[0] = SST25VFXXXX_WRDI;
+ sst->spi_t.output_length = 1;
+ sst->spi_t.input_length = 0;
+ spi_submit(sst->spi_p, &sst->spi_t);
+ break;
+ }
+ sst->status_idx = 1;
+ sst->output_buf[0] = SST25VFXXXX_RDSR;
+ sst->spi_t.output_length = 1;
+ sst->spi_t.input_length = 2;
+ spi_submit(sst->spi_p, &sst->spi_t);
+ break;
+
+ // Goto idle
+ default:
+ sst->status = SST25VFXXXX_IDLE;
+ break;
+ }
+ break;
+
+ // Write bytes to flash
+ case SST25VFXXXX_WRITE_BYTES:
+ switch (sst->status_idx) {
+ // Send the address with 2 or 1 byte(s) of data
+ case 0:
+ sst->status_idx = 1;
+
+ if ((sst->transfer_length - sst->transfer_idx) > 1) {
+ sst->output_buf[0] = SST25VFXXXX_AAI_PROG;
+ } else {
+ sst->output_buf[0] = SST25VFXXXX_BYTE_PROG;
+ }
+ sst->output_buf[1] = (sst->flash_addr >> 16) & 0xFF;
+ sst->output_buf[2] = (sst->flash_addr >> 8) & 0xFF;
+ sst->output_buf[3] = sst->flash_addr & 0xFF;
+ sst->output_buf[4] = sst->transfer_buf[sst->transfer_idx++];
+
+ if ((sst->transfer_length - sst->transfer_idx) > 1) {
+ sst->output_buf[5] = sst->transfer_buf[sst->transfer_idx++];
+ sst->spi_t.output_length = 6;
+ } else {
+ sst->spi_t.output_length = 5;
+ }
+
+ sst->spi_t.input_length = 0;
+ spi_submit(sst->spi_p, &sst->spi_t);
+ break;
+
+ // Read the status register
+ case 1:
+ sst->status_idx = 2;
+ sst->output_buf[0] = SST25VFXXXX_RDSR;
+ sst->spi_t.output_length = 1;
+ sst->spi_t.input_length = 2;
+ spi_submit(sst->spi_p, &sst->spi_t);
+ break;
+
+ // Check the status register and send new bytes if possible
+ case 2:
+ // Still busy
+ if (sst->input_buf[1] & 0x1) {
+ sst->output_buf[0] = SST25VFXXXX_RDSR;
+ sst->spi_t.output_length = 1;
+ sst->spi_t.input_length = 2;
+ spi_submit(sst->spi_p, &sst->spi_t);
+ break;
+ }
+
+ // Write disabeling
+ if ((sst->transfer_length - sst->transfer_idx) <= 0) {
+ sst->status_idx = 3;
+ sst->output_buf[0] = SST25VFXXXX_WRDI;
+ sst->spi_t.output_length = 1;
+ sst->spi_t.input_length = 0;
+ spi_submit(sst->spi_p, &sst->spi_t);
+ break;
+ }
+
+ // Transfer new bytes
+ sst->status_idx = 1;
+ sst->output_buf[0] = SST25VFXXXX_AAI_PROG;
+ sst->output_buf[1] = sst->transfer_buf[sst->transfer_idx++];
+
+ if ((sst->transfer_length - sst->transfer_idx) <= 0) {
+ sst->output_buf[2] = sst->transfer_buf[sst->transfer_idx++]; //FIXME: uneven packets!!!!!
+ } else {
+ sst->output_buf[2] = 0x0;
+ }
+
+ sst->spi_t.output_length = 3;
+ sst->spi_t.input_length = 0;
+ spi_submit(sst->spi_p, &sst->spi_t);
+ break;
+
+ // Goto idle and update the flash address
+ default:
+ sst->flash_addr += sst->transfer_length;
+ sst->status = SST25VFXXXX_IDLE;
+ break;
+ }
+ break;
+
+ // Read bytes from flash memory
+ case SST25VFXXXX_READ_BYTES:
+ // Reset the buffer pointer and goto idle
+ sst->spi_t.input_buf = sst->input_buf;
+ sst->status = SST25VFXXXX_IDLE;
+ break;
+
+ // Default goto idle
+ default:
+ sst->status = SST25VFXXXX_IDLE;
+ break;
+ }
+}
+
+/**
+ * Read the chip identifier
+ */
+void sst25vfxxxx_read_id(struct SST25VFxxxx* sst)
+{
+ if (sst->status != SST25VFXXXX_IDLE) {
+ return;
+ }
+
+ // Write the read id command to the chip
+ sst->status = SST25VFXXXX_READ_ID;
+ sst->output_buf[0] = SST25VFXXXX_RDID;
+ sst->output_buf[1] = 0x0;
+ sst->output_buf[2] = 0x0;
+ sst->output_buf[3] = 0x0; //READ the MFG ID first
+ sst->spi_t.output_length = 4;
+ sst->spi_t.input_length = 8;
+ spi_submit(sst->spi_p, &sst->spi_t);
+}
+
+/**
+ * Enable block writing
+ */
+void sst25vfxxxx_block_write_en(struct SST25VFxxxx* sst)
+{
+ if (sst->status != SST25VFXXXX_IDLE) {
+ return;
+ }
+
+ // Enable writing to the status register
+ sst->status = SST25VFXXXX_WRITE_EN;
+ sst->status_idx = 0;
+ sst->output_buf[0] = SST25VFXXXX_EWSR;
+ sst->spi_t.output_length = 1;
+ sst->spi_t.input_length = 0;
+ spi_submit(sst->spi_p, &sst->spi_t);
+}
+
+/**
+ * Full chip erase
+ */
+void sst25vfxxxx_chip_erase(struct SST25VFxxxx* sst)
+{
+ if (sst->status != SST25VFXXXX_IDLE) {
+ return;
+ }
+
+ // Enable writing
+ sst->status = SST25VFXXXX_CHIP_ERASE;
+ sst->status_idx = 0;
+ sst->output_buf[0] = SST25VFXXXX_WREN;
+ sst->spi_t.output_length = 1;
+ sst->spi_t.input_length = 0;
+ spi_submit(sst->spi_p, &sst->spi_t);
+}
+
+/**
+ * Write bytes
+ */
+void sst25vfxxxx_write(struct SST25VFxxxx* sst, uint8_t* transfer_buffer, uint8_t transfer_length)
+{
+ if (sst->status != SST25VFXXXX_IDLE) {
+ return;
+ }
+
+ // Set the transfer buffer
+ sst->transfer_buf = transfer_buffer; // Not copied so keep buffer available!
+ sst->transfer_idx = 0;
+ sst->transfer_length = transfer_length;
+
+ // Enable writing
+ sst->status = SST25VFXXXX_WRITE_BYTES;
+ sst->status_idx = 0;
+ sst->output_buf[0] = SST25VFXXXX_WREN;
+ sst->spi_t.output_length = 1;
+ sst->spi_t.input_length = 0;
+ spi_submit(sst->spi_p, &sst->spi_t);
+}
+
+/**
+ * Read bytes
+ * Need 5 more extra bytes because of SPI overhead
+ */
+void sst25vfxxxx_read(struct SST25VFxxxx* sst, uint8_t* transfer_buffer, uint8_t transfer_length)
+{
+ if (sst->status != SST25VFXXXX_IDLE) {
+ return;
+ }
+
+ // Read all bytes at once
+ sst->status = SST25VFXXXX_READ_BYTES;
+ sst->status_idx = 0;
+ sst->output_buf[0] = SST25VFXXXX_HGIH_SPEAD_READ;
+ sst->output_buf[1] = (sst->flash_addr >> 16) & 0xFF;
+ sst->output_buf[2] = (sst->flash_addr >> 8) & 0xFF;
+ sst->output_buf[3] = sst->flash_addr & 0xFF;
+ sst->output_buf[4] = 0x0;
+ sst->spi_t.output_length = 5;
+ sst->spi_t.input_buf = transfer_buffer; // Need to reset this afterwards
+ sst->spi_t.input_length = transfer_length + 5;
+ spi_submit(sst->spi_p, &sst->spi_t);
+}
diff --git a/sw/airborne/peripherals/sst25vfxxxx.h b/sw/airborne/peripherals/sst25vfxxxx.h
new file mode 100644
index 0000000000..fe4938cfbd
--- /dev/null
+++ b/sw/airborne/peripherals/sst25vfxxxx.h
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2014 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/sst25vfxxxx.h
+ * Driver for the SST25Vxxxx flash chips
+ */
+
+#ifndef SST25VFXXXX_H
+#define SST25VFXXXX_H
+
+#include "mcu_periph/spi.h"
+
+/* Register defines */
+#define SST25VFXXXX_READ 0x03
+#define SST25VFXXXX_HGIH_SPEAD_READ 0x0B
+#define SST25VFXXXX_ERASE_4K 0x20
+#define SST25VFXXXX_ERASE_32K 0x52
+#define SST25VFXXXX_ERASE_64K 0xD8
+#define SST25VFXXXX_ERASE_CHIP 0x60
+#define SST25VFXXXX_BYTE_PROG 0x02
+#define SST25VFXXXX_AAI_PROG 0xAD
+#define SST25VFXXXX_RDSR 0x05
+#define SST25VFXXXX_EWSR 0x50
+#define SST25VFXXXX_WRSR 0x01
+#define SST25VFXXXX_WREN 0x06
+#define SST25VFXXXX_WRDI 0x04
+#define SST25VFXXXX_RDID 0x90
+#define SST25VFXXXX_JEDEC_ID 0x9F
+#define SST25VFXXXX_EBSY 0x70
+#define SST25VFXXXX_DBSY 0x80
+
+/* The different statuses the SST25VFxxxx chip can be in */
+enum SST25VFxxxxStatus {
+ SST25VFXXXX_UNINIT, /**< The chip isn't initialized */
+ SST25VFXXXX_IDLE, /**< The chip is idle and can be used */
+ SST25VFXXXX_READ_ID, /**< The chip is busy with getting the chip ID */
+ SST25VFXXXX_WRITE_EN, /**< The chip is busy enabeling writing to blocks */
+ SST25VFXXXX_CHIP_ERASE, /**< The chip is busy erasing itself */
+ SST25VFXXXX_WRITE_BYTES, /**< The chip is busy writing bytes */
+ SST25VFXXXX_READ_BYTES, /**< The chip is busy reading bytes */
+};
+
+/* The structure for the SST25VFxxxx chip that handles all the buffers and requests */
+struct SST25VFxxxx {
+ volatile enum SST25VFxxxxStatus status; /**< The status of the SST25VFxxxx flash chip */
+ uint8_t status_idx; /**< The counter of substatuses */
+ struct spi_periph* spi_p; /**< The SPI peripheral for the connection */
+ struct spi_transaction spi_t; /**< The SPI transaction used for the writing and reading of registers */
+ uint8_t input_buf[16]; /**< The input buffer for the SPI transaction */
+ uint8_t output_buf[16]; /**< The output buffer for the SPI transaction */
+ uint32_t flash_addr; /**< The flash address to write at */
+
+ uint8_t* transfer_buf; /**< The transfer buffer */
+ uint8_t transfer_idx; /**< The transfer idx is used for counting input/output bytes */
+ uint8_t transfer_length; /**< The transfer buffer length */
+};
+
+void sst25vfxxxx_init(struct SST25VFxxxx* sst, struct spi_periph* spi_p, const uint8_t slave_idx, SPICallback spi_cb);
+void sst25vfxxxx_after_cb(struct SST25VFxxxx* sst);
+void sst25vfxxxx_read_id(struct SST25VFxxxx* sst);
+void sst25vfxxxx_block_write_en(struct SST25VFxxxx* sst);
+void sst25vfxxxx_chip_erase(struct SST25VFxxxx* sst);
+void sst25vfxxxx_write(struct SST25VFxxxx* sst, uint8_t* transfer_buffer, uint8_t transfer_length);
+void sst25vfxxxx_read(struct SST25VFxxxx* sst, uint8_t* transfer_buffer, uint8_t transfer_length);
+
+#endif /* SST25VFXXXX_H */