mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: move gripper up to AP_Vehicle
also make the singleton return a reference rather than a pointer
This commit is contained in:
parent
f3b9304d80
commit
f2a9f1516b
|
@ -236,9 +236,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
|
|||
#if AP_TERRAIN_AVAILABLE
|
||||
SCHED_TASK(terrain_update, 10, 100, 144),
|
||||
#endif
|
||||
#if AP_GRIPPER_ENABLED
|
||||
SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75, 147),
|
||||
#endif
|
||||
#if AP_WINCH_ENABLED
|
||||
SCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50, 150),
|
||||
#endif
|
||||
|
|
|
@ -110,10 +110,6 @@
|
|||
#include <AC_WPNav/AC_WPNav_OA.h>
|
||||
#include <AC_Avoidance/AP_OAPathPlanner.h>
|
||||
#endif
|
||||
#include <AP_Gripper/AP_Gripper_config.h>
|
||||
#if AP_GRIPPER_ENABLED
|
||||
# include <AP_Gripper/AP_Gripper.h>
|
||||
#endif
|
||||
#if AC_PRECLAND_ENABLED
|
||||
# include <AC_PrecLand/AC_PrecLand.h>
|
||||
# include <AC_PrecLand/AC_PrecLand_StateMachine.h>
|
||||
|
|
|
@ -1,5 +1,7 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#include <AP_Gripper/AP_Gripper.h>
|
||||
|
||||
/*
|
||||
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
|
||||
|
@ -817,11 +819,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
|
||||
// 12 was AP_Stats
|
||||
|
||||
#if AP_GRIPPER_ENABLED
|
||||
// @Group: GRIP_
|
||||
// @Path: ../libraries/AP_Gripper/AP_Gripper.cpp
|
||||
AP_SUBGROUPINFO(gripper, "GRIP_", 13, ParametersG2, AP_Gripper),
|
||||
#endif
|
||||
// 13 was AP_Gripper
|
||||
|
||||
// @Param: 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);
|
||||
#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
|
||||
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER);
|
||||
}
|
||||
|
|
|
@ -6,10 +6,6 @@
|
|||
#include "RC_Channel.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
|
||||
# include <AP_Follow/AP_Follow.h>
|
||||
#endif
|
||||
|
@ -508,10 +504,6 @@ public:
|
|||
AP_Button *button_ptr;
|
||||
#endif
|
||||
|
||||
#if AP_GRIPPER_ENABLED
|
||||
AP_Gripper gripper;
|
||||
#endif
|
||||
|
||||
#if MODE_THROW_ENABLED == ENABLED
|
||||
// Throw mode parameters
|
||||
AP_Int8 throw_nextmode;
|
||||
|
|
|
@ -172,7 +172,7 @@ void Copter::thrust_loss_check()
|
|||
|
||||
#if AP_GRIPPER_ENABLED
|
||||
if ((copter.g2.flight_options & uint32_t(FlightOptions::RELEASE_GRIPPER_ON_THRUST_LOSS)) != 0) {
|
||||
copter.g2.gripper.release();
|
||||
gripper.release();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -504,7 +504,7 @@ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){
|
|||
|
||||
#if AP_GRIPPER_ENABLED
|
||||
if (failsafe_option(FailsafeOption::RELEASE_GRIPPER)) {
|
||||
copter.g2.gripper.release();
|
||||
gripper.release();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -1235,8 +1235,7 @@ void PayloadPlace::run()
|
|||
|
||||
#if AP_GRIPPER_ENABLED == ENABLED
|
||||
// if pilot releases load manually:
|
||||
if (AP::gripper() != nullptr &&
|
||||
AP::gripper()->valid() && AP::gripper()->released()) {
|
||||
if (AP::gripper().valid() && AP::gripper().released()) {
|
||||
switch (state) {
|
||||
case State::FlyToLocation:
|
||||
case State::Descent_Start:
|
||||
|
@ -1334,9 +1333,9 @@ void PayloadPlace::run()
|
|||
// Reinitialise vertical position controller to remove discontinuity due to touch down of payload
|
||||
pos_control->init_z_controller_no_descent();
|
||||
#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);
|
||||
g2.gripper.release();
|
||||
AP::gripper().release();
|
||||
state = State::Releasing;
|
||||
} else {
|
||||
state = State::Delay;
|
||||
|
@ -1347,8 +1346,8 @@ void PayloadPlace::run()
|
|||
break;
|
||||
|
||||
case State::Releasing:
|
||||
#if AP_GRIPPER_ENABLED == ENABLED
|
||||
if (g2.gripper.valid() && !g2.gripper.released()) {
|
||||
#if AP_GRIPPER_ENABLED
|
||||
if (AP::gripper().valid() && !AP::gripper().released()) {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -15,11 +15,6 @@ static void failsafe_check_static()
|
|||
|
||||
void Copter::init_ardupilot()
|
||||
{
|
||||
// init cargo gripper
|
||||
#if AP_GRIPPER_ENABLED
|
||||
g2.gripper.init();
|
||||
#endif
|
||||
|
||||
// init winch
|
||||
#if AP_WINCH_ENABLED
|
||||
g2.winch.init();
|
||||
|
|
Loading…
Reference in New Issue