From: Nils Forssén Date: Wed, 18 Jun 2025 22:58:13 +0000 (+0200) Subject: mavlink thread in controller X-Git-Url: https://gitweb.forssennils.se/?a=commitdiff_plain;h=407a48f43155d3bacab69745aa895a4ba326e26d;p=flygplan.git mavlink thread in controller --- diff --git a/processes/GNSS_reader/gnssrtk.cpp b/processes/GNSS_reader/gnssrtk.cpp index b15238c..59c6433 100644 --- 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 57d78c8..d28d241 100644 --- a/processes/GNSS_reader/main.cpp +++ b/processes/GNSS_reader/main.cpp @@ -23,7 +23,7 @@ static std::shared_ptr 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 fe89c61..c0f062e 100644 --- a/processes/controller/main.cpp +++ b/processes/controller/main.cpp @@ -3,6 +3,8 @@ #include #include #include +#include +#include // Pipes #include @@ -15,6 +17,7 @@ #include #include #include +#include // Local #include "pipe_def.h" @@ -28,16 +31,26 @@ static std::shared_ptr 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 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(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(temperature * 100); - scaled_imu.xacc = static_cast(accel_vec[0] * 1000); // Maybe not correct? - scaled_imu.yacc = static_cast(accel_vec[1] * 1000); // Maybe not correct? - scaled_imu.zacc = static_cast(accel_vec[2] * 1000); // Maybe not correct? - scaled_imu.xgyro = static_cast(gyro_vec[0] * 1000); // Maybe not correct? - scaled_imu.ygyro = static_cast(gyro_vec[1] * 1000); // Maybe not correct? - scaled_imu.zgyro = static_cast(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 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(temperature * 100); + scaled_imu.xacc = static_cast(accel_vec[0] * 1000); // Maybe not correct? + scaled_imu.yacc = static_cast(accel_vec[1] * 1000); // Maybe not correct? + scaled_imu.zacc = static_cast(accel_vec[2] * 1000); // Maybe not correct? + scaled_imu.xgyro = static_cast(gyro_vec[0] * 1000); // Maybe not correct? + scaled_imu.ygyro = static_cast(gyro_vec[1] * 1000); // Maybe not correct? + scaled_imu.zgyro = static_cast(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 18bccf7..d1e6695 100644 --- 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 af94c11..75b2643 100755 --- 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 f534dea..5394653 100644 --- 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 07a430f..24bb793 100755 --- 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 &