fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 17:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: remove RC trim calibration

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-04-03 08:52:10 +02:00
parent 8ceeda730d
commit 9dfb761504
6 changed files with 3 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/CMakeLists.txt
-1
View File
@@ -57,7 +57,6 @@ px4_add_module(
level_calibration.cpp
lm_fit.cpp
mag_calibration.cpp
rc_calibration.cpp
Safety.cpp
UserModeIntention.cpp
worker_thread.cpp
src/modules/commander/Commander.cpp
+3 -2
View File
@@ -1284,8 +1284,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
mavlink_log_critical(&_mavlink_log_pub, "RC trim calibration is deprecated\t");
events::send(events::ID("commander_rc_trim_cal_denied"), events::Log::Critical,
"RC trim calibration is deprecated");
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
src/modules/commander/rc_calibration.cpp
-110
View File
@@ -1,110 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rc_calibration.cpp
* Remote Control calibration routine
*/
#include <px4_platform_common/events.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <px4_platform_common/defines.h>
#include "rc_calibration.h"
#include "commander_helper.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
#include <systemlib/err.h>
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
{
uORB::Subscription manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
px4_usleep(400000);
manual_control_setpoint_s manual_control_setpoint{};
bool changed = manual_control_setpoint_sub.updated();
if (!changed) {
mavlink_log_critical(mavlink_log_pub, "no inputs, aborting\t");
events::send(events::ID("commander_cal_no_inputs"), {events::Log::Error, events::LogInternal::Info},
"No inputs, aborting RC trim calibration");
return PX4_ERROR;
}
manual_control_setpoint_sub.copy(&manual_control_setpoint);
/* load trim values which are active */
float roll_trim_active;
param_get(param_find("TRIM_ROLL"), &roll_trim_active);
float pitch_trim_active;
param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
float yaw_trim_active;
param_get(param_find("TRIM_YAW"), &yaw_trim_active);
/* get manual control scale values */
float roll_scale;
param_get(param_find("FW_MAN_R_SC"), &roll_scale);
float pitch_scale;
param_get(param_find("FW_MAN_P_SC"), &pitch_scale);
float yaw_scale;
param_get(param_find("FW_MAN_Y_SC"), &yaw_scale);
/* set parameters: the new trim values are the combination of active trim values
and the values coming from the remote control of the user
*/
float p = manual_control_setpoint.roll * roll_scale + roll_trim_active;
int p1r = param_set(param_find("TRIM_ROLL"), &p);
/*
we explicitly swap sign here because the trim is added to the actuator controls
which are moving in an inverse sense to manual pitch inputs
*/
p = -manual_control_setpoint.pitch * pitch_scale + pitch_trim_active;
int p2r = param_set(param_find("TRIM_PITCH"), &p);
p = manual_control_setpoint.yaw * yaw_scale + yaw_trim_active;
int p3r = param_set(param_find("TRIM_YAW"), &p);
if (p1r != 0 || p2r != 0 || p3r != 0) {
mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL\t");
events::send(events::ID("commander_cal_trim_param_set_failed"), events::Log::Critical,
"RC trim calibration: failed to set parameters");
return PX4_ERROR;
}
mavlink_log_info(mavlink_log_pub, "trim cal done\t");
events::send(events::ID("commander_cal_trim_done"), events::Log::Info, "RC trim calibration completed");
return PX4_OK;
}
src/modules/commander/rc_calibration.h
-47
View File
@@ -1,47 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rc_calibration.h
* Remote Control calibration routine
*/
#ifndef RC_CALIBRATION_H_
#define RC_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_trim_calibration(orb_advert_t *mavlink_log_pub);
#endif /* RC_CALIBRATION_H_ */
src/modules/commander/worker_thread.cpp
-5
View File
@@ -40,7 +40,6 @@
#include "gyro_calibration.h"
#include "level_calibration.h"
#include "mag_calibration.h"
#include "rc_calibration.h"
#include <px4_platform_common/events.h>
#include <px4_platform_common/log.h>
@@ -115,10 +114,6 @@ void WorkerThread::threadEntry()
_ret_value = do_mag_calibration(&_mavlink_log_pub);
break;
case Request::RCTrimCalibration:
_ret_value = do_trim_calibration(&_mavlink_log_pub);
break;
case Request::AccelCalibration:
_ret_value = do_accel_calibration(&_mavlink_log_pub);
break;
src/modules/commander/worker_thread.hpp
-1
View File
@@ -51,7 +51,6 @@ public:
enum class Request {
GyroCalibration,
MagCalibration,
RCTrimCalibration,
AccelCalibration,
LevelCalibration,
AccelCalibrationQuick,