21342096616

Committed 26 Jan 2026 12:15AM UTC coverage: 49.09% (-0.1%) from 49.23%

Build # 21342096616

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2746 from mavlink/pr-autopilot-cleanup

Add MAVLink pure mode and config/component cleanup

Run Details

58 of 197 new or added lines in 19 files covered. (29.44%)

43 existing lines in 13 files now uncovered.

18310 of 37299 relevant lines covered (49.09%)

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

mavlink / MAVSDK / 21342096616

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