mavlink thread in controller

diff --git a/processes/GNSS_reader/gnssrtk.cpp b/processes/GNSS_reader/gnssrtk.cpp
index b15238c2cd9ca1fbc483406b93e1d4b8e22ef1d4..59c643323388b8a3d90083e84a135afac6e0a093 100644 (file)
--- a/processes/GNSS_reader/gnssrtk.cpp
+++ b/processes/GNSS_reader/gnssrtk.cpp
@@ -266,6 +266,7 @@ static gnssrtk_gnrmc_t parse_gnrmc(std::string& NMEA_msg)
         if (gngga.latitude != gnrmc.latitude || gngga.longitude != gnrmc.longitude)
         {
             logger->warn("GNGGA and GNRMC position information mismatch.");
+            continue;
         }
 
         logger->debug("Constructing MAVLINK messages.");

diff --git a/processes/GNSS_reader/main.cpp b/processes/GNSS_reader/main.cpp
index 57d78c842bc2be796420faa97ed8a1664b3706cb..d28d2414d133c1f5693e268ed8085737950c09d7 100644 (file)
--- a/processes/GNSS_reader/main.cpp
+++ b/processes/GNSS_reader/main.cpp
@@ -23,7 +23,7 @@ static std::shared_ptr<spdlog::logger> logger;
 int main(int argc, char* argv[])
 {
     logger = spdlog::stdout_color_mt("GNSS_reader");
-    logger->set_level(spdlog::level::debug); // TODO change to glob
+    logger->set_level(GLOB_LOGLEVEL);
 
     logger->debug("Opening sendpipe for writing.");
     int sendpipe_fd = open(SEND_PIPE, O_WRONLY);

diff --git a/processes/controller/main.cpp b/processes/controller/main.cpp
index fe89c613645d1837decdc483016914e400c77b90..c0f062eb1a9b508b0f8793070dfafb7e5744b288 100644 (file)
--- a/processes/controller/main.cpp
+++ b/processes/controller/main.cpp
@@ -3,6 +3,8 @@
 #include <stdio.h>
 #include <chrono>
 #include <math.h>
+#include <mutex>
+#include <thread>
 
 // Pipes
 #include <sys/types.h>
@@ -15,6 +17,7 @@
 #include <spdlog/sinks/stdout_color_sinks.h>
 #include <common/mavlink.h>
 #include <bcm2835.h>
+#include <Eigen/Core>
 
 // Local
 #include "pipe_def.h"
@@ -28,16 +31,26 @@ static std::shared_ptr<spdlog::logger> logger;
 static err_t spi_init();
 static err_t spi_write_alias(uint8_t reg, uint8_t* data_out, uint8_t len);
 static err_t spi_read_alias(uint8_t reg, uint8_t* data_in, uint8_t len);
+[[noreturn]] static void send_mavlink(int);
+
+static vec3d gyro_vec;
+static vec3d accel_vec;
+static double temperature = 0.0;
+static Complementary_filter comp_filter{logger, 0.02};
+
+static std::mutex data_lock;
 
 int main(int argc, char* argv[])
 {
-    
-    logger = spdlog::stdout_color_mt("click_reader");
+    logger = spdlog::stdout_color_mt("controller");
     logger->set_level(GLOB_LOGLEVEL);
 
     logger->debug("Opening sendpipe for writing.");
     int sendpipe_fd = open(SEND_PIPE, O_WRONLY);
 
+    logger->debug("Opening recvpipe for reading.");
+    int recvpipe_fd = open(RECV_PIPE, O_RDONLY);
+
     logger->info("Initializing BCM2835 and SPI."); 
     if (spi_init() != OK)
     {
@@ -65,32 +78,32 @@ int main(int argc, char* argv[])
 
     c6dofimu17_cfg(&gyro_cfg, &accel_cfg);
 
-    c6dofimu17_axis_t accel_data;
-    c6dofimu17_axis_t gyro_data;
-    double temperature = 0.0;
-
-    Complementary_filter comp_filter{logger, 0.02};
-
     int res = 0;
     auto t_start = std::chrono::high_resolution_clock::now();
     auto t_end = std::chrono::high_resolution_clock::now();
     double time_d = 0.0;
 
-    mavlink_message_t msg;
-    mavlink_attitude_t attitude;
-    mavlink_scaled_imu_t scaled_imu; 
+    c6dofimu17_axis_t accel_data;
+    c6dofimu17_axis_t gyro_data;
+
+    // Mavlink thread
+    std::thread mavlink_task{send_mavlink, sendpipe_fd};
+
     // Control loop
     while (true) 
     {
         THREAD_SLEEP(CLICK_READ_SPI_PERIOD);
 
+        std::lock_guard<std::mutex> lk{data_lock};
+
+        logger->debug("Controlling!");
+
         res = c6dofimu17_get_gyro_data(&gyro_data);
         t_end = std::chrono::high_resolution_clock::now();
         time_d = std::chrono::duration<double>(t_end-t_start).count();
         t_start = std::chrono::high_resolution_clock::now();
 
         res += c6dofimu17_get_accel_data(&accel_data);
-
         res += c6dofimu17_get_temperature(&temperature);
 
         if (res != OK) {
@@ -98,53 +111,10 @@ int main(int argc, char* argv[])
             continue;
         }
 
-        vec3d gyro_vec = struct_to_vec_scaled(gyro_data, (M_PI * 1000.0) / (32768.0 * 180.0));
-        vec3d accel_vec = struct_to_vec_scaled(accel_data, (GRAVITY * 16.0) / 32768.0);
+        gyro_vec = struct_to_vec_scaled(gyro_data, (M_PI * 1000.0) / (32768.0 * 180.0));
+        accel_vec = struct_to_vec_scaled(accel_data, (GRAVITY * 16.0) / 32768.0);
 
         comp_filter.update(gyro_vec, accel_vec, time_d);
-        
-        // Pack data into MAVLINK packages
-        attitude.time_boot_ms = system_uptime_ms(); 
-        attitude.roll = comp_filter.get_state()[1];
-        attitude.pitch = comp_filter.get_state()[0];
-        attitude.yaw = 0.0;
-        attitude.rollspeed = 0.0;
-        attitude.pitchspeed = 0.0;
-        attitude.yawspeed = 0.0;
-
-        scaled_imu.time_boot_ms = system_uptime_ms();
-        scaled_imu.temperature = static_cast<int16_t>(temperature * 100);
-        scaled_imu.xacc = static_cast<int16_t>(accel_vec[0] * 1000); // Maybe not correct?
-        scaled_imu.yacc = static_cast<int16_t>(accel_vec[1] * 1000); // Maybe not correct?
-        scaled_imu.zacc = static_cast<int16_t>(accel_vec[2] * 1000); // Maybe not correct?
-        scaled_imu.xgyro = static_cast<int16_t>(gyro_vec[0] * 1000); // Maybe not correct?
-        scaled_imu.ygyro = static_cast<int16_t>(gyro_vec[1] * 1000); // Maybe not correct?
-        scaled_imu.zgyro = static_cast<int16_t>(gyro_vec[2] * 1000); // Maybe not correct?
-        scaled_imu.xmag = INT16_MAX;
-        scaled_imu.ymag = INT16_MAX;
-        scaled_imu.zmag = INT16_MAX;
-
-        mavlink_msg_attitude_encode(
-            MAV_SYS_ID, 
-            MAV_COMP_ID,
-            &msg,
-            &attitude
-        );
-
-        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-        int len;
-
-        len = mavlink_msg_to_send_buffer(buffer, &msg);
-        write(sendpipe_fd, buffer, len);
-
-        mavlink_msg_scaled_imu_encode(
-            MAV_SYS_ID, 
-            MAV_COMP_ID,
-            &msg,
-            &scaled_imu
-        );
-        len = mavlink_msg_to_send_buffer(buffer, &msg);
-        write(sendpipe_fd, buffer, len);
     }
 
     close(sendpipe_fd);
@@ -195,4 +165,62 @@ static err_t spi_init()
     bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW);      // the default
 
     return OK;
+}
+
+[[noreturn]] static void send_mavlink(int sendpipe_fd)
+{
+    mavlink_message_t msg;
+    mavlink_attitude_t attitude;
+    mavlink_scaled_imu_t scaled_imu;
+
+    while (true)
+    {
+        THREAD_SLEEP(CONTROLLER_MAVLINK_PERIOD);
+        std::lock_guard<std::mutex> lk{data_lock};
+
+        logger->debug("Sending IMU Mavlink.");
+
+        // Pack data into MAVLINK packages
+        attitude.time_boot_ms = system_uptime_ms(); 
+        attitude.roll = comp_filter.get_state()[1];
+        attitude.pitch = comp_filter.get_state()[0];
+        attitude.yaw = 0.0;
+        attitude.rollspeed = 0.0;
+        attitude.pitchspeed = 0.0;
+        attitude.yawspeed = 0.0;
+
+        scaled_imu.time_boot_ms = system_uptime_ms();
+        scaled_imu.temperature = static_cast<int16_t>(temperature * 100);
+        scaled_imu.xacc = static_cast<int16_t>(accel_vec[0] * 1000); // Maybe not correct?
+        scaled_imu.yacc = static_cast<int16_t>(accel_vec[1] * 1000); // Maybe not correct?
+        scaled_imu.zacc = static_cast<int16_t>(accel_vec[2] * 1000); // Maybe not correct?
+        scaled_imu.xgyro = static_cast<int16_t>(gyro_vec[0] * 1000); // Maybe not correct?
+        scaled_imu.ygyro = static_cast<int16_t>(gyro_vec[1] * 1000); // Maybe not correct?
+        scaled_imu.zgyro = static_cast<int16_t>(gyro_vec[2] * 1000); // Maybe not correct?
+        scaled_imu.xmag = INT16_MAX;
+        scaled_imu.ymag = INT16_MAX;
+        scaled_imu.zmag = INT16_MAX;
+
+        mavlink_msg_attitude_encode(
+            MAV_SYS_ID, 
+            MAV_COMP_ID,
+            &msg,
+            &attitude
+        );
+
+        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+        int len;
+
+        len = mavlink_msg_to_send_buffer(buffer, &msg);
+        write(sendpipe_fd, buffer, len);
+
+        mavlink_msg_scaled_imu_encode(
+            MAV_SYS_ID, 
+            MAV_COMP_ID,
+            &msg,
+            &scaled_imu
+        );
+        len = mavlink_msg_to_send_buffer(buffer, &msg);
+        write(sendpipe_fd, buffer, len);
+    }
 }
\ No newline at end of file

diff --git a/processes/include/system_def.h b/processes/include/system_def.h
index 18bccf7dd139106c9f2dbecd517ae0d6a9c5d589..d1e6695cf9e56a2d40a925ce82e073da78e307f0 100644 (file)
--- a/processes/include/system_def.h
+++ b/processes/include/system_def.h
@@ -10,6 +10,7 @@ const uint8_t MAV_CHAN_ID = MAVLINK_COMM_0;
 
 #define CLICK_READ_SPI_PERIOD 10ms
 #define GNSS_MAVLINK_PERIOD 100ms
+#define CONTROLLER_MAVLINK_PERIOD 100ms
 #define MAVLINK_HEARTBEAT_PERIOD 1s
 
 #endif
\ No newline at end of file

diff --git a/processes/udp_server/main.cpp b/processes/udp_server/main.cpp
index af94c11de86d629b6e0ad75d1a63dab5892efadf..75b2643dfdf93215a1d9d3ab1df79f940ec931b8 100755 (executable)
--- a/processes/udp_server/main.cpp
+++ b/processes/udp_server/main.cpp
@@ -26,9 +26,11 @@ int main(int argc, char* argv[])
     int sendpipe_fd = open(SEND_PIPE, O_RDONLY);
     int recvpipe_fd = open(RECV_PIPE, O_WRONLY);
 
+    logger->info("Initializing UDP-server.");
     udp_server_init(logger, recvpipe_fd, sendpipe_fd);
 
     // Server mainloop, only exits on error
+    logger->info("Running UDP-server.");
     udp_server_run();
 
     logger->debug("Closing sendpipe.");

diff --git a/processes/udp_server/udp_server.cpp b/processes/udp_server/udp_server.cpp
index f534dea90a8a8dbe5b75d77c9324eb7dd1211112..539465301f882971bac16c98f74570c8f9b0c489 100644 (file)
--- a/processes/udp_server/udp_server.cpp
+++ b/processes/udp_server/udp_server.cpp
@@ -36,7 +36,7 @@ static int sendpipe;
 static void handle_request_autopilot_capabilities(const mavlink_command_long_t& cmd_long);
 static void handle_heartbeat(const mavlink_message_t& message);
 static void recv_thread();
-static void heartbeat_thread();
+[[noreturn]] static void heartbeat_thread();
 static void handle_mavlink(mavlink_message_t& message);
 
 // Definitions
@@ -114,7 +114,7 @@ static void recv_thread()
     }
 }
 
-static void heartbeat_thread()
+[[noreturn]] static void heartbeat_thread()
 {
     mavlink_message_t message;
     mavlink_msg_heartbeat_pack(

diff --git a/run.sh b/run.sh
index 07a430fc35987e9a53727400b6b813145a1a89ac..24bb793eb8d4d8e9d08a8c1f6f6d0ed2b90467c9 100755 (executable)
--- a/run.sh
+++ b/run.sh
@@ -24,7 +24,7 @@ trap intexit INT
 sleep 1
 
 # start udp server
-./udp_server &
+./udp_server & 
 
 # start click reader
 ./controller &