+}
+
+[[noreturn]] static void imu_controller()
+{
+ 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;
+
+ c6dofimu17_axis_t accel_data;
+ c6dofimu17_axis_t gyro_data;
+
+ while (true)
+ {
+ THREAD_SLEEP(CLICK_READ_SPI_PERIOD);
+
+ /* __________________________________________________________ */
+ std::lock_guard<std::mutex> lk{imu_data_lock};
+
+ logger->debug("Reading IMU!");
+
+ 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) {
+ logger->warn("Failed to get imu data: {}", res);
+ continue;
+ }
+
+ 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);
+
+ /* __________________________________________________________ */
+ }
+}
+
+[[noreturn]] static void mavlink_handler(int recvpipe_fd)
+{
+ int res = 0;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ mavlink_message_t message;
+ mavlink_status_t status;
+
+ while (true)
+ {
+ res = read(recvpipe_fd, buffer, sizeof(buffer));
+ if (res <= 0) {
+ logger->error("Failed to read from recvpipe: {}", strerror(errno));
+ continue;
+ }
+
+ for (int i = 0; i < res; i++) {
+ if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &message, &status)) {
+
+ switch (message.msgid) {
+ case MAVLINK_MSG_ID_COMMAND_LONG:
+ {
+ mavlink_command_long_t cmd;
+ mavlink_msg_command_long_decode(&message, &cmd);
+
+ switch (cmd.command)
+ {
+ case MAV_CMD_PREFLIGHT_CALIBRATION:
+ {
+ logger->debug("Resetting and calibrating IMU etc.");
+
+ // Handle preflight calibration command here
+ std::lock_guard<std::mutex> lk{imu_data_lock};
+
+ // Reset the filter state
+ c6dofimu17_signal_path_reset();
+ comp_filter.reset();
+ gyro_vec = vec3d::Zero();
+ accel_vec = vec3d::Zero();
+ temperature = 0.0;
+ break;
+ }
+ default:
+ logger->warn("Unhandled COMMAND_LONG: {}", cmd.command);
+ break;
+ }
+ break;
+ }
+ case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
+ {
+ logger->debug("Handled mavlink message RC channels.");
+
+ std::lock_guard<std::mutex> lk{joystick_data_lock};
+
+ mavlink_rc_channels_override_t rc;
+ mavlink_msg_rc_channels_override_decode(&message, &rc);
+
+ joystick_left_x = rc.chan1_raw;
+ joystick_left_y = rc.chan2_raw;
+ joystick_right_x = rc.chan3_raw;
+ joystick_right_y = rc.chan4_raw;
+ trigger = rc.chan5_raw;
+ break;
+ }
+ default:
+ logger->warn("Unhandled MAVLink message id: {}", (int) message.msgid);
+ break;
+ }
+ break; // Exit the for loop after processing a complete message
+ }
+ }
+ }
+}
+
+[[noreturn]] static void controller()
+{
+ while (true)
+ {
+ THREAD_SLEEP(MAIN_CONTROLLER_PERIOD);
+ logger->info("Controlling!");
+
+ std::lock_guard<std::mutex> lk{joystick_data_lock};
+ logger->info("Joystick values: {}, {}, {}, {}, {}", joystick_left_x, joystick_left_y, joystick_right_x, joystick_right_y, trigger);
+
+ }
+}
projects / flygplan.git / commitdiff