diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 21e8295182..effd0c144c 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -79,6 +79,7 @@ set(msg_files led_control.msg log_message.msg logger_status.msg + mag_worker_data.msg manual_control_setpoint.msg manual_control_switches.msg mavlink_log.msg diff --git a/msg/mag_worker_data.msg b/msg/mag_worker_data.msg new file mode 100644 index 0000000000..09626e8a80 --- /dev/null +++ b/msg/mag_worker_data.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint8 MAX_MAGS = 4 + +uint32 done_count +uint32 calibration_points_perside +uint64 calibration_interval_perside_us +uint32[4] calibration_counter_total +bool[4] side_data_collected +float32[4] x +float32[4] y +float32[4] z diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index c892b0d278..69dabf85c1 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -308,6 +308,8 @@ rtps: id: 146 - msg: control_allocator_status id: 147 + - msg: mag_worker_data + id: 148 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 170 diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 71f538a4e3..fe79b78794 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -63,6 +63,7 @@ #include #include #include +#include using namespace matrix; using namespace time_literals; @@ -78,6 +79,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma /// Data passed to calibration worker routine struct mag_worker_data_t { orb_advert_t *mavlink_log_pub; + orb_advert_t mag_worker_data_pub; bool append_to_existing_calibration; unsigned last_mag_progress; unsigned done_count; @@ -388,6 +390,38 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } } + hrt_abstime now = hrt_absolute_time(); + mag_worker_data_s status; + status.timestamp = now; + status.timestamp_sample = now; + status.done_count = worker_data->done_count; + status.calibration_points_perside = worker_data->calibration_points_perside; + status.calibration_interval_perside_us = worker_data->calibration_interval_perside_us; + + for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + status.calibration_counter_total[cur_mag] = worker_data->calibration_counter_total[cur_mag]; + status.side_data_collected[cur_mag] = worker_data->side_data_collected[cur_mag]; + + if (worker_data->calibration[cur_mag].device_id() != 0) { + const unsigned int sample = worker_data->calibration_counter_total[cur_mag] - 1; + status.x[cur_mag] = worker_data->x[cur_mag][sample]; + status.y[cur_mag] = worker_data->y[cur_mag][sample]; + status.z[cur_mag] = worker_data->z[cur_mag][sample]; + + } else { + status.x[cur_mag] = 0.f; + status.y[cur_mag] = 0.f; + status.z[cur_mag] = 0.f; + } + } + + if (worker_data->mag_worker_data_pub == nullptr) { + worker_data->mag_worker_data_pub = orb_advertise(ORB_ID(mag_worker_data), &status); + + } else { + orb_publish(ORB_ID(mag_worker_data), worker_data->mag_worker_data_pub, &status); + } + calibration_counter_side++; unsigned new_progress = progress_percentage(worker_data) + @@ -436,6 +470,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma mag_worker_data_t worker_data{}; + worker_data.mag_worker_data_pub = nullptr; + // keep and update the existing calibration when we are not doing a full 6-axis calibration worker_data.append_to_existing_calibration = cal_mask < ((1 << 6) - 1); worker_data.mavlink_log_pub = mavlink_log_pub; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index c83ce0d718..af97eb643f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -65,6 +65,7 @@ void LoggedTopics::add_default_topics() add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); + add_topic("mag_worker_data"); add_topic("manual_control_setpoint", 200); add_topic("manual_control_switches"); add_topic("mission");