ArduSub: 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:36 +11:00 committed by Peter Barker
parent d2d89b02cc
commit ccc9231c67
5 changed files with 18 additions and 23 deletions

View File

@ -102,9 +102,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_RPM, &sub.rpm_sensor, update, 10, 200, 66), SCHED_TASK_CLASS(AP_RPM, &sub.rpm_sensor, update, 10, 200, 66),
#endif #endif
SCHED_TASK(terrain_update, 10, 100, 72), SCHED_TASK(terrain_update, 10, 100, 72),
#if AP_GRIPPER_ENABLED
SCHED_TASK_CLASS(AP_Gripper, &sub.g2.gripper, update, 10, 75, 75),
#endif
#if AP_STATS_ENABLED #if AP_STATS_ENABLED
SCHED_TASK(stats_update, 1, 200, 76), SCHED_TASK(stats_update, 1, 200, 76),
#endif #endif

View File

@ -1,5 +1,7 @@
#include "Sub.h" #include "Sub.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
@ -693,11 +695,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(proximity, "PRX", 2, ParametersG2, AP_Proximity), AP_SUBGROUPINFO(proximity, "PRX", 2, ParametersG2, AP_Proximity),
#endif #endif
#if AP_GRIPPER_ENABLED // 3 was AP_Gripper
// @Group: GRIP_
// @Path: ../libraries/AP_Gripper/AP_Gripper.cpp
AP_SUBGROUPINFO(gripper, "GRIP_", 3, ParametersG2, AP_Gripper),
#endif
// @Group: SERVO // @Group: SERVO
// @Path: ../libraries/SRV_Channel/SRV_Channels.cpp // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp
@ -791,6 +789,21 @@ void Sub::load_parameters()
} }
#endif #endif
// PARAMETER_CONVERSION - Added: Feb-2024
#if AP_GRIPPER_ENABLED
{
// Find G2's Top Level Key
AP_Param::ConversionInfo gripper_info;
if (!AP_Param::find_top_level_key_by_pointer(&g2, gripper_info.old_key)) {
return;
}
const uint16_t old_gripper_index = 3; // Old parameter index in g2
const uint16_t old_gripper_top_element = 4035; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP)
AP_Param::convert_class(gripper_info.old_key, &gripper, gripper.var_info, old_gripper_index, old_gripper_top_element, false);
}
#endif
// PARAMETER_CONVERSION - Added: Feb-2024 // PARAMETER_CONVERSION - Added: Feb-2024
#if HAL_LOGGING_ENABLED #if HAL_LOGGING_ENABLED
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);

View File

@ -4,7 +4,6 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Gripper/AP_Gripper.h>
#include <AP_Arming/AP_Arming.h> #include <AP_Arming/AP_Arming.h>
// Global parameter class. // Global parameter class.
@ -353,10 +352,6 @@ public:
// var_info for holding Parameter information // var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
#if AP_GRIPPER_ENABLED
AP_Gripper gripper;
#endif
#if HAL_PROXIMITY_ENABLED #if HAL_PROXIMITY_ENABLED
// proximity (aka object avoidance) library // proximity (aka object avoidance) library
AP_Proximity proximity; AP_Proximity proximity;

View File

@ -88,11 +88,6 @@
#include <AP_RPM/AP_RPM.h> #include <AP_RPM/AP_RPM.h>
#endif #endif
#include <AP_Gripper/AP_Gripper_config.h>
#if AP_GRIPPER_ENABLED
#include <AP_Gripper/AP_Gripper.h> // gripper stuff
#endif
#if AVOIDANCE_ENABLED == ENABLED #if AVOIDANCE_ENABLED == ENABLED
#include <AC_Avoidance/AC_Avoid.h> // Stop at fence library #include <AC_Avoidance/AC_Avoid.h> // Stop at fence library
#endif #endif

View File

@ -14,11 +14,6 @@ static void failsafe_check_static()
void Sub::init_ardupilot() void Sub::init_ardupilot()
{ {
// init cargo gripper
#if AP_GRIPPER_ENABLED
g2.gripper.init();
#endif
// initialise notify system // initialise notify system
notify.init(); notify.init();