vec3d struct_to_vec_scaled(const c6dofimu17_axis_t&, const double);
-class Complementary_filter
+class Abstract_Filter
+{
+public:
+ Abstract_Filter(std::shared_ptr<spdlog::logger> logg) :
+ logger{logg},
+ current_state{vec2d::Zero()}
+ {}
+ ~Abstract_Filter() = default;
+
+ virtual void update(const vec3d&, const vec3d&, double) = 0;
+
+ const vec2d get_state() {
+ return current_state;
+ }
+
+ virtual void reset() = 0;
+
+protected:
+ std::shared_ptr<spdlog::logger> logger;
+ vec2d current_state; // Pitch and roll
+};
+
+class Complementary_filter : public Abstract_Filter
{
public:
Complementary_filter(std::shared_ptr<spdlog::logger>, const double = 0.02);
- void update(const vec3d&, const vec3d&,
- double, const unsigned int = 1000, const unsigned int = 16);
+ void update(const vec3d&, const vec3d&, double) override;
- const vec2d get_state();
+ void reset() override;
private:
- std::shared_ptr<spdlog::logger> logger;
const double alpha;
- vec2d current_state;
};
-class EKF
+class EKF : public Abstract_Filter
{
public:
EKF(std::shared_ptr<spdlog::logger>);
- void update(const vec3d&, const vec3d&,
- double);
-
- const vec2d get_state();
+ void update(const vec3d&, const vec3d&, double) override;
+ void reset() override;
private:
- std::shared_ptr<spdlog::logger> logger;
- vec2d current_state;
};
+
#endif
\ No newline at end of file
static std::shared_ptr<spdlog::logger> logger;
+static err_t c6dofimu17_cfg();
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);
+static err_t spi_write_alias(uint8_t, uint8_t*, uint8_t);
+static err_t spi_read_alias(uint8_t, uint8_t*, uint8_t);
[[noreturn]] static void send_mavlink(int);
+[[noreturn]] static void imu_controller();
+[[noreturn]] static void mavlink_handler(int);
+[[noreturn]] static void controller();
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;
+static std::mutex imu_data_lock;
+
+static uint16_t joystick_left_x = 0.0;
+static uint16_t joystick_left_y = 0.0;
+static uint16_t joystick_right_x = 0.0;
+static uint16_t joystick_right_y = 0.0;
+static uint16_t trigger = 0.0;
+
+static std::mutex joystick_data_lock;
int main(int argc, char* argv[])
{
}
logger->debug("Initializing 6DOF IMU 17 Click.");
+ if (c6dofimu17_cfg() != OK)
+ {
+ logger->error("IMU cfg init failed.");
+ return 1;
+ }
+
+ // Starting threads
+ std::thread mavlink_task{send_mavlink, sendpipe_fd};
+ std::thread imu_task{imu_controller};
+ std::thread mavlink_handler_task{mavlink_handler, recvpipe_fd};
+ std::thread controller_task{controller};
+
+
+ mavlink_task.join();
+ imu_task.join();
+ mavlink_handler_task.join();
+ controller_task.join();
+
+ close(sendpipe_fd);
+ return 0;
+}
+
+static err_t c6dofimu17_cfg()
+{
c6dofimu17_init_comm(*spi_read_alias, *spi_write_alias);
c6dofimu17_gyro_cfg_t gyro_cfg;
gyro_cfg.gyro_ui_filt_bw = C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_20; // ~50Hz
c6dofimu17_cfg(&gyro_cfg, &accel_cfg);
-
- 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;
-
- // 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) {
- 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);
-
- /* __________________________________________________________ */
- }
-
- close(sendpipe_fd);
- return 0;
+ return OK;
}
// Some good old C!
logger->debug("Sending IMU Mavlink.");
/* __________________________________________________________ */
- data_lock.lock();
+ imu_data_lock.lock();
// Pack data into MAVLINK packages
attitude.time_boot_ms = system_uptime_ms();
scaled_imu.ymag = INT16_MAX;
scaled_imu.zmag = INT16_MAX;
- data_lock.unlock();
+ imu_data_lock.unlock();
/* __________________________________________________________ */
mavlink_msg_attitude_encode(
len = mavlink_msg_to_send_buffer(buffer, &msg);
write(sendpipe_fd, buffer, len);
}
-}
\ No newline at end of file
+}
+
+[[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