Rover: include precision landing library in rover

This compiles rover with precision landing library included
This commit is contained in:
Shiv Tyagi 2022-05-30 14:56:55 +05:30 committed by Randy Mackay
parent 7c6ec00e11
commit 82d1750e8c
8 changed files with 53 additions and 1 deletions

View File

@ -306,6 +306,12 @@ const AP_Param::Info Rover::var_info[] = {
GOBJECT(camera, "CAM_", AP_Camera), GOBJECT(camera, "CAM_", AP_Camera),
#endif #endif
#if PRECISION_LANDING == ENABLED
// @Group: PLND_
// @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp
GOBJECT(precland, "PLND_", AC_PrecLand),
#endif
#if HAL_MOUNT_ENABLED #if HAL_MOUNT_ENABLED
// @Group: MNT // @Group: MNT
// @Path: ../libraries/AP_Mount/AP_Mount.cpp // @Path: ../libraries/AP_Mount/AP_Mount.cpp

View File

@ -55,6 +55,8 @@ public:
k_param_battery_volt_pin, k_param_battery_volt_pin,
k_param_battery_curr_pin, k_param_battery_curr_pin,
k_param_precland = 24,
// braking // braking
k_param_braking_percent_old = 30, // unused k_param_braking_percent_old = 30, // unused
k_param_braking_speederr_old, // unused k_param_braking_speederr_old, // unused

View File

@ -94,6 +94,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events, 50, 200, 66), SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events, 50, 200, 66),
#if GRIPPER_ENABLED == ENABLED #if GRIPPER_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Gripper, &rover.g2.gripper, update, 10, 75, 69), SCHED_TASK_CLASS(AP_Gripper, &rover.g2.gripper, update, 10, 75, 69),
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50, 70),
#endif
#endif #endif
SCHED_TASK_CLASS(AP_RPM, &rover.rpm_sensor, update, 10, 100, 72), SCHED_TASK_CLASS(AP_RPM, &rover.rpm_sensor, update, 10, 100, 72),
#if HAL_MOUNT_ENABLED #if HAL_MOUNT_ENABLED

View File

@ -61,6 +61,9 @@
#include "GCS_Rover.h" #include "GCS_Rover.h"
#include "AP_Rally.h" #include "AP_Rally.h"
#include "RC_Channel.h" // RC Channel Library #include "RC_Channel.h" // RC Channel Library
#if PRECISION_LANDING == ENABLED
#include <AC_PrecLand/AC_PrecLand.h>
#endif
#include "mode.h" #include "mode.h"
@ -136,7 +139,9 @@ private:
#if OSD_ENABLED || OSD_PARAM_ENABLED #if OSD_ENABLED || OSD_PARAM_ENABLED
AP_OSD osd; AP_OSD osd;
#endif #endif
#if PRECISION_LANDING == ENABLED
AC_PrecLand precland;
#endif
// GCS handling // GCS handling
GCS_Rover _gcs; // avoid using this; use gcs() GCS_Rover _gcs; // avoid using this; use gcs()
GCS_Rover &gcs() { return _gcs; } GCS_Rover &gcs() { return _gcs; }
@ -315,6 +320,10 @@ private:
// Parameters.cpp // Parameters.cpp
void load_parameters(void) override; void load_parameters(void) override;
// precision_landing.cpp
void init_precland();
void update_precland();
// radio.cpp // radio.cpp
void set_control_channels(void) override; void set_control_channels(void) override;
void init_rc_in(); void init_rc_in();

View File

@ -52,6 +52,12 @@
#define AP_RALLY ENABLED #define AP_RALLY ENABLED
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// Precision Landing with companion computer or IRLock sensor
#ifndef PRECISION_LANDING
# define PRECISION_LANDING ENABLED
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// NAVL1 // NAVL1
// //

View File

@ -0,0 +1,19 @@
//
// functions to support precision landing
//
#include "Rover.h"
#if PRECISION_LANDING == ENABLED
void Rover::init_precland()
{
rover.precland.init(400);
}
void Rover::update_precland()
{
// alt will be unused if we pass false through as the second parameter:
return precland.update(0, false);
}
#endif

View File

@ -105,6 +105,11 @@ void Rover::init_ardupilot()
camera_mount.init(); camera_mount.init();
#endif #endif
#if PRECISION_LANDING == ENABLED
// initialise precision landing
init_precland();
#endif
/* /*
setup the 'main loop is dead' check. Note that this relies on setup the 'main loop is dead' check. Note that this relies on
the RC library being initialised. the RC library being initialised.

View File

@ -25,6 +25,8 @@ def build(bld):
'AP_WindVane', 'AP_WindVane',
'AR_Motors', 'AR_Motors',
'AP_Torqeedo', 'AP_Torqeedo',
'AC_PrecLand',
'AP_IRLock',
], ],
) )