Added more logging, integrated xbee receive, verified xbee receive wo…

…rking properly

include/input_manager.h

@@ -41,6 +41,7 @@ typedef struct user_input_t
     int initialized;                 ///< set to 1 after input_manager_init(void)
     flight_mode_t flight_mode;       ///< this is the user commanded flight_mode.
     int input_active;                ///< nonzero indicates some user control is coming in
+    int kill_switch;                 ///< for printing
     arm_state_t requested_arm_mode;  ///< set to ARMED after arming sequence is entered.
 
     // All sticks scaled from -1 to 1

include/log_manager.h

@@ -1,9 +1,11 @@
 /**
  * <log_manager.h>
  *
- * @brief      Functions to start, stop, and interact with the log manager
- *             thread.
+ * @brief   Functions to start, stop, and interact with the log manager
+ * thread.
  *
+ * @addtogroup LogManager
+ * @{
  */
 
 #ifndef LOG_MANAGER_H
@@ -20,8 +22,9 @@ typedef struct log_entry_t
 {
     /** @name index, always printed */
     ///@{
-    uint64_t loop_index;  // timing
+    uint64_t loop_index;
     uint64_t last_step_ns;
+    uint64_t imu_time_ns;  ///< Time that ISR occurs
     ///@}
 
     /** @name sensors */
@@ -49,6 +52,29 @@ typedef struct log_entry_t
     double Zdot;
     ///@}
 
+    /*** @name xbee data */
+    ///@{
+    uint32_t xbee_time;
+    uint64_t xbee_time_received_ns;
+    float xbee_x;
+    float xbee_y;
+    float xbee_z;
+    float xbee_qw;
+    float xbee_qx;
+    float xbee_qy;
+    float xbee_qz;
+    ///@}
+
+    /** @name throttles */
+    ///@{
+    double X_throttle;
+    double Y_throttle;
+    double Z_throttle;
+    double roll_throttle;
+    double pitch_throttle;
+    double yaw_throttle;
+    ///@}
+
     /** @name setpoint */
     ///@{
     double sp_roll;
@@ -84,32 +110,43 @@ typedef struct log_entry_t
     double mot_8;
     ///@}
 
+    /** @name dsm connection valid */
+    ///@{
+    int dsm_con;
+    ///@}
+
+    /** @name flight mode */
+    ///@{
+    int flight_mode;
+    ///@}
 } log_entry_t;
 
 /**
- * @brief      creates a new csv log file and starts the background thread.
+ * @brief   creates a new csv log file and starts the background thread.
  *
- * @return     0 on success, -1 on failure
+ * @return  0 on success, -1 on failure
  */
 int log_manager_init(void);
 
 /**
- * @brief      quickly add new data to local buffer
+ * @brief   quickly add new data to local buffer
  *
  * This is called after feedback_march after signals have been sent to
  * the motors.
  *
- * @return     0 on success, -1 on failure
+ * @return  0 on success, -1 on failure
  */
 int log_manager_add_new();
 
 /**
- * @brief      Finish writing remaining data to log and close thread.
+ * @brief   Finish writing remaining data to log and close thread.
  *
- *             Used in log_manager.c
+ * Used in log_manager.c
  *
- * @return     0 on sucess and clean exit, -1 on exit timeout/force close.
+ * @return  0 on sucess and clean exit, -1 on exit timeout/force close.
  */
 int log_manager_cleanup(void);
 
 #endif  // LOG_MANAGER_H
+
+/* @} end group LogManager */

include/serial_com.h

@@ -0,0 +1,73 @@
+/**
+ * <serial_com.h>
+ *
+ * @brief       Simplified serial utilities
+ *
+ * @author [email protected] (2004)
+ *
+ * @addtogroup SerialCom
+ * @{
+ */
+
+#ifndef SERIAL_COM_H
+#define SERIAL_COM_H
+
+// clang-format off
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @brief   Creates a basic fd, setting baud to 9600, raw data i/o
+ * (no flow control, no fancy character handling. Configures
+ * it for blocking reads.  8 data bits, 1 stop bit, no parity.
+ *
+ * @return      the fd or -1 on error
+ */
+int serial_open(const char *port, int baud, int blocking);
+
+/** @brief   Set the baud rate, where the baudrate is just the integer value
+ *  desired.
+ *
+ * @return      non-zero on error.
+ */
+int serial_setbaud(int fd, int baudrate);
+
+/**
+ * @brief   Enable cts/rts flow control.
+ *
+ * @return      non-zero on error.
+ */
+int serial_enablectsrts(int fd);
+
+/**
+ * @brief   Enable xon/xoff flow control.
+ *
+ * @returns non-zero on error.
+ */
+int serial_enablexon(int fd);
+
+/**
+ * @brief   Set the port to 8 data bits, 2 stop bits, no parity.
+ *
+ * @return non-zero on error.
+ */
+int serial_set_N82(int fd);
+
+/**
+ * @brief   Closes the serial interface
+ *
+ * @return non-zero on error
+ */
+int serial_close(int fd);
+#ifdef __cplusplus
+}
+#endif
+
+// clang-format on
+
+#endif /*__SERIAL_COM__ */
+
+/* @} end group SerialCom */

include/settings.h

@@ -74,6 +74,7 @@ typedef struct settings_t
     int printf_sticks;
     int printf_setpoint;
     int printf_u;
+    int printf_xbee;
     int printf_motors;
     int printf_mode;
     ///@}
@@ -86,6 +87,10 @@ typedef struct settings_t
     int log_setpoint;
     int log_control_u;
     int log_motor_signals;
+    int log_throttles;
+    int log_xbee;
+    int log_dsm;
+    int log_flight_mode;
     ///@}
 
     /** @name mavlink stuff */

include/state_estimator.h

@@ -31,6 +31,7 @@
 typedef struct state_estimate_t
 {
     int initialized;
+    uint64_t imu_time_ns;  ///< Time when ISR occurs (for logging purposes)
 
     /** @name IMU (accel gyro)
      * Normalized Quaternion is straight from the DMP but converted to NED
@@ -98,10 +99,11 @@ typedef struct state_estimate_t
     ///@{
     int mocap_running;            ///< 1 if motion capture data is recent and valid
     uint64_t mocap_timestamp_ns;  ///< timestamp of last received packet in nanoseconds since boot
-    double pos_mocap[3];          ///< position in mocap frame, converted to NED if necessary
-    double quat_mocap[4];         ///< UAV orientation according to mocap
-    double tb_mocap[3];           ///< Tait-Bryan angles according to mocap
-    int is_active;                ///< TODO used by mavlink manager, purpose unclear... (pg)
+    uint64_t xbee_time_received_ns;  ///< timestamp of xbee message received
+    double pos_mocap[3];             ///< position in mocap frame, converted to NED if necessary
+    double quat_mocap[4];            ///< UAV orientation according to mocap
+    double tb_mocap[3];              ///< Tait-Bryan angles according to mocap
+    int is_active;                   ///< TODO used by mavlink manager, purpose unclear... (pg)
     ///@}
 
     /** @name Global Position Estimate

include/xbee_receive.h

@@ -0,0 +1,63 @@
+/**
+ * <xbee_receive.h>
+ *
+ * @brief       Functions for connecting to and recieving xbee messages
+ *
+ * @addtogroup  XbeeReceive
+ * @{
+ */
+
+#ifndef XBEE_RECEIVE_H
+#define XBEE_RECEIVE_H
+
+#include <stdint.h>
+
+/**
+ * @brief       Possition and orientation data sent/received from xbee
+ *
+ * This message definition must be identical to the sending message.  This depends on the ground
+ * station and should be checked prior to using.  There are many different xbee_packet_t's floating
+ * around
+ */
+typedef struct __attribute__((packed)) xbee_packet_t
+{
+    uint32_t time;           ///< Unique id for the rigid body being described
+    float x;                 ///< x-position in the Optitrack frame
+    float y;                 ///< y-position in the Optitrack frame
+    float z;                 ///< z-position in the Optitrack frame
+    float qx;                ///< qx of quaternion
+    float qy;                ///< qy of quaternion
+    float qz;                ///< qz of quaternion
+    float qw;                ///< qw of quaternion
+    uint32_t trackingValid;  ///< (bool) of whether or not tracking was valid (0 or 1)
+} xbee_packet_t;
+
+#define NUM_FRAMING_BYTES 4                     ///< 2 START bytes + 2 Fletcher-16 checksum bytes
+#define OPTI_DATA_LENGTH sizeof(xbee_packet_t)  ///< Actual Packet Being Sent
+#define OPTI_PACKET_LENGTH OPTI_DATA_LENGTH + NUM_FRAMING_BYTES
+
+extern xbee_packet_t xbeeMsg;
+extern int xbee_portID;
+
+/**
+ * @brief       Xbee initialization function
+ *
+ * @return      0 on success, -1 on failure
+ */
+int XBEE_init();
+
+/**
+ * @brief       Read message recieved from XBee
+ *
+ * @return      0 on success, -1 on failure
+ */
+int XBEE_getData();
+
+/**
+ * @brief       Print current XBee message to stdout
+ */
+void XBEE_printData();
+
+#endif /*__XBEE_RECEIVE__ */
+
+/* @} end group XbeeReceive */

settings/hex_6dof_settings.json

@@ -37,13 +37,18 @@
 	"printf_u": true,
 	"printf_motors": true,
 	"printf_mode": true,
+	"printf_xbee": false,
 
 	"enable_logging": false,
 	"log_sensors": true,
 	"log_state": true,
 	"log_setpoint": true,
 	"log_control_u": true,
 	"log_motor_signals": true,
+	"log_throttles":true,
+	"log_xbee":true,
+	"log_dsm":true,
+	"log_flight_mode":true,
 
 	"dest_ip": "192.168.8.1",
 	"my_sys_id": 1,

settings/jellyfish_settings.json

@@ -39,7 +39,7 @@
 	"printf_u": false,
 	"printf_motors": false,
 	"printf_mode": true,
-	"printf_xbee": false, 
+	"printf_xbee": true, 
 	"printf_tracking": true, 
 
 	"enable_logging": true,

settings/pgaskell_settings.json

@@ -38,13 +38,18 @@
 	"printf_u": true,
 	"printf_motors": true,
 	"printf_mode": true,
+	"printf_xbee":false,
 
 	"enable_logging": true,
 	"log_sensors": true,
 	"log_state": true,
 	"log_setpoint": true,
 	"log_control_u": true,
 	"log_motor_signals": true,
+	"log_throttles":true,
+	"log_xbee":true,
+	"log_dsm":true,
+	"log_flight_mode":true,
 
 	"dest_ip": "192.168.8.1",
 	"my_sys_id": 1,

src/input_manager.c

@@ -118,10 +118,12 @@ void new_dsm_data_callback()
             if (new_kill <= 0.1)
             {
                 kill_switch = DISARMED;
+                user_input.kill_switch = 0;
                 user_input.requested_arm_mode = DISARMED;
             }
             else
             {
+                user_input.kill_switch = 1;
                 kill_switch = ARMED;
             }
             break;