[superbitrf] Downlink is now functional

Still needs some extensive testing to make it more relaible.

conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml

@@ -12,7 +12,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_m_2.0">
-      <subsystem name="radio_control" type="superbitrf">
+      <subsystem name="radio_control" type="superbitrf_rc">
       </subsystem>
       <!-- MPU6000 is configured to output data at 2kHz, but polled at 512Hz PERIODIC_FREQUENCY -->
     </target>
@@ -28,7 +28,7 @@
       <define name="USE_SERVOS_7AND8"/>
     </subsystem>
 
-    <subsystem name="telemetry"     type="transparent"/>
+    <subsystem name="telemetry"     type="superbitrf"/>
     <subsystem name="imu"           type="aspirin_v2.1"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="stabilization" type="int_quat"/>
@@ -39,10 +39,10 @@
   </firmware>
 
   <servos driver="Pwm">
-    <servo name="FRONT"   no="0" min="1000" neutral="1100" max="1900"/>
-    <servo name="BACK"    no="1" min="1000" neutral="1100" max="1900"/>
-    <servo name="RIGHT"   no="2" min="1000" neutral="1100" max="1900"/>
-    <servo name="LEFT"    no="3" min="1000" neutral="1100" max="1900"/>
+    <servo name="FRONT" no="0" min="1000" neutral="1080" max="1900"/>
+    <servo name="BACK"  no="1" min="1000" neutral="1080" max="1900"/>
+    <servo name="RIGHT" no="2" min="1000" neutral="1080" max="1900"/>
+    <servo name="LEFT"  no="3" min="1000" neutral="1080" max="1900"/>
   </servos>
 
   <commands>
@@ -59,9 +59,9 @@
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
     <!-- front/back turning CW, right/left CCW -->
-    <define name="ROLL_COEF"   value="{    0,    0, -256,  256 }"/>
-    <define name="PITCH_COEF"  value="{  256, -256,    0,    0 }"/>
-    <define name="YAW_COEF"    value="{ -256, -256,  256,  256 }"/>
+    <define name="ROLL_COEF"   value="{ -256, -256,  256,  256 }"/>
+    <define name="PITCH_COEF"  value="{  256, -256, -256,  256 }"/>
+    <define name="YAW_COEF"    value="{ -256,  256, -256,  256 }"/>
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
   </section>
 
@@ -88,7 +88,7 @@
 
     <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
     <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
-    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-45." unit="deg"/>
   </section>
 
   <section name="AHRS" prefix="AHRS_">
@@ -153,22 +153,22 @@
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
-    <define name="PHI_PGAIN"  value="1000"/>
-    <define name="PHI_DGAIN"  value="400"/>
-    <define name="PHI_IGAIN"  value="200"/>
+    <define name="PHI_PGAIN"  value="100"/>
+    <define name="PHI_DGAIN"  value="160"/>
+    <define name="PHI_IGAIN"  value="70"/>
 
-    <define name="THETA_PGAIN"  value="1000"/>
-    <define name="THETA_DGAIN"  value="400"/>
-    <define name="THETA_IGAIN"  value="200"/>
+    <define name="THETA_PGAIN"  value="100"/>
+    <define name="THETA_DGAIN"  value="160"/>
+    <define name="THETA_IGAIN"  value="170"/>
 
-    <define name="PSI_PGAIN"  value="500"/>
-    <define name="PSI_DGAIN"  value="300"/>
-    <define name="PSI_IGAIN"  value="10"/>
+    <define name="PSI_PGAIN"  value="300"/>
+    <define name="PSI_DGAIN"  value="150"/>
+    <define name="PSI_IGAIN"  value="2"/>
 
     <!-- feedforward -->
-    <define name="PHI_DDGAIN"   value="300"/>
-    <define name="THETA_DDGAIN" value="300"/>
-    <define name="PSI_DDGAIN"   value="300"/>
+    <define name="PHI_DDGAIN"   value="100"/>
+    <define name="THETA_DDGAIN" value="100"/>
+    <define name="PSI_DDGAIN"   value="100"/>
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">

conf/firmwares/subsystems/shared/radio_control_superbitrf.makefile → conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile

@@ -2,13 +2,14 @@
 # Makefile for shared radio_control superbitrf subsystem
 #
 
-ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE_SUPERBITRF -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/superbitrf.h\"
-ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1
+ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE_SUPERBITRF -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/superbitrf_rc.h\"
+ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI1 -DUSE_SPI_SLAVE1
 
 ifneq ($(RADIO_CONTROL_LED),none)
 ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
 endif
 
 ap.srcs += peripherals/cyrf6936.c \
+		   $(SRC_SUBSYSTEMS)/datalink/superbitrf.c\
            $(SRC_SUBSYSTEMS)/radio_control.c \
-           $(SRC_SUBSYSTEMS)/radio_control/superbitrf.c 
+           $(SRC_SUBSYSTEMS)/radio_control/superbitrf_rc.c 

conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile

@@ -0,0 +1,11 @@
+#
+# The superbitRF module as telemetry downlink/uplink
+#
+#
+
+ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=SuperbitRF
+ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF
+ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI1 -DUSE_SPI_SLAVE1
+ap.srcs += peripherals/cyrf6936.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/superbitrf.c subsystems/datalink/pprz_transport.c
+ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c

conf/messages.xml

@@ -605,8 +605,10 @@
 
   <message name="SUPERBITRF" id="72">
       <field name="status" type="uint8" values="UNINIT|INIT_BINDING|INIT_TRANSFER|BINDING|SYNCING_A|SYNCING_B|TRANSFER"/>
-      <field name="cyrf_status" type="uint8" values="UNINIT|IDLE|GET_MFG_ID|MULTIWRITE|DATA_CODE|CHAN_SOP_DATA_CRC|RX_IRQ_STATUS_PACKET"/>
+      <field name="cyrf_status" type="uint8" values="UNINIT|IDLE|GET_MFG_ID|MULTIWRITE|DATA_CODE|CHAN_SOP_DATA_CRC|RX_IRQ_STATUS_PACKET|SEND"/>
       <field name="packet_count" type="uint32"/>
+      <field name="timing1" type="uint32"/>
+      <field name="timing2" type="uint32"/>
       <field name="bind_mfg_id" type="uint32"/>
       <field name="mfg_id" type="uint8[]"/>
   </message>

sw/airborne/firmwares/rotorcraft/telemetry.h

@@ -134,14 +134,16 @@
 #define PERIODIC_SEND_PPM(_trans, _dev) {}
 #endif
 
-#ifdef RADIO_CONTROL_TYPE_SUPERBITRF
-#include "subsystems/radio_control/superbitrf.h"
+#ifdef USE_SUPERBITRF
+#include "subsystems/datalink/superbitrf.h"
 #define PERIODIC_SEND_SUPERBITRF(_trans, _dev) {        \
     DOWNLINK_SEND_SUPERBITRF(_trans, _dev,              \
                       &superbitrf.status,               \
                       &superbitrf.cyrf6936.status,      \
                       &superbitrf.packet_count,         \
-                      &superbitrf.packet_count,          \
+                      &superbitrf.timing1,              \
+                      &superbitrf.timing2,              \
+                      &superbitrf.bind_mfg_id32,        \
                       6,                                \
                       superbitrf.cyrf6936.mfg_id);}
 #else

sw/airborne/peripherals/cyrf6936.c

@@ -48,7 +48,7 @@ void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_
   /* Set spi_peripheral and the status */
   cyrf->spi_p = spi_p;
   cyrf->status = CYRF6936_UNINIT;
-  cyrf->has_packet = FALSE;
+  cyrf->has_irq = FALSE;
 
   /* Set the spi transaction */
   cyrf->spi_t.cpol = SPICpolIdleLow;
@@ -191,11 +191,15 @@ void cyrf6936_event(struct Cyrf6936 *cyrf) {
     case 0: // Read the receive IRQ status
       cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS);
       break;
-    case 1: // Read the receive status
+    case 1: // Read the send IRQ status
       cyrf->rx_irq_status = cyrf->input_buf[1];
+      cyrf6936_read_register(cyrf, CYRF_TX_IRQ_STATUS);
+      break;
+    case 2: // Read the receive status
+      cyrf->tx_irq_status = cyrf->input_buf[1];
       cyrf6936_read_register(cyrf, CYRF_RX_STATUS);
       break;
-    case 2: // Read the receive packet
+    case 3: // Read the receive packet
       cyrf->rx_status = cyrf->input_buf[1];
       cyrf6936_read_block(cyrf, CYRF_RX_BUFFER, 16);
       break;
@@ -204,7 +208,35 @@ void cyrf6936_event(struct Cyrf6936 *cyrf) {
       for(i = 0; i < 16; i++)
         cyrf->rx_packet[i] = cyrf->input_buf[i+1];
 
-      cyrf->has_packet = TRUE;
+      cyrf->has_irq = TRUE;
+      cyrf->status = CYRF6936_IDLE;
+      break;
+    }
+    break;
+
+  /* The CYRF6936 is busy sending a packet */
+  case CYRF6936_SEND:
+    // When the last transaction isn't failed send the next
+    if(cyrf->spi_t.status != SPITransFailed)
+      cyrf->buffer_idx++;
+
+    cyrf->spi_t.status = SPITransDone;
+
+    // Switch for the different states
+    switch (cyrf->buffer_idx) {
+    case 0: // Set the packet length
+      cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]);
+      break;
+    case 1: // Clear the TX buffer
+      cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_CLR);
+      break;
+    case 2: // Write the send packet
+      cyrf6936_write_block(cyrf, CYRF_TX_BUFFER, &cyrf->buffer[1], 16);
+      break;
+    case 3: // Send the packet
+      cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_GO | CYRF_TXC_IRQEN | CYRF_TXE_IRQEN);
+      break;
+    default:
       cyrf->status = CYRF6936_IDLE;
       break;
     }
@@ -358,3 +390,28 @@ bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf) {
 
   return TRUE;
 }
+
+/**
+ * Send a packet with a certain length
+ */
+bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length) {
+  int i;
+
+  /* Check if the cyrf6936 isn't busy */
+  if(cyrf->status != CYRF6936_IDLE)
+    return FALSE;
+
+  // Set the status
+  cyrf->status = CYRF6936_SEND;
+
+  // Copy the length and the data
+  cyrf->buffer[0] = length;
+  for(i = 0; i < length; i++)
+    cyrf->buffer[i+1] = data[i];
+
+  /* Try to set the packet length */
+  cyrf->buffer_idx = 0;
+  cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]);
+
+  return TRUE;
+}

sw/airborne/peripherals/cyrf6936.h

@@ -20,7 +20,7 @@
  */
 
 /**
- * @file peripherals/yrf6936c.h
+ * @file peripherals/cyrf6936.h
  * Driver for the cyrf6936 2.4GHz radio chip
  */
 
@@ -40,7 +40,8 @@ enum Cyrf6936Status {
   CYRF6936_MULTIWRITE,              /**< The chip is writing multiple registers */
   CYRF6936_DATA_CODE,               /**< The chip is writing a data code */
   CYRF6936_CHAN_SOP_DATA_CRC,       /**< The chip is setting the channel, SOP code, DATA code and the CRC seed */
-  CYRF6936_RX_IRQ_STATUS_PACKET     /**< The chip is getting the receive irq status, receive status and the receive packet */
+  CYRF6936_RX_IRQ_STATUS_PACKET,    /**< The chip is getting the receive irq status, receive status and the receive packet */
+  CYRF6936_SEND                     /**< The chip is busy sending a packet */
 };
 
 /* The structure for the cyrf6936 chip that handles all the buffers and requests */
@@ -55,8 +56,9 @@ struct Cyrf6936 {
   uint8_t buffer_length;                    /**< The length of the buffer used for MULTIWRITE */
   uint8_t buffer_idx;                       /**< The index of the buffer used for MULTIWRITE and used as sub-status for other statuses */
 
-  bool_t has_packet;                        /**< When the CYRF6936 is done reading the receive registers */
+  bool_t has_irq;                           /**< When the CYRF6936 is done reading the irq */
   uint8_t mfg_id[6];                        /**< The manufacturer id of the CYRF6936 chip */
+  uint8_t tx_irq_status;                    /**< The last send interrupt status */
   uint8_t rx_irq_status;                    /**< The last receive interrupt status */
   uint8_t rx_status;                        /**< The last receive status */
   uint8_t rx_packet[16];                    /**< The last received packet */
@@ -69,5 +71,6 @@ bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t d
 bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length);
 bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed);
 bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf);
+bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length);
 
 #endif /* CYRF6936_H */

sw/airborne/subsystems/datalink/datalink.h

@@ -46,6 +46,7 @@
 #define PPRZ 1
 #define XBEE 2
 #define UDP 3
+#define SUPERBITRF 4
 
 EXTERN bool_t dl_msg_available;
 /** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/
@@ -90,7 +91,7 @@ EXTERN void dl_parse_msg(void);
 #elif defined DATALINK && DATALINK == UDP
 
 #define DatalinkEvent() {                       \
-    UdpCheckAndParse();                        \
+    UdpCheckAndParse();                         \
     DlCheckAndParse();                          \
   }
 

sw/airborne/subsystems/datalink/downlink.h

@@ -50,6 +50,7 @@
 #include "subsystems/datalink/pprz_transport.h"
 #include "subsystems/datalink/xbee.h"
 #include "subsystems/datalink/w5100.h"
+#include "subsystems/datalink/superbitrf.h"
 #if USE_AUDIO_TELEMETRY
 #include "subsystems/datalink/audio_telemetry.h"
 #endif