NRF updates, better servo output support

Makefile

@@ -108,6 +108,7 @@ CSRC = $(PORTSRC) \
        servo_dec.c \
        utils.c \
        servo.c \
+       servo_simple.c \
        packet.c \
        terminal.c \
        conf_general.c \

applications/app_ppm.c

@@ -34,6 +34,9 @@
 #include "comm_can.h"
 #include <math.h>
 
+// Only available if servo output is not active
+#if !SERVO_OUT_ENABLE
+
 // Settings
 #define MAX_CAN_AGE						0.1
 #define MIN_PULSES_WITHOUT_POWER		50
@@ -54,24 +57,32 @@ static volatile int pulses_without_power = 0;
 
 // Private functions
 static void update(void *p);
+#endif
 
 void app_ppm_configure(ppm_config *conf) {
+#if !SERVO_OUT_ENABLE
 	config = *conf;
 	pulses_without_power = 0;
 
 	if (is_running) {
 		servodec_set_pulse_options(config.pulse_start, config.pulse_end, config.median_filter);
 	}
+#else
+	(void)conf;
+#endif
 }
 
 void app_ppm_start(void) {
+#if !SERVO_OUT_ENABLE
 	chThdCreateStatic(ppm_thread_wa, sizeof(ppm_thread_wa), NORMALPRIO, ppm_thread, NULL);
 
 	chSysLock();
 	chVTSetI(&vt, MS2ST(1), update, NULL);
 	chSysUnlock();
+#endif
 }
 
+#if !SERVO_OUT_ENABLE
 static void servodec_func(void) {
 	chSysLockFromIsr();
 	timeout_reset();
@@ -308,3 +319,4 @@ static msg_t ppm_thread(void *arg) {
 
 	return 0;
 }
+#endif

buffer.c

@@ -24,6 +24,16 @@
 
 #include "buffer.h"
 
+void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index) {
+	buffer[(*index)++] = number >> 8;
+	buffer[(*index)++] = number;
+}
+
+void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index) {
+	buffer[(*index)++] = number >> 8;
+	buffer[(*index)++] = number;
+}
+
 void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index) {
 	buffer[(*index)++] = number >> 24;
 	buffer[(*index)++] = number >> 16;
@@ -38,14 +48,12 @@ void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index) {
 	buffer[(*index)++] = number;
 }
 
-void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index) {
-	buffer[(*index)++] = number >> 8;
-	buffer[(*index)++] = number;
+void buffer_append_float16(uint8_t* buffer, float number, float scale, int32_t *index) {
+    buffer_append_int16(buffer, (int16_t)(number * scale), index);
 }
 
-void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index) {
-	buffer[(*index)++] = number >> 8;
-	buffer[(*index)++] = number;
+void buffer_append_float32(uint8_t* buffer, float number, float scale, int32_t *index) {
+    buffer_append_int32(buffer, (int32_t)(number * scale), index);
 }
 
 int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index) {
@@ -79,3 +87,11 @@ uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index) {
 	*index += 4;
 	return res;
 }
+
+float buffer_get_float16(const uint8_t *buffer, float scale, int32_t *index) {
+    return (float)buffer_get_int16(buffer, index) / scale;
+}
+
+float buffer_get_float32(const uint8_t *buffer, float scale, int32_t *index) {
+    return (float)buffer_get_int32(buffer, index) / scale;
+}

buffer.h

@@ -27,13 +27,17 @@
 
 #include <stdint.h>
 
-void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index);
-void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index);
 void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index);
 void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index);
+void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index);
+void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index);
+void buffer_append_float16(uint8_t* buffer, float number, float scale, int32_t *index);
+void buffer_append_float32(uint8_t* buffer, float number, float scale, int32_t *index);
 int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index);
 uint16_t buffer_get_uint16(const uint8_t *buffer, int32_t *index);
 int32_t buffer_get_int32(const uint8_t *buffer, int32_t *index);
 uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index);
+float buffer_get_float16(const uint8_t *buffer, float scale, int32_t *index);
+float buffer_get_float32(const uint8_t *buffer, float scale, int32_t *index);
 
 #endif /* BUFFER_H_ */

commands.c

@@ -28,6 +28,7 @@
 #include "main.h"
 #include "stm32f4xx_conf.h"
 #include "servo.h"
+#include "servo_simple.h"
 #include "buffer.h"
 #include "myUSB.h"
 #include "terminal.h"
@@ -215,8 +216,15 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		timeout_reset();
 		break;
 
-	case COMM_SET_SERVO_OFFSET:
-		servos[0].offset = data[0];
+	case COMM_SET_SERVO_POS:
+#if SERVO_OUT_ENABLE
+		ind = 0;
+#if SERVO_OUT_SIMPLE
+		servo_simple_set_output(buffer_get_float16(data, 1000.0, &ind));
+#else
+		servos[0].pos = (int16_t)(buffer_get_float16(data, 1000.0, &ind) * 255.0);
+#endif
+#endif
 		break;
 
 	case COMM_SET_MCCONF:

conf_general.h

@@ -27,7 +27,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR	1
-#define FW_VERSION_MINOR	8
+#define FW_VERSION_MINOR	9
 
 #include "datatypes.h"
 
@@ -87,7 +87,7 @@
 /*
  * Settings for the external LEDs (hardcoded for now)
  */
-#define LED_EXT_BATT_LOW		25.6
+#define LED_EXT_BATT_LOW		28.0
 #define LED_EXT_BATT_HIGH		33.0
 
 /*
@@ -99,6 +99,15 @@
 #define WS2811_LED_NUM			14
 #define WS2811_USE_CH2			1		// 0: CH1 (PB6) 1: CH2 (PB7)
 
+/*
+ * Servo output driver
+ */
+#define SERVO_OUT_ENABLE		0		// Enable servo output
+#define SERVO_OUT_SIMPLE		1		// Use simple HW-based driver (recommended)
+#define SERVO_OUT_PULSE_MIN_US	1000	// Minimum pulse length in microseconds
+#define SERVO_OUT_PULSE_MAX_US	2000	// Maximum pulse length in microseconds
+#define SERVO_OUT_RATE_HZ		50		// Update rate in Hz
+
 // Functions
 void conf_general_init(void);
 void conf_general_read_app_configuration(app_configuration *conf);

datatypes.h

@@ -274,7 +274,7 @@ typedef enum {
 	COMM_SET_RPM,
 	COMM_SET_POS,
 	COMM_SET_DETECT,
-	COMM_SET_SERVO_OFFSET,
+	COMM_SET_SERVO_POS,
 	COMM_SET_MCCONF,
 	COMM_GET_MCCONF,
 	COMM_SET_APPCONF,
@@ -363,7 +363,8 @@ typedef struct {
 
 typedef enum {
 	MOTE_PACKET_BATT_LEVEL = 0,
-	MOTE_PACKET_BUTTONS
+	MOTE_PACKET_BUTTONS,
+	MOTE_PACKET_ALIVE
 } MOTE_PACKET;
 
 #endif /* DATATYPES_H_ */

irq_handlers.c

@@ -24,12 +24,14 @@
 #include "servo.h"
 #include "hw.h"
 
+#if SERVO_OUT_ENABLE && !SERVO_OUT_SIMPLE
 CH_IRQ_HANDLER(TIM7_IRQHandler) {
 	CH_IRQ_PROLOGUE();
 	TIM_ClearITPendingBit(TIM7, TIM_IT_Update);
 	servo_irq();
 	CH_IRQ_EPILOGUE();
 }
+#endif
 
 CH_IRQ_HANDLER(ADC1_2_3_IRQHandler) {
 	CH_IRQ_PROLOGUE();

main.c

@@ -39,6 +39,8 @@
 #include "ws2811.h"
 #include "led_external.h"
 #include "encoder.h"
+#include "servo.h"
+#include "servo_simple.h"
 
 /*
  * Timers used:
@@ -47,7 +49,7 @@
  * TIM2: mcpwm
  * TIM12: mcpwm
  * TIM8: mcpwm
- * TIM3: servo_dec/Encoder (HW_R2)
+ * TIM3: servo_dec/Encoder (HW_R2)/servo_simple
  * TIM4: WS2811/WS2812 LEDs/Encoder (other HW)
  *
  * DMA/stream	Device		Function
@@ -322,6 +324,14 @@ int main(void) {
 	encoder_init();
 #endif
 
+#if SERVO_OUT_ENABLE
+#if SERVO_OUT_SIMPLE
+	servo_simple_init();
+#else
+	servo_init();
+#endif
+#endif
+
 	// Threads
 	chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
 	chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);

nrf/nrf_driver.c

@@ -58,9 +58,12 @@ static msg_t tx_thread(void *arg) {
 	chRegSetThreadName("Nrf TX");
 
 	for(;;) {
-		// TODO! Send status
+		uint8_t pl[6];
+		int32_t index = 0;
+		pl[index++] = MOTE_PACKET_ALIVE;
+		rfhelp_send_data_crc((char*)pl, index);
 
-		chThdSleepMilliseconds(500);
+		chThdSleepMilliseconds(50);
 	}
 
 	return 0;
@@ -79,7 +82,7 @@ static msg_t rx_thread(void *arg) {
 		int pipe;
 
 		for(;;) {
-			int res = rfhelp_read_rx_data((char*)buf, &len, &pipe);
+			int res = rfhelp_read_rx_data_crc((char*)buf, &len, &pipe);
 			chuck_data cdata;
 			int32_t index = 0;
 			int buttons;

nrf/rf.c

@@ -23,7 +23,7 @@
 void rf_init(void) {
 	spi_sw_init();
 
-	rf_power_up();
+	rf_power_down();
 
 	// Set default register values (TODO for the rest)
 	rf_write_reg_byte(NRF_REG_EN_RXADDR, 0);
@@ -44,6 +44,7 @@ void rf_init(void) {
 
 	// Note: The address should be set by the application.
 
+	rf_power_up();
 	rf_mode_rx();
 	rf_flush_all();
 	rf_clear_irq();

nrf/rfhelp.c

@@ -19,6 +19,7 @@
 #include "rf.h"
 #include "ch.h"
 #include "hal.h"
+#include "crc.h"
 #include <string.h>
 #include <stdbool.h>
 
@@ -129,6 +130,33 @@ int rfhelp_send_data(char *data, int len) {
 	return retval;
 }
 
+/**
+ * Same as rfhelp_send_data, but will add a crc checksum to the end. This is
+ * useful for protecting against corruption between the NRF and the MCU in case
+ * there are errors on the SPI bus.
+ *
+ * @param data
+ * The data to be sent.
+ *
+ * @param len
+ * Length of the data. Should be no more than 30 bytes.
+ *
+ * @return
+ * 0: Send OK.
+ * -1: Max RT.
+ * -2: Timeout
+ */
+int rfhelp_send_data_crc(char *data, int len) {
+	char buffer[len + 2];
+	unsigned short crc = crc16((unsigned char*)data, len);
+
+	memcpy(buffer, data, len);
+	buffer[len] = (char)(crc >> 8);
+	buffer[len + 1] = (char)(crc & 0xFF);
+
+	return rfhelp_send_data(buffer, len + 2);
+}
+
 /**
  * Read data from the RX fifo
  *
@@ -182,6 +210,42 @@ int rfhelp_read_rx_data(char *data, int *len, int *pipe) {
 	return retval;
 }
 
+/**
+ * Same as rfhelp_read_rx_data, but will check if there is a valid CRC in the
+ * end of the payload.
+ *
+ * @param data
+ * Pointer to the array in which to store the data.
+ *
+ * @param len
+ * Pointer to variable storing the data length.
+ *
+ * @param pipe
+ * Pointer to the pipe on which the data was received. Can be 0.
+ *
+ * @return
+ * 1: Read OK, more data to read.
+ * 0: Read OK
+ * -1: No RX data
+ * -2: Wrong length read. Something is likely wrong.
+ * -3: Data read, but CRC does not match.
+ */
+int rfhelp_read_rx_data_crc(char *data, int *len, int *pipe) {
+	int res = rfhelp_read_rx_data(data, len, pipe);
+
+	if (res >= 0 && *len > 2) {
+		unsigned short crc = crc16((unsigned char*)data, *len - 2);
+
+		if (crc	!= ((unsigned short) data[*len - 2] << 8 | (unsigned short) data[*len - 1])) {
+			res = -3;
+		}
+	}
+
+	*len -= 2;
+
+	return res;
+}
+
 int rfhelp_rf_status(void) {
 	chMtxLock(&rf_mutex);
 	int s = rf_status();
@@ -212,3 +276,15 @@ void rfhelp_set_rx_addr(int pipe, const char *addr, int addr_len) {
 	rf_set_rx_addr(pipe, addr, address_length);
 	chMtxUnlock();
 }
+
+void rfhelp_power_down(void) {
+	chMtxLock(&rf_mutex);
+	rf_power_down();
+	chMtxUnlock();
+}
+
+void rfhelp_power_up(void) {
+	chMtxLock(&rf_mutex);
+	rf_power_up();
+	chMtxUnlock();
+}