7965506471

Committed 19 Feb 2024 09:51PM CUT coverage: 36.22% (+0.008%) from 36.212%

Build # 7965506471

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2223 from mavlink/pr-absl-fix

Fix illegal instruction on RPi 4

Run Details

10035 of 27706 relevant lines covered (36.22%)

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

mavlink / MAVSDK / 7965506471

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