20604212380

Committed 30 Dec 2025 07:24PM UTC coverage: 48.017% (-0.06%) from 48.078%

Build # 20604212380

Build Type

Pull #2746

github

Committed by

web-flow

Commit Message

Merge 5269e5f1e into 5d2947b34

Pull Request Pull Request #2746: Configuration and component cleanup

Run Details

47 of 158 new or added lines in 19 files covered. (29.75%)

21 existing lines in 9 files now uncovered.

17769 of 37006 relevant lines covered (48.02%)

483.39 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/src/mavsdk/plugins/calibration/calibration_impl.cpp

#include "calibration_impl.h"

#include <functional>
#include <string>

#include "autopilot.h"
#include "px4_custom_mode.h"
#include "system.h"

namespace mavsdk {

CalibrationImpl::CalibrationImpl(System& system) : PluginImplBase(system)
{
    _system_impl->register_plugin(this);
}

CalibrationImpl::CalibrationImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))
{
    _system_impl->register_plugin(this);
}

CalibrationImpl::~CalibrationImpl()
{
    _system_impl->unregister_plugin(this);
}

void CalibrationImpl::init()
{
    _system_impl->register_statustext_handler(
        [this](const MavlinkStatustextHandler::Statustext& statustext) {
            receive_statustext(statustext);
        },
        this);
}

void CalibrationImpl::deinit()
{
    _system_impl->unregister_statustext_handler(this);
}

void CalibrationImpl::enable() {}

void CalibrationImpl::disable() {}

void CalibrationImpl::calibrate_gyro_async(const CalibrationCallback& callback)
{
    // The calibration is currently implemented using a PX4 specific
    // API based on statustext for progress and instructions.
    if (_system_impl->effective_autopilot() != Autopilot::Px4) {
        call_callback(callback, Calibration::Result::Unsupported, {});
        return;
    }

    std::lock_guard<std::mutex> lock(_calibration_mutex);

    if (_system_impl->is_armed()) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::FailedArmed, progress_data);
        return;
    }

    if (_state != State::None) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::Busy, progress_data);
        return;
    }

    _state = State::GyroCalibration;
    _calibration_callback = callback;

    MavlinkCommandSender::CommandLong command{};
    command.command = MAV_CMD_PREFLIGHT_CALIBRATION;
    command.params.maybe_param1 = 1.0f; // Gyro
    command.params.maybe_param2 = 0.0f;
    command.params.maybe_param3 = 0.0f;
    command.params.maybe_param4 = 0.0f;
    command.params.maybe_param5 = 0.0f;
    command.target_component_id = MAV_COMP_ID_AUTOPILOT1;
    _system_impl->send_command_async(
        command, [this](MavlinkCommandSender::Result command_result, float progress) {
            command_result_callback(command_result, progress);
        });
}

void CalibrationImpl::call_callback(
    const CalibrationCallback& callback,
    const Calibration::Result& result,
    const Calibration::ProgressData progress_data)
{
    if (callback) {
        _system_impl->call_user_callback(
            [callback, result, progress_data]() { callback(result, progress_data); });
    }
}

void CalibrationImpl::calibrate_accelerometer_async(const CalibrationCallback& callback)
{
    // The calibration is currently implemented using a PX4 specific
    // API based on statustext for progress and instructions.
    if (_system_impl->effective_autopilot() != Autopilot::Px4) {
        call_callback(callback, Calibration::Result::Unsupported, {});
        return;
    }

    std::lock_guard<std::mutex> lock(_calibration_mutex);

    if (_system_impl->is_armed()) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::FailedArmed, progress_data);
        return;
    }

    if (_state != State::None) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::Busy, progress_data);
        return;
    }

    _state = State::AccelerometerCalibration;
    _calibration_callback = callback;

    MavlinkCommandSender::CommandLong command{};
    command.command = MAV_CMD_PREFLIGHT_CALIBRATION;
    command.params.maybe_param1 = 0.0f;
    command.params.maybe_param2 = 0.0f;
    command.params.maybe_param3 = 0.0f;
    command.params.maybe_param4 = 0.0f;
    command.params.maybe_param5 = 1.0f; // Accel
    command.target_component_id = MAV_COMP_ID_AUTOPILOT1;
    _system_impl->send_command_async(
        command, [this](MavlinkCommandSender::Result command_result, float progress) {
            command_result_callback(command_result, progress);
        });
}

void CalibrationImpl::calibrate_magnetometer_async(const CalibrationCallback& callback)
{
    // The calibration is currently implemented using a PX4 specific
    // API based on statustext for progress and instructions.
    if (_system_impl->effective_autopilot() != Autopilot::Px4) {
        call_callback(callback, Calibration::Result::Unsupported, {});
        return;
    }

    std::lock_guard<std::mutex> lock(_calibration_mutex);

    if (_system_impl->is_armed()) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::FailedArmed, progress_data);
        return;
    }

    if (_state != State::None) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::Busy, progress_data);
        return;
    }

    _state = State::MagnetometerCalibration;
    _calibration_callback = callback;

    MavlinkCommandSender::CommandLong command{};
    command.command = MAV_CMD_PREFLIGHT_CALIBRATION;
    command.params.maybe_param1 = 0.0f;
    command.params.maybe_param2 = 1.0f; // Mag
    command.params.maybe_param3 = 0.0f;
    command.params.maybe_param4 = 0.0f;
    command.params.maybe_param5 = 0.0f;
    command.target_component_id = MAV_COMP_ID_AUTOPILOT1;
    _system_impl->send_command_async(
        command, [this](MavlinkCommandSender::Result command_result, float progress) {
            command_result_callback(command_result, progress);
        });
}

void CalibrationImpl::calibrate_level_horizon_async(const CalibrationCallback& callback)
{
    // The calibration is currently implemented using a PX4 specific
    // API based on statustext for progress and instructions.
    if (_system_impl->effective_autopilot() != Autopilot::Px4) {
        call_callback(callback, Calibration::Result::Unsupported, {});
        return;
    }

    std::lock_guard<std::mutex> lock(_calibration_mutex);

    if (_system_impl->is_armed()) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::FailedArmed, progress_data);
        return;
    }

    if (_state != State::None) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::Busy, progress_data);
        return;
    }

    _state = State::AccelerometerCalibration;
    _calibration_callback = callback;

    MavlinkCommandSender::CommandLong command{};
    command.command = MAV_CMD_PREFLIGHT_CALIBRATION;
    command.params.maybe_param1 = 0.0f;
    command.params.maybe_param2 = 0.0f;
    command.params.maybe_param3 = 0.0f;
    command.params.maybe_param4 = 0.0f;
    command.params.maybe_param5 = 2.0f; // Board Level
    command.target_component_id = MAV_COMP_ID_AUTOPILOT1;
    _system_impl->send_command_async(
        command, [this](MavlinkCommandSender::Result command_result, float progress) {
            command_result_callback(command_result, progress);
        });
}

void CalibrationImpl::calibrate_gimbal_accelerometer_async(const CalibrationCallback& callback)
{
    // The calibration is currently implemented using a PX4 specific
    // API based on statustext for progress and instructions.
    if (_system_impl->effective_autopilot() != Autopilot::Px4) {
        call_callback(callback, Calibration::Result::Unsupported, {});
        return;
    }

    std::lock_guard<std::mutex> lock(_calibration_mutex);

    if (_system_impl->is_armed()) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::FailedArmed, progress_data);
        return;
    }

    if (_state != State::None) {
        Calibration::ProgressData progress_data;
        call_callback(callback, Calibration::Result::Busy, progress_data);
        return;
    }

    _state = State::GimbalAccelerometerCalibration;
    _calibration_callback = callback;

    MavlinkCommandSender::CommandLong command{};
    command.command = MAV_CMD_PREFLIGHT_CALIBRATION;
    command.params.maybe_param1 = 0.0f;
    command.params.maybe_param2 = 0.0f;
    command.params.maybe_param3 = 0.0f;
    command.params.maybe_param4 = 0.0f;
    command.params.maybe_param5 = 1.0f; // Accel
    command.target_component_id = MAV_COMP_ID_GIMBAL;
    _system_impl->send_command_async(
        command, [this](MavlinkCommandSender::Result command_result, float progress) {
            command_result_callback(command_result, progress);
            if (command_result == MavlinkCommandSender::Result::Success) {
                // The gimbal which implements this so far actually uses acks
                // with progress and final ack, so we need to finish at the end.
                report_done();
            }
        });
}

Calibration::Result CalibrationImpl::cancel()
{
    // The calibration is currently implemented using a PX4 specific
    // API based on statustext for progress and instructions.
    if (_system_impl->effective_autopilot() != Autopilot::Px4) {
        return Calibration::Result::Unsupported;
    }

    std::lock_guard<std::mutex> lock(_calibration_mutex);

    uint8_t target_component_id = MAV_COMP_ID_AUTOPILOT1;

    switch (_state) {
        case State::None:
            LogWarn() << "No calibration to cancel";
            return Calibration::Result::Success;
        case State::GyroCalibration:
            break;
        case State::AccelerometerCalibration:
            break;
        case State::MagnetometerCalibration:
            break;
        case State::LevelHorizonCalibration:
            break;
        case State::GimbalAccelerometerCalibration:
            target_component_id = MAV_COMP_ID_GIMBAL;
            break;
    }

    MavlinkCommandSender::CommandLong command{};
    command.command = MAV_CMD_PREFLIGHT_CALIBRATION;
    // All params 0 signal cancellation of a calibration.
    command.params.maybe_param1 = 0.0f;
    command.params.maybe_param2 = 0.0f;
    command.params.maybe_param3 = 0.0f;
    command.params.maybe_param4 = 0.0f;
    command.params.maybe_param5 = 0.0f;
    command.params.maybe_param6 = 0.0f;
    command.params.maybe_param7 = 0.0f;
    command.target_component_id = target_component_id;

    auto prom = std::promise<Calibration::Result>();
    auto fut = prom.get_future();
    _system_impl->send_command_async(
        command, [&prom](MavlinkCommandSender::Result command_result, float) {
            if (command_result != MavlinkCommandSender::Result::Success) {
                prom.set_value(Calibration::Result::ConnectionError);
            } else {
                prom.set_value(Calibration::Result::Success);
            }
        });

    return fut.get();
}

void CalibrationImpl::command_result_callback(
    MavlinkCommandSender::Result command_result, float progress)
{
    std::lock_guard<std::mutex> lock(_calibration_mutex);

    if (_state == State::None) {
        // It might be someone else like a ground station trying to do a
        // calibration. We silently ignore it.
        return;
    }

    // If we get a progress result here, it is the new interface and we can
    // use the progress info. If we get an ack, we need to translate that to
    // a first progress update, and then parse the statustexts for progress.
    switch (command_result) {
        case MavlinkCommandSender::Result::Success:
            // Silently ignore.
            break;

        case MavlinkCommandSender::Result::NoSystem:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::ConnectionError:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::Busy:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::Denied:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::Unsupported:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::Timeout:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::TemporarilyRejected:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::Failed:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::Cancelled:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::UnknownError: {
            // Report all error cases.
            const auto timeout_result = calibration_result_from_command_result(command_result);
            call_callback(_calibration_callback, timeout_result, Calibration::ProgressData());
            _calibration_callback = nullptr;
            _state = State::None;
            break;
        }

        case MavlinkCommandSender::Result::InProgress: {
            const auto progress_result = calibration_result_from_command_result(command_result);
            Calibration::ProgressData progress_data;
            progress_data.has_progress = true;
            progress_data.progress = progress;

            call_callback(_calibration_callback, progress_result, progress_data);
            break;
        }
    };
}

Calibration::Result
CalibrationImpl::calibration_result_from_command_result(MavlinkCommandSender::Result result)
{
    switch (result) {
        case MavlinkCommandSender::Result::Success:
            return Calibration::Result::Success;
        case MavlinkCommandSender::Result::NoSystem:
            return Calibration::Result::NoSystem;
        case MavlinkCommandSender::Result::ConnectionError:
            return Calibration::Result::ConnectionError;
        case MavlinkCommandSender::Result::Busy:
            return Calibration::Result::Busy;
        case MavlinkCommandSender::Result::Denied:
            // Fallthrough
        case MavlinkCommandSender::Result::TemporarilyRejected:
            return Calibration::Result::CommandDenied;
        case MavlinkCommandSender::Result::Failed:
            return Calibration::Result::Failed;
        case MavlinkCommandSender::Result::Cancelled:
            return Calibration::Result::Cancelled;
        case MavlinkCommandSender::Result::Timeout:
            return Calibration::Result::Timeout;
        case MavlinkCommandSender::Result::InProgress:
            return Calibration::Result::Next;
        case MavlinkCommandSender::Result::Unsupported:
            return Calibration::Result::Unsupported;
        default:
            return Calibration::Result::Unknown;
    }
}

void CalibrationImpl::receive_statustext(const MavlinkStatustextHandler::Statustext& statustext)
{
    std::lock_guard<std::mutex> lock(_calibration_mutex);
    if (_state == State::None) {
        return;
    }

    _parser.reset();

    _parser.parse(statustext.text);

    switch (_parser.get_status()) {
        case CalibrationStatustextParser::Status::None:
            // Ignore it.
            break;
        case CalibrationStatustextParser::Status::Started:
            report_started();
            break;
        case CalibrationStatustextParser::Status::Done:
            report_done();
            break;
        case CalibrationStatustextParser::Status::Failed:
            report_failed(_parser.get_failed_message());
            break;
        case CalibrationStatustextParser::Status::Cancelled:
            report_cancelled();
            break;
        case CalibrationStatustextParser::Status::Progress:
            report_progress(_parser.get_progress());
            break;
        case CalibrationStatustextParser::Status::Instruction:
            report_instruction(_parser.get_instruction());
            break;
    }

    // In case we succeed or fail we need to notify that params
    // might have changed.
    switch (_parser.get_status()) {
        case CalibrationStatustextParser::Status::Done:
            // FALLTHROUGH
        case CalibrationStatustextParser::Status::Failed:
            // FALLTHROUGH
        case CalibrationStatustextParser::Status::Cancelled:
            switch (_state) {
                case State::None:
                    break;
                case State::GyroCalibration:
                    _system_impl->param_changed("CAL_GYRO0_ID");
                    break;
                case State::AccelerometerCalibration:
                    _system_impl->param_changed("CAL_ACC0_ID");
                    break;
                case State::MagnetometerCalibration:
                    _system_impl->param_changed("CAL_MAG0_ID");
                    break;
                case State::LevelHorizonCalibration:
                    _system_impl->param_changed("SENS_BOARD_X_OFF");
                    _system_impl->param_changed("SENS_BOARD_Y_OFF");
                    _system_impl->param_changed("SENS_BOARD_Z_OFF");
                    break;
                case State::GimbalAccelerometerCalibration:
                    break;
            }

        default:
            break;
    }

    switch (_parser.get_status()) {
        case CalibrationStatustextParser::Status::Done:
            // FALLTHROUGH
        case CalibrationStatustextParser::Status::Failed:
            // FALLTHROUGH
        case CalibrationStatustextParser::Status::Cancelled:
            _calibration_callback = nullptr;
            _state = State::None;
            break;
        default:
            break;
    }
}

void CalibrationImpl::report_started()
{
    report_progress(0.0f);
}

void CalibrationImpl::report_done()
{
    const Calibration::ProgressData progress_data;
    call_callback(_calibration_callback, Calibration::Result::Success, progress_data);
}

void CalibrationImpl::report_failed(const std::string& failed)
{
    LogErr() << "Calibration failed: " << failed;
    const Calibration::ProgressData progress_data;
    call_callback(_calibration_callback, Calibration::Result::Failed, progress_data);
}

void CalibrationImpl::report_cancelled()
{
    LogWarn() << "Calibration was cancelled";
    const Calibration::ProgressData progress_data;
    call_callback(_calibration_callback, Calibration::Result::Cancelled, progress_data);
}

void CalibrationImpl::report_progress(float progress)
{
    Calibration::ProgressData progress_data;
    progress_data.has_progress = true;
    progress_data.progress = progress;
    call_callback(_calibration_callback, Calibration::Result::Next, progress_data);
}

void CalibrationImpl::report_instruction(const std::string& instruction)
{
    Calibration::ProgressData progress_data;
    progress_data.has_status_text = true;
    progress_data.status_text = instruction;
    call_callback(_calibration_callback, Calibration::Result::Next, progress_data);
}

} // namespace mavsdk

1	#include "calibration_impl.h"
2
3	#include <functional>
4	#include <string>
5
6	#include "autopilot.h"
7	#include "px4_custom_mode.h"
8	#include "system.h"
9
10	namespace mavsdk {
11
12	CalibrationImpl::CalibrationImpl(System& system) : PluginImplBase(system)	×
13	{
14	_system_impl->register_plugin(this);	×
15	}	×
16
17	CalibrationImpl::CalibrationImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))	×
18	{
19	_system_impl->register_plugin(this);	×
20	}	×
21
22	CalibrationImpl::~CalibrationImpl()	×
23	{
24	_system_impl->unregister_plugin(this);	×
25	}	×
26
27	void CalibrationImpl::init()	×
28	{
29	_system_impl->register_statustext_handler(	×
30	[this](const MavlinkStatustextHandler::Statustext& statustext) {	×
31	receive_statustext(statustext);	×
32	},	×
33	this);
34	}	×
35
36	void CalibrationImpl::deinit()	×
37	{
38	_system_impl->unregister_statustext_handler(this);	×
39	}	×
40
41	void CalibrationImpl::enable() {}	×
42
43	void CalibrationImpl::disable() {}	×
44
45	void CalibrationImpl::calibrate_gyro_async(const CalibrationCallback& callback)	×
46	{
47	// The calibration is currently implemented using a PX4 specific
48	// API based on statustext for progress and instructions.
NEW 49	if (_system_impl->effective_autopilot() != Autopilot::Px4) {	×
NEW 50	call_callback(callback, Calibration::Result::Unsupported, {});	×
NEW 51	return;	×
52	}
53
UNCOV 54	std::lock_guard<std::mutex> lock(_calibration_mutex);	×
55
56	if (_system_impl->is_armed()) {	×
57	Calibration::ProgressData progress_data;	×
58	call_callback(callback, Calibration::Result::FailedArmed, progress_data);	×
59	return;	×
60	}	×
61
62	if (_state != State::None) {	×
63	Calibration::ProgressData progress_data;	×
64	call_callback(callback, Calibration::Result::Busy, progress_data);	×
65	return;	×
66	}	×
67
68	_state = State::GyroCalibration;	×
69	_calibration_callback = callback;	×
70
71	MavlinkCommandSender::CommandLong command{};	×
72	command.command = MAV_CMD_PREFLIGHT_CALIBRATION;	×
73	command.params.maybe_param1 = 1.0f; // Gyro	×
74	command.params.maybe_param2 = 0.0f;	×
75	command.params.maybe_param3 = 0.0f;	×
76	command.params.maybe_param4 = 0.0f;	×
77	command.params.maybe_param5 = 0.0f;	×
78	command.target_component_id = MAV_COMP_ID_AUTOPILOT1;	×
79	_system_impl->send_command_async(	×
80	command, [this](MavlinkCommandSender::Result command_result, float progress) {	×
81	command_result_callback(command_result, progress);	×
82	});	×
83	}	×
84
85	void CalibrationImpl::call_callback(	×
86	const CalibrationCallback& callback,
87	const Calibration::Result& result,
88	const Calibration::ProgressData progress_data)
89	{
90	if (callback) {	×
91	_system_impl->call_user_callback(	×
92	[callback, result, progress_data]() { callback(result, progress_data); });
93	}
94	}	×
95
96	void CalibrationImpl::calibrate_accelerometer_async(const CalibrationCallback& callback)	×
97	{
98	// The calibration is currently implemented using a PX4 specific
99	// API based on statustext for progress and instructions.
NEW 100	if (_system_impl->effective_autopilot() != Autopilot::Px4) {	×
NEW 101	call_callback(callback, Calibration::Result::Unsupported, {});	×
NEW 102	return;	×
103	}
104
UNCOV 105	std::lock_guard<std::mutex> lock(_calibration_mutex);	×
106
107	if (_system_impl->is_armed()) {	×
108	Calibration::ProgressData progress_data;	×
109	call_callback(callback, Calibration::Result::FailedArmed, progress_data);	×
110	return;	×
111	}	×
112
113	if (_state != State::None) {	×
114	Calibration::ProgressData progress_data;	×
115	call_callback(callback, Calibration::Result::Busy, progress_data);	×
116	return;	×
117	}	×
118
119	_state = State::AccelerometerCalibration;	×
120	_calibration_callback = callback;	×
121
122	MavlinkCommandSender::CommandLong command{};	×
123	command.command = MAV_CMD_PREFLIGHT_CALIBRATION;	×
124	command.params.maybe_param1 = 0.0f;	×
125	command.params.maybe_param2 = 0.0f;	×
126	command.params.maybe_param3 = 0.0f;	×
127	command.params.maybe_param4 = 0.0f;	×
128	command.params.maybe_param5 = 1.0f; // Accel	×
129	command.target_component_id = MAV_COMP_ID_AUTOPILOT1;	×
130	_system_impl->send_command_async(	×
131	command, [this](MavlinkCommandSender::Result command_result, float progress) {	×
132	command_result_callback(command_result, progress);	×
133	});	×
134	}	×
135
136	void CalibrationImpl::calibrate_magnetometer_async(const CalibrationCallback& callback)	×
137	{
138	// The calibration is currently implemented using a PX4 specific
139	// API based on statustext for progress and instructions.
NEW 140	if (_system_impl->effective_autopilot() != Autopilot::Px4) {	×
NEW 141	call_callback(callback, Calibration::Result::Unsupported, {});	×
NEW 142	return;	×
143	}
144
UNCOV 145	std::lock_guard<std::mutex> lock(_calibration_mutex);	×
146
147	if (_system_impl->is_armed()) {	×
148	Calibration::ProgressData progress_data;	×
149	call_callback(callback, Calibration::Result::FailedArmed, progress_data);	×
150	return;	×
151	}	×
152
153	if (_state != State::None) {	×
154	Calibration::ProgressData progress_data;	×
155	call_callback(callback, Calibration::Result::Busy, progress_data);	×
156	return;	×
157	}	×
158
159	_state = State::MagnetometerCalibration;	×
160	_calibration_callback = callback;	×
161
162	MavlinkCommandSender::CommandLong command{};	×
163	command.command = MAV_CMD_PREFLIGHT_CALIBRATION;	×
164	command.params.maybe_param1 = 0.0f;	×
165	command.params.maybe_param2 = 1.0f; // Mag	×
166	command.params.maybe_param3 = 0.0f;	×
167	command.params.maybe_param4 = 0.0f;	×
168	command.params.maybe_param5 = 0.0f;	×
169	command.target_component_id = MAV_COMP_ID_AUTOPILOT1;	×
170	_system_impl->send_command_async(	×
171	command, [this](MavlinkCommandSender::Result command_result, float progress) {	×
172	command_result_callback(command_result, progress);	×
173	});	×
174	}	×
175
176	void CalibrationImpl::calibrate_level_horizon_async(const CalibrationCallback& callback)	×
177	{
178	// The calibration is currently implemented using a PX4 specific
179	// API based on statustext for progress and instructions.
NEW 180	if (_system_impl->effective_autopilot() != Autopilot::Px4) {	×
NEW 181	call_callback(callback, Calibration::Result::Unsupported, {});	×
NEW 182	return;	×
183	}
184
UNCOV 185	std::lock_guard<std::mutex> lock(_calibration_mutex);	×
186
187	if (_system_impl->is_armed()) {	×
188	Calibration::ProgressData progress_data;	×
189	call_callback(callback, Calibration::Result::FailedArmed, progress_data);	×
190	return;	×
191	}	×
192
193	if (_state != State::None) {	×
194	Calibration::ProgressData progress_data;	×
195	call_callback(callback, Calibration::Result::Busy, progress_data);	×
196	return;	×
197	}	×
198
199	_state = State::AccelerometerCalibration;	×
200	_calibration_callback = callback;	×
201
202	MavlinkCommandSender::CommandLong command{};	×
203	command.command = MAV_CMD_PREFLIGHT_CALIBRATION;	×
204	command.params.maybe_param1 = 0.0f;	×
205	command.params.maybe_param2 = 0.0f;	×
206	command.params.maybe_param3 = 0.0f;	×
207	command.params.maybe_param4 = 0.0f;	×
208	command.params.maybe_param5 = 2.0f; // Board Level	×
209	command.target_component_id = MAV_COMP_ID_AUTOPILOT1;	×
210	_system_impl->send_command_async(	×
211	command, [this](MavlinkCommandSender::Result command_result, float progress) {	×
212	command_result_callback(command_result, progress);	×
213	});	×
214	}	×
215
216	void CalibrationImpl::calibrate_gimbal_accelerometer_async(const CalibrationCallback& callback)	×
217	{
218	// The calibration is currently implemented using a PX4 specific
219	// API based on statustext for progress and instructions.
NEW 220	if (_system_impl->effective_autopilot() != Autopilot::Px4) {	×
NEW 221	call_callback(callback, Calibration::Result::Unsupported, {});	×
NEW 222	return;	×
223	}
224
UNCOV 225	std::lock_guard<std::mutex> lock(_calibration_mutex);	×
226
227	if (_system_impl->is_armed()) {	×
228	Calibration::ProgressData progress_data;	×
229	call_callback(callback, Calibration::Result::FailedArmed, progress_data);	×
230	return;	×
231	}	×
232
233	if (_state != State::None) {	×
234	Calibration::ProgressData progress_data;	×
235	call_callback(callback, Calibration::Result::Busy, progress_data);	×
236	return;	×
237	}	×
238
239	_state = State::GimbalAccelerometerCalibration;	×
240	_calibration_callback = callback;	×
241
242	MavlinkCommandSender::CommandLong command{};	×
243	command.command = MAV_CMD_PREFLIGHT_CALIBRATION;	×
244	command.params.maybe_param1 = 0.0f;	×
245	command.params.maybe_param2 = 0.0f;	×
246	command.params.maybe_param3 = 0.0f;	×
247	command.params.maybe_param4 = 0.0f;	×
248	command.params.maybe_param5 = 1.0f; // Accel	×
249	command.target_component_id = MAV_COMP_ID_GIMBAL;	×
250	_system_impl->send_command_async(	×
251	command, [this](MavlinkCommandSender::Result command_result, float progress) {	×
252	command_result_callback(command_result, progress);	×
253	if (command_result == MavlinkCommandSender::Result::Success) {	×
254	// The gimbal which implements this so far actually uses acks
255	// with progress and final ack, so we need to finish at the end.
256	report_done();	×
257	}
258	});	×
259	}	×
260
261	Calibration::Result CalibrationImpl::cancel()	×
262	{
263	// The calibration is currently implemented using a PX4 specific
264	// API based on statustext for progress and instructions.
NEW 265	if (_system_impl->effective_autopilot() != Autopilot::Px4) {	×
NEW 266	return Calibration::Result::Unsupported;	×
267	}
268
UNCOV 269	std::lock_guard<std::mutex> lock(_calibration_mutex);	×
270
271	uint8_t target_component_id = MAV_COMP_ID_AUTOPILOT1;	×
272
273	switch (_state) {	×
274	case State::None:	×
275	LogWarn() << "No calibration to cancel";	×
276	return Calibration::Result::Success;	×
277	case State::GyroCalibration:	×
278	break;	×
279	case State::AccelerometerCalibration:	×
280	break;	×
281	case State::MagnetometerCalibration:	×
282	break;	×
283	case State::LevelHorizonCalibration:	×
284	break;	×
285	case State::GimbalAccelerometerCalibration:	×
286	target_component_id = MAV_COMP_ID_GIMBAL;	×
287	break;	×
288	}
289
290	MavlinkCommandSender::CommandLong command{};	×
291	command.command = MAV_CMD_PREFLIGHT_CALIBRATION;	×
292	// All params 0 signal cancellation of a calibration.
293	command.params.maybe_param1 = 0.0f;	×
294	command.params.maybe_param2 = 0.0f;	×
295	command.params.maybe_param3 = 0.0f;	×
296	command.params.maybe_param4 = 0.0f;	×
297	command.params.maybe_param5 = 0.0f;	×
298	command.params.maybe_param6 = 0.0f;	×
299	command.params.maybe_param7 = 0.0f;	×
300	command.target_component_id = target_component_id;	×
301
302	auto prom = std::promise<Calibration::Result>();	×
303	auto fut = prom.get_future();	×
304	_system_impl->send_command_async(	×
305	command, [&prom](MavlinkCommandSender::Result command_result, float) {	×
306	if (command_result != MavlinkCommandSender::Result::Success) {	×
307	prom.set_value(Calibration::Result::ConnectionError);	×
308	} else {
309	prom.set_value(Calibration::Result::Success);	×
310	}
311	});	×
312
313	return fut.get();	×
314	}	×
315
316	void CalibrationImpl::command_result_callback(	×
317	MavlinkCommandSender::Result command_result, float progress)
318	{
319	std::lock_guard<std::mutex> lock(_calibration_mutex);	×
320
321	if (_state == State::None) {	×
322	// It might be someone else like a ground station trying to do a
323	// calibration. We silently ignore it.
324	return;	×
325	}
326
327	// If we get a progress result here, it is the new interface and we can
328	// use the progress info. If we get an ack, we need to translate that to
329	// a first progress update, and then parse the statustexts for progress.
330	switch (command_result) {	×
331	case MavlinkCommandSender::Result::Success:	×
332	// Silently ignore.
333	break;	×
334
335	case MavlinkCommandSender::Result::NoSystem:	×
336	// FALLTHROUGH
337	case MavlinkCommandSender::Result::ConnectionError:
338	// FALLTHROUGH
339	case MavlinkCommandSender::Result::Busy:
340	// FALLTHROUGH
341	case MavlinkCommandSender::Result::Denied:
342	// FALLTHROUGH
343	case MavlinkCommandSender::Result::Unsupported:
344	// FALLTHROUGH
345	case MavlinkCommandSender::Result::Timeout:
346	// FALLTHROUGH
347	case MavlinkCommandSender::Result::TemporarilyRejected:
348	// FALLTHROUGH
349	case MavlinkCommandSender::Result::Failed:
350	// FALLTHROUGH
351	case MavlinkCommandSender::Result::Cancelled:
352	// FALLTHROUGH
353	case MavlinkCommandSender::Result::UnknownError: {
354	// Report all error cases.
355	const auto timeout_result = calibration_result_from_command_result(command_result);	×
356	call_callback(_calibration_callback, timeout_result, Calibration::ProgressData());	×
357	_calibration_callback = nullptr;	×
358	_state = State::None;	×
359	break;	×
360	}
361
362	case MavlinkCommandSender::Result::InProgress: {	×
363	const auto progress_result = calibration_result_from_command_result(command_result);	×
364	Calibration::ProgressData progress_data;	×
365	progress_data.has_progress = true;	×
366	progress_data.progress = progress;	×
367
368	call_callback(_calibration_callback, progress_result, progress_data);	×
369	break;	×
370	}	×
371	};
372	}	×
373
374	Calibration::Result
375	CalibrationImpl::calibration_result_from_command_result(MavlinkCommandSender::Result result)	×
376	{
377	switch (result) {	×
378	case MavlinkCommandSender::Result::Success:	×
379	return Calibration::Result::Success;	×
380	case MavlinkCommandSender::Result::NoSystem:	×
381	return Calibration::Result::NoSystem;	×
382	case MavlinkCommandSender::Result::ConnectionError:	×
383	return Calibration::Result::ConnectionError;	×
384	case MavlinkCommandSender::Result::Busy:	×
385	return Calibration::Result::Busy;	×
386	case MavlinkCommandSender::Result::Denied:	×
387	// Fallthrough
388	case MavlinkCommandSender::Result::TemporarilyRejected:
389	return Calibration::Result::CommandDenied;	×
390	case MavlinkCommandSender::Result::Failed:	×
391	return Calibration::Result::Failed;	×
392	case MavlinkCommandSender::Result::Cancelled:	×
393	return Calibration::Result::Cancelled;	×
394	case MavlinkCommandSender::Result::Timeout:	×
395	return Calibration::Result::Timeout;	×
396	case MavlinkCommandSender::Result::InProgress:	×
397	return Calibration::Result::Next;	×
398	case MavlinkCommandSender::Result::Unsupported:	×
399	return Calibration::Result::Unsupported;	×
400	default:	×
401	return Calibration::Result::Unknown;	×
402	}
403	}
404
405	void CalibrationImpl::receive_statustext(const MavlinkStatustextHandler::Statustext& statustext)	×
406	{
407	std::lock_guard<std::mutex> lock(_calibration_mutex);	×
408	if (_state == State::None) {	×
409	return;	×
410	}
411
412	_parser.reset();	×
413
414	_parser.parse(statustext.text);	×
415
416	switch (_parser.get_status()) {	×
417	case CalibrationStatustextParser::Status::None:	×
418	// Ignore it.
419	break;	×
420	case CalibrationStatustextParser::Status::Started:	×
421	report_started();	×
422	break;	×
423	case CalibrationStatustextParser::Status::Done:	×
424	report_done();	×
425	break;	×
426	case CalibrationStatustextParser::Status::Failed:	×
427	report_failed(_parser.get_failed_message());	×
428	break;	×
429	case CalibrationStatustextParser::Status::Cancelled:	×
430	report_cancelled();	×
431	break;	×
432	case CalibrationStatustextParser::Status::Progress:	×
433	report_progress(_parser.get_progress());	×
434	break;	×
435	case CalibrationStatustextParser::Status::Instruction:	×
436	report_instruction(_parser.get_instruction());	×
437	break;	×
438	}
439
440	// In case we succeed or fail we need to notify that params
441	// might have changed.
442	switch (_parser.get_status()) {	×
443	case CalibrationStatustextParser::Status::Done:	×
444	// FALLTHROUGH
445	case CalibrationStatustextParser::Status::Failed:
446	// FALLTHROUGH
447	case CalibrationStatustextParser::Status::Cancelled:
448	switch (_state) {	×
449	case State::None:	×
450	break;	×
451	case State::GyroCalibration:	×
452	_system_impl->param_changed("CAL_GYRO0_ID");	×
453	break;	×
454	case State::AccelerometerCalibration:	×
455	_system_impl->param_changed("CAL_ACC0_ID");	×
456	break;	×
457	case State::MagnetometerCalibration:	×
458	_system_impl->param_changed("CAL_MAG0_ID");	×
459	break;	×
460	case State::LevelHorizonCalibration:	×
461	_system_impl->param_changed("SENS_BOARD_X_OFF");	×
462	_system_impl->param_changed("SENS_BOARD_Y_OFF");	×
463	_system_impl->param_changed("SENS_BOARD_Z_OFF");	×
464	break;	×
465	case State::GimbalAccelerometerCalibration:	×
466	break;	×
467	}
468
469	default:
470	break;	×
471	}
472
473	switch (_parser.get_status()) {	×
474	case CalibrationStatustextParser::Status::Done:	×
475	// FALLTHROUGH
476	case CalibrationStatustextParser::Status::Failed:
477	// FALLTHROUGH
478	case CalibrationStatustextParser::Status::Cancelled:
479	_calibration_callback = nullptr;	×
480	_state = State::None;	×
481	break;	×
482	default:	×
483	break;	×
484	}
485	}	×
486
487	void CalibrationImpl::report_started()	×
488	{
489	report_progress(0.0f);	×
490	}	×
491
492	void CalibrationImpl::report_done()	×
493	{
494	const Calibration::ProgressData progress_data;	×
495	call_callback(_calibration_callback, Calibration::Result::Success, progress_data);	×
496	}	×
497
498	void CalibrationImpl::report_failed(const std::string& failed)	×
499	{
500	LogErr() << "Calibration failed: " << failed;	×
501	const Calibration::ProgressData progress_data;	×
502	call_callback(_calibration_callback, Calibration::Result::Failed, progress_data);	×
503	}	×
504
505	void CalibrationImpl::report_cancelled()	×
506	{
507	LogWarn() << "Calibration was cancelled";	×
508	const Calibration::ProgressData progress_data;	×
509	call_callback(_calibration_callback, Calibration::Result::Cancelled, progress_data);	×
510	}	×
511
512	void CalibrationImpl::report_progress(float progress)	×
513	{
514	Calibration::ProgressData progress_data;	×
515	progress_data.has_progress = true;	×
516	progress_data.progress = progress;	×
517	call_callback(_calibration_callback, Calibration::Result::Next, progress_data);	×
518	}	×
519
520	void CalibrationImpl::report_instruction(const std::string& instruction)	×
521	{
522	Calibration::ProgressData progress_data;	×
523	progress_data.has_status_text = true;	×
524	progress_data.status_text = instruction;	×
525	call_callback(_calibration_callback, Calibration::Result::Next, progress_data);	×
526	}	×
527
528	} // namespace mavsdk

mavlink / MAVSDK / 20604212380

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous