ArduCopter: move gripper up to AP_Vehicle

also make the singleton return a reference rather than a pointer
This commit is contained in:
Peter Barker 2024-02-21 17:40:35 +11:00 committed by Peter Barker
parent f3b9304d80
commit f2a9f1516b
8 changed files with 25 additions and 33 deletions

View File

@ -236,9 +236,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
SCHED_TASK(terrain_update, 10, 100, 144), SCHED_TASK(terrain_update, 10, 100, 144),
#endif #endif
#if AP_GRIPPER_ENABLED
SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75, 147),
#endif
#if AP_WINCH_ENABLED #if AP_WINCH_ENABLED
SCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50, 150), SCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50, 150),
#endif #endif

View File

@ -110,10 +110,6 @@
#include <AC_WPNav/AC_WPNav_OA.h> #include <AC_WPNav/AC_WPNav_OA.h>
#include <AC_Avoidance/AP_OAPathPlanner.h> #include <AC_Avoidance/AP_OAPathPlanner.h>
#endif #endif
#include <AP_Gripper/AP_Gripper_config.h>
#if AP_GRIPPER_ENABLED
# include <AP_Gripper/AP_Gripper.h>
#endif
#if AC_PRECLAND_ENABLED #if AC_PRECLAND_ENABLED
# include <AC_PrecLand/AC_PrecLand.h> # include <AC_PrecLand/AC_PrecLand.h>
# include <AC_PrecLand/AC_PrecLand_StateMachine.h> # include <AC_PrecLand/AC_PrecLand_StateMachine.h>

View File

@ -1,5 +1,7 @@
#include "Copter.h" #include "Copter.h"
#include <AP_Gripper/AP_Gripper.h>
/* /*
This program is free software: you can redistribute it and/or modify This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
@ -817,11 +819,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// 12 was AP_Stats // 12 was AP_Stats
#if AP_GRIPPER_ENABLED // 13 was AP_Gripper
// @Group: GRIP_
// @Path: ../libraries/AP_Gripper/AP_Gripper.cpp
AP_SUBGROUPINFO(gripper, "GRIP_", 13, ParametersG2, AP_Gripper),
#endif
// @Param: FRAME_CLASS // @Param: FRAME_CLASS
// @DisplayName: Frame Class // @DisplayName: Frame Class
@ -1393,6 +1391,21 @@ void Copter::load_parameters(void)
AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true); AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true);
#endif #endif
// PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6
#if AP_GRIPPER_ENABLED
{
// Find G2's Top Level Key
AP_Param::ConversionInfo info;
if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) {
return;
}
const uint16_t old_index = 13; // Old parameter index in g2
const uint16_t old_top_element = 4045; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP)
AP_Param::convert_class(info.old_key, &gripper, gripper.var_info, old_index, old_top_element, false);
}
#endif
// setup AP_Param frame type flags // setup AP_Param frame type flags
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER); AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER);
} }

View File

@ -6,10 +6,6 @@
#include "RC_Channel.h" #include "RC_Channel.h"
#include <AP_Proximity/AP_Proximity.h> #include <AP_Proximity/AP_Proximity.h>
#include <AP_Gripper/AP_Gripper_config.h>
#if AP_GRIPPER_ENABLED
# include <AP_Gripper/AP_Gripper.h>
#endif
#if MODE_FOLLOW_ENABLED == ENABLED #if MODE_FOLLOW_ENABLED == ENABLED
# include <AP_Follow/AP_Follow.h> # include <AP_Follow/AP_Follow.h>
#endif #endif
@ -508,10 +504,6 @@ public:
AP_Button *button_ptr; AP_Button *button_ptr;
#endif #endif
#if AP_GRIPPER_ENABLED
AP_Gripper gripper;
#endif
#if MODE_THROW_ENABLED == ENABLED #if MODE_THROW_ENABLED == ENABLED
// Throw mode parameters // Throw mode parameters
AP_Int8 throw_nextmode; AP_Int8 throw_nextmode;

View File

@ -172,7 +172,7 @@ void Copter::thrust_loss_check()
#if AP_GRIPPER_ENABLED #if AP_GRIPPER_ENABLED
if ((copter.g2.flight_options & uint32_t(FlightOptions::RELEASE_GRIPPER_ON_THRUST_LOSS)) != 0) { if ((copter.g2.flight_options & uint32_t(FlightOptions::RELEASE_GRIPPER_ON_THRUST_LOSS)) != 0) {
copter.g2.gripper.release(); gripper.release();
} }
#endif #endif
} }

View File

@ -504,7 +504,7 @@ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){
#if AP_GRIPPER_ENABLED #if AP_GRIPPER_ENABLED
if (failsafe_option(FailsafeOption::RELEASE_GRIPPER)) { if (failsafe_option(FailsafeOption::RELEASE_GRIPPER)) {
copter.g2.gripper.release(); gripper.release();
} }
#endif #endif
} }

View File

@ -1235,8 +1235,7 @@ void PayloadPlace::run()
#if AP_GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED == ENABLED
// if pilot releases load manually: // if pilot releases load manually:
if (AP::gripper() != nullptr && if (AP::gripper().valid() && AP::gripper().released()) {
AP::gripper()->valid() && AP::gripper()->released()) {
switch (state) { switch (state) {
case State::FlyToLocation: case State::FlyToLocation:
case State::Descent_Start: case State::Descent_Start:
@ -1334,9 +1333,9 @@ void PayloadPlace::run()
// Reinitialise vertical position controller to remove discontinuity due to touch down of payload // Reinitialise vertical position controller to remove discontinuity due to touch down of payload
pos_control->init_z_controller_no_descent(); pos_control->init_z_controller_no_descent();
#if AP_GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED == ENABLED
if (g2.gripper.valid()) { if (AP::gripper().valid()) {
gcs().send_text(MAV_SEVERITY_INFO, "%s Releasing the gripper", prefix_str); gcs().send_text(MAV_SEVERITY_INFO, "%s Releasing the gripper", prefix_str);
g2.gripper.release(); AP::gripper().release();
state = State::Releasing; state = State::Releasing;
} else { } else {
state = State::Delay; state = State::Delay;
@ -1347,8 +1346,8 @@ void PayloadPlace::run()
break; break;
case State::Releasing: case State::Releasing:
#if AP_GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED
if (g2.gripper.valid() && !g2.gripper.released()) { if (AP::gripper().valid() && !AP::gripper().released()) {
break; break;
} }
#endif #endif

View File

@ -15,11 +15,6 @@ static void failsafe_check_static()
void Copter::init_ardupilot() void Copter::init_ardupilot()
{ {
// init cargo gripper
#if AP_GRIPPER_ENABLED
g2.gripper.init();
#endif
// init winch // init winch
#if AP_WINCH_ENABLED #if AP_WINCH_ENABLED
g2.winch.init(); g2.winch.init();