2015-05-12 01:58:56 -03:00
|
|
|
/*
|
|
|
|
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
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
2016-05-05 19:10:08 -03:00
|
|
|
/*
|
2015-05-12 01:58:56 -03:00
|
|
|
main Rover class, containing all vehicle specific state
|
|
|
|
*/
|
2016-02-17 21:25:59 -04:00
|
|
|
#pragma once
|
2015-05-13 00:16:45 -03:00
|
|
|
|
2016-03-31 18:43:36 -03:00
|
|
|
#include <cmath>
|
2015-05-12 04:00:25 -03:00
|
|
|
#include <stdarg.h>
|
2022-08-19 07:55:01 -03:00
|
|
|
#include <stdint.h>
|
2015-05-12 04:00:25 -03:00
|
|
|
|
|
|
|
// Libraries
|
2015-08-11 03:28:40 -03:00
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2017-08-11 22:28:00 -03:00
|
|
|
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
|
|
|
|
#include <AP_Camera/AP_Camera.h> // Camera triggering
|
|
|
|
#include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
|
|
|
|
#include <AP_Param/AP_Param.h>
|
2016-12-20 09:30:57 -04:00
|
|
|
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
|
2017-08-11 22:28:00 -03:00
|
|
|
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
|
2022-03-03 20:36:14 -04:00
|
|
|
#include <AP_RPM/AP_RPM.h> // RPM input library
|
2017-08-11 22:28:00 -03:00
|
|
|
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
|
2016-12-20 09:30:57 -04:00
|
|
|
#include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
|
2017-08-11 22:28:00 -03:00
|
|
|
#include <AP_WheelEncoder/AP_WheelEncoder.h>
|
2018-08-08 00:48:30 -03:00
|
|
|
#include <AP_WheelEncoder/AP_WheelRateControl.h>
|
2019-01-18 00:23:42 -04:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
2018-08-27 16:55:29 -03:00
|
|
|
#include <AP_OSD/AP_OSD.h>
|
2021-07-18 11:04:27 -03:00
|
|
|
#include <AR_Motors/AP_MotorsUGV.h>
|
2022-08-19 07:55:01 -03:00
|
|
|
#include <AP_Mission/AP_Mission.h>
|
|
|
|
#include <AP_Mission/AP_Mission_ChangeDetector.h>
|
|
|
|
#include <AR_WPNav/AR_WPNav_OA.h>
|
2022-12-14 19:31:11 -04:00
|
|
|
#include <AP_OpticalFlow/AP_OpticalFlow.h>
|
2023-03-22 05:45:41 -03:00
|
|
|
#include <AC_PrecLand/AC_PrecLand_config.h>
|
2022-08-19 07:55:01 -03:00
|
|
|
|
|
|
|
// Configuration
|
|
|
|
#include "defines.h"
|
|
|
|
#include "config.h"
|
2019-03-01 02:40:40 -04:00
|
|
|
|
2021-11-15 01:08:35 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2019-03-01 02:40:40 -04:00
|
|
|
#include <AP_Scripting/AP_Scripting.h>
|
|
|
|
#endif
|
|
|
|
|
2017-08-11 22:28:00 -03:00
|
|
|
// Local modules
|
2016-08-17 01:28:16 -03:00
|
|
|
#include "AP_Arming.h"
|
2019-05-07 15:21:02 -03:00
|
|
|
#include "sailboat.h"
|
2017-01-30 10:21:55 -04:00
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
|
|
|
#include "afs_rover.h"
|
|
|
|
#endif
|
2015-05-12 04:00:25 -03:00
|
|
|
#include "Parameters.h"
|
2016-05-27 10:04:55 -03:00
|
|
|
#include "GCS_Mavlink.h"
|
2017-02-13 07:00:19 -04:00
|
|
|
#include "GCS_Rover.h"
|
2018-08-29 21:34:39 -03:00
|
|
|
#include "AP_Rally.h"
|
2023-03-22 05:45:41 -03:00
|
|
|
#if AC_PRECLAND_ENABLED
|
2022-05-30 06:26:55 -03:00
|
|
|
#include <AC_PrecLand/AC_PrecLand.h>
|
|
|
|
#endif
|
2023-03-22 05:45:41 -03:00
|
|
|
#include "RC_Channel.h" // RC Channel Library
|
2015-05-12 04:00:25 -03:00
|
|
|
|
2022-08-19 07:55:01 -03:00
|
|
|
#include "mode.h"
|
|
|
|
|
2018-12-27 02:33:48 -04:00
|
|
|
class Rover : public AP_Vehicle {
|
2015-05-12 01:58:56 -03:00
|
|
|
public:
|
2016-05-27 10:04:55 -03:00
|
|
|
friend class GCS_MAVLINK_Rover;
|
2015-05-12 04:00:25 -03:00
|
|
|
friend class Parameters;
|
2017-01-06 06:31:10 -04:00
|
|
|
friend class ParametersG2;
|
2018-08-29 21:34:39 -03:00
|
|
|
friend class AP_Rally_Rover;
|
2016-08-17 01:28:16 -03:00
|
|
|
friend class AP_Arming_Rover;
|
2017-01-30 10:21:55 -04:00
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
|
|
|
friend class AP_AdvancedFailsafe_Rover;
|
|
|
|
#endif
|
2017-02-13 07:00:19 -04:00
|
|
|
friend class GCS_Rover;
|
2017-07-18 23:19:08 -03:00
|
|
|
friend class Mode;
|
2017-11-28 02:59:13 -04:00
|
|
|
friend class ModeAcro;
|
2017-07-18 23:19:08 -03:00
|
|
|
friend class ModeAuto;
|
|
|
|
friend class ModeGuided;
|
|
|
|
friend class ModeHold;
|
2018-04-28 03:31:31 -03:00
|
|
|
friend class ModeLoiter;
|
2017-07-18 23:19:08 -03:00
|
|
|
friend class ModeSteering;
|
|
|
|
friend class ModeManual;
|
|
|
|
friend class ModeRTL;
|
2017-11-29 21:58:11 -04:00
|
|
|
friend class ModeSmartRTL;
|
2018-05-24 01:47:07 -03:00
|
|
|
friend class ModeFollow;
|
2018-07-02 04:21:37 -03:00
|
|
|
friend class ModeSimple;
|
2022-07-10 01:02:55 -03:00
|
|
|
#if MODE_DOCK_ENABLED == ENABLED
|
|
|
|
friend class ModeDock;
|
|
|
|
#endif
|
2015-05-12 04:00:25 -03:00
|
|
|
|
2018-05-07 23:35:08 -03:00
|
|
|
friend class RC_Channel_Rover;
|
|
|
|
friend class RC_Channels_Rover;
|
|
|
|
|
2019-05-07 15:21:02 -03:00
|
|
|
friend class Sailboat;
|
|
|
|
|
2015-05-12 01:58:56 -03:00
|
|
|
Rover(void);
|
2015-05-12 04:00:25 -03:00
|
|
|
|
2015-05-12 01:58:56 -03:00
|
|
|
private:
|
2017-08-19 07:04:56 -03:00
|
|
|
|
2015-05-12 01:58:56 -03:00
|
|
|
// must be the first AP_Param variable declared to ensure its
|
|
|
|
// constructor runs before the constructors of the other AP_Param
|
|
|
|
// variables
|
|
|
|
AP_Param param_loader;
|
|
|
|
|
|
|
|
// all settable parameters
|
|
|
|
Parameters g;
|
2016-10-25 22:37:08 -03:00
|
|
|
ParametersG2 g2;
|
2015-05-12 01:58:56 -03:00
|
|
|
|
|
|
|
// mapping between input channels
|
2017-12-12 21:06:15 -04:00
|
|
|
RCMapper rcmap;
|
2015-05-12 01:58:56 -03:00
|
|
|
|
|
|
|
// primary control channels
|
|
|
|
RC_Channel *channel_steer;
|
|
|
|
RC_Channel *channel_throttle;
|
2018-05-10 04:10:34 -03:00
|
|
|
RC_Channel *channel_lateral;
|
2020-07-22 10:42:56 -03:00
|
|
|
RC_Channel *channel_roll;
|
|
|
|
RC_Channel *channel_pitch;
|
2020-08-28 15:51:26 -03:00
|
|
|
RC_Channel *channel_walking_height;
|
2015-05-12 01:58:56 -03:00
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
AP_Logger logger;
|
2015-05-12 01:58:56 -03:00
|
|
|
|
|
|
|
// flight modes convenience array
|
2016-12-20 09:30:57 -04:00
|
|
|
AP_Int8 *modes;
|
2018-05-07 23:35:08 -03:00
|
|
|
const uint8_t num_modes = 6;
|
2015-05-12 01:58:56 -03:00
|
|
|
|
2022-07-15 08:53:42 -03:00
|
|
|
#if AP_RPM_ENABLED
|
2018-12-30 16:43:39 -04:00
|
|
|
// AP_RPM Module
|
|
|
|
AP_RPM rpm_sensor;
|
2022-07-15 08:53:42 -03:00
|
|
|
#endif
|
2018-12-30 16:43:39 -04:00
|
|
|
|
2016-12-20 09:30:57 -04:00
|
|
|
// Arming/Disarming management class
|
2018-06-25 02:26:31 -03:00
|
|
|
AP_Arming_Rover arming;
|
2015-10-30 02:56:41 -03:00
|
|
|
|
2022-01-05 07:13:40 -04:00
|
|
|
#if AP_OPTICALFLOW_ENABLED
|
2022-08-14 22:31:15 -03:00
|
|
|
AP_OpticalFlow optflow;
|
2022-01-05 07:13:40 -04:00
|
|
|
#endif
|
2016-12-20 09:30:57 -04:00
|
|
|
|
2020-10-28 16:39:43 -03:00
|
|
|
#if OSD_ENABLED || OSD_PARAM_ENABLED
|
2018-08-27 16:55:29 -03:00
|
|
|
AP_OSD osd;
|
|
|
|
#endif
|
2023-03-22 05:45:41 -03:00
|
|
|
#if AC_PRECLAND_ENABLED
|
2022-05-30 06:26:55 -03:00
|
|
|
AC_PrecLand precland;
|
|
|
|
#endif
|
2015-05-12 01:58:56 -03:00
|
|
|
// GCS handling
|
2017-02-13 07:00:19 -04:00
|
|
|
GCS_Rover _gcs; // avoid using this; use gcs()
|
|
|
|
GCS_Rover &gcs() { return _gcs; }
|
2015-05-12 01:58:56 -03:00
|
|
|
|
2018-05-07 23:35:08 -03:00
|
|
|
// RC Channels:
|
|
|
|
RC_Channels_Rover &rc() { return g2.rc_channels; }
|
|
|
|
|
2017-08-29 15:49:08 -03:00
|
|
|
// The rover's current location
|
2023-02-02 18:58:40 -04:00
|
|
|
Location current_loc;
|
2017-08-29 15:49:08 -03:00
|
|
|
|
2015-05-12 01:58:56 -03:00
|
|
|
// Camera
|
2022-06-02 05:28:26 -03:00
|
|
|
#if AP_CAMERA_ENABLED
|
2019-03-26 08:08:39 -03:00
|
|
|
AP_Camera camera{MASK_LOG_CAMERA};
|
2015-05-12 01:58:56 -03:00
|
|
|
#endif
|
|
|
|
|
|
|
|
// Camera/Antenna mount tracking and stabilisation stuff
|
2020-07-24 14:23:45 -03:00
|
|
|
#if HAL_MOUNT_ENABLED
|
2019-03-26 08:36:50 -03:00
|
|
|
AP_Mount camera_mount;
|
2015-05-12 01:58:56 -03:00
|
|
|
#endif
|
|
|
|
|
2017-06-27 23:32:01 -03:00
|
|
|
// true if initialisation has completed
|
|
|
|
bool initialised;
|
|
|
|
|
2015-05-12 01:58:56 -03:00
|
|
|
// This is the state of the flight control system
|
2017-07-18 23:19:08 -03:00
|
|
|
// There are multiple states defined such as MANUAL, AUTO, ...
|
|
|
|
Mode *control_mode;
|
2015-05-12 01:58:56 -03:00
|
|
|
|
|
|
|
// Used to maintain the state of the previous control switch position
|
|
|
|
// This is set to -1 when we need to re-read the switch
|
|
|
|
uint8_t oldSwitchPosition;
|
|
|
|
|
2018-11-14 07:32:23 -04:00
|
|
|
// structure for holding failsafe state
|
2015-05-12 04:00:25 -03:00
|
|
|
struct {
|
2018-11-14 07:32:23 -04:00
|
|
|
uint8_t bits; // bit flags of failsafes that have started (but not necessarily triggered an action)
|
|
|
|
uint32_t start_time; // start time of the earliest failsafe
|
|
|
|
uint8_t triggered; // bit flags of failsafes that have triggered an action
|
|
|
|
uint32_t last_valid_rc_ms; // system time of most recent RC input from pilot
|
2018-11-01 04:04:58 -03:00
|
|
|
bool ekf;
|
2015-05-12 01:58:56 -03:00
|
|
|
} failsafe;
|
|
|
|
|
|
|
|
// true if we have a position estimate from AHRS
|
|
|
|
bool have_position;
|
|
|
|
|
2021-09-29 14:44:02 -03:00
|
|
|
// range finder last update for each instance (used for DPTH logging)
|
|
|
|
uint32_t rangefinder_last_reading_ms[RANGEFINDER_MAX_INSTANCES];
|
2018-08-27 04:03:43 -03:00
|
|
|
|
2015-05-12 01:58:56 -03:00
|
|
|
// Ground speed
|
|
|
|
// The amount current ground speed is below min ground speed. meters per second
|
2016-12-20 09:30:57 -04:00
|
|
|
float ground_speed;
|
2015-05-12 01:58:56 -03:00
|
|
|
|
|
|
|
// Battery Sensors
|
2018-03-01 23:30:40 -04:00
|
|
|
AP_BattMonitor battery{MASK_LOG_CURRENT,
|
|
|
|
FUNCTOR_BIND_MEMBER(&Rover::handle_battery_failsafe, void, const char*, const int8_t),
|
|
|
|
_failsafe_priorities};
|
2015-05-12 01:58:56 -03:00
|
|
|
|
2018-05-28 02:15:36 -03:00
|
|
|
// flyforward timer
|
|
|
|
uint32_t flyforward_start_ms;
|
2015-05-12 01:58:56 -03:00
|
|
|
|
|
|
|
static const AP_Scheduler::Task scheduler_tasks[];
|
|
|
|
|
2015-05-12 04:00:25 -03:00
|
|
|
static const AP_Param::Info var_info[];
|
2015-05-13 00:16:45 -03:00
|
|
|
static const LogStructure log_structure[];
|
2015-05-12 04:00:25 -03:00
|
|
|
|
2015-10-30 02:56:41 -03:00
|
|
|
// time that rudder/steering arming has been running
|
|
|
|
uint32_t rudder_arm_timer;
|
|
|
|
|
2019-10-17 08:03:18 -03:00
|
|
|
// latest wheel encoder values
|
|
|
|
float wheel_encoder_last_distance_m[WHEELENCODER_MAX_INSTANCES]; // total distance recorded by wheel encoder (for reporting to GCS)
|
|
|
|
bool wheel_encoder_initialised; // true once arrays below have been initialised to sensors initial values
|
2017-07-20 03:12:09 -03:00
|
|
|
float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES]; // distance in radians at time of last update to EKF
|
2019-10-17 08:03:18 -03:00
|
|
|
uint32_t wheel_encoder_last_reading_ms[WHEELENCODER_MAX_INSTANCES]; // system time of last ping from each encoder
|
|
|
|
uint8_t wheel_encoder_last_index_sent; // index of the last wheel encoder sent to the EKF
|
2017-07-20 03:12:09 -03:00
|
|
|
|
2017-07-14 23:59:28 -03:00
|
|
|
// True when we are doing motor test
|
|
|
|
bool motor_test;
|
|
|
|
|
2017-07-18 23:19:08 -03:00
|
|
|
ModeInitializing mode_initializing;
|
|
|
|
ModeHold mode_hold;
|
|
|
|
ModeManual mode_manual;
|
2017-11-28 02:59:13 -04:00
|
|
|
ModeAcro mode_acro;
|
2017-07-18 23:19:08 -03:00
|
|
|
ModeGuided mode_guided;
|
|
|
|
ModeAuto mode_auto;
|
2018-04-28 03:31:31 -03:00
|
|
|
ModeLoiter mode_loiter;
|
2017-07-18 23:19:08 -03:00
|
|
|
ModeSteering mode_steering;
|
|
|
|
ModeRTL mode_rtl;
|
2017-11-29 21:58:11 -04:00
|
|
|
ModeSmartRTL mode_smartrtl;
|
2018-05-24 01:47:07 -03:00
|
|
|
ModeFollow mode_follow;
|
2018-07-02 04:21:37 -03:00
|
|
|
ModeSimple mode_simple;
|
2022-07-10 01:02:55 -03:00
|
|
|
#if MODE_DOCK_ENABLED == ENABLED
|
|
|
|
ModeDock mode_dock;
|
|
|
|
#endif
|
2017-07-18 23:19:08 -03:00
|
|
|
|
2017-08-22 08:53:36 -03:00
|
|
|
// cruise throttle and speed learning
|
2018-12-14 13:54:31 -04:00
|
|
|
typedef struct {
|
2021-06-06 05:41:49 -03:00
|
|
|
LowPassFilterFloat speed_filt{2.0f};
|
|
|
|
LowPassFilterFloat throttle_filt{2.0f};
|
2018-12-14 13:54:31 -04:00
|
|
|
uint32_t learn_start_ms;
|
|
|
|
uint32_t log_count;
|
|
|
|
} cruise_learn_t;
|
|
|
|
cruise_learn_t cruise_learn;
|
2017-08-22 08:53:36 -03:00
|
|
|
|
2015-05-12 01:58:56 -03:00
|
|
|
private:
|
2017-08-11 23:05:45 -03:00
|
|
|
|
2020-02-18 00:11:45 -04:00
|
|
|
// Rover.cpp
|
2021-11-15 01:08:35 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2020-02-18 00:11:45 -04:00
|
|
|
bool set_target_location(const Location& target_loc) override;
|
2020-03-09 00:12:45 -03:00
|
|
|
bool set_target_velocity_NED(const Vector3f& vel_ned) override;
|
2020-06-15 04:29:00 -03:00
|
|
|
bool set_steering_and_throttle(float steering, float throttle) override;
|
2022-01-16 06:08:34 -04:00
|
|
|
// set desired turn rate (degrees/sec) and speed (m/s). Used for scripting
|
|
|
|
bool set_desired_turn_rate_and_speed(float turn_rate, float speed) override;
|
2022-06-30 15:45:14 -03:00
|
|
|
bool set_desired_speed(float speed) override;
|
2020-08-07 20:05:11 -03:00
|
|
|
bool get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) override;
|
2022-02-19 01:36:35 -04:00
|
|
|
bool nav_scripting_enable(uint8_t mode) override;
|
2022-10-13 21:37:52 -03:00
|
|
|
bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override;
|
2022-02-19 01:36:35 -04:00
|
|
|
void nav_script_time_done(uint16_t id) override;
|
2021-11-15 01:08:35 -04:00
|
|
|
#endif // AP_SCRIPTING_ENABLED
|
2017-08-11 23:05:45 -03:00
|
|
|
void stats_update();
|
2015-05-12 01:58:56 -03:00
|
|
|
void ahrs_update();
|
|
|
|
void gcs_failsafe_check(void);
|
|
|
|
void update_logging1(void);
|
|
|
|
void update_logging2(void);
|
|
|
|
void one_second_loop(void);
|
|
|
|
void update_current_mode(void);
|
2017-08-11 23:05:45 -03:00
|
|
|
|
2018-08-04 04:25:07 -03:00
|
|
|
// balance_bot.cpp
|
|
|
|
void balancebot_pitch_control(float &throttle);
|
|
|
|
bool is_balancebot() const;
|
|
|
|
|
2017-08-11 23:05:45 -03:00
|
|
|
// commands.cpp
|
2018-05-29 21:52:57 -03:00
|
|
|
bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;
|
|
|
|
bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;
|
2017-08-11 23:05:45 -03:00
|
|
|
void update_home();
|
|
|
|
|
|
|
|
// crash_check.cpp
|
|
|
|
void crash_check();
|
|
|
|
|
2017-08-22 08:53:36 -03:00
|
|
|
// cruise_learn.cpp
|
|
|
|
void cruise_learn_start();
|
|
|
|
void cruise_learn_update();
|
|
|
|
void cruise_learn_complete();
|
2021-02-01 12:26:24 -04:00
|
|
|
void log_write_cruise_learn() const;
|
2017-08-22 08:53:36 -03:00
|
|
|
|
2018-11-01 04:04:58 -03:00
|
|
|
// ekf_check.cpp
|
|
|
|
void ekf_check();
|
|
|
|
bool ekf_over_threshold();
|
|
|
|
bool ekf_position_ok();
|
|
|
|
void failsafe_ekf_event();
|
|
|
|
void failsafe_ekf_off_event(void);
|
|
|
|
|
2017-08-11 23:05:45 -03:00
|
|
|
// failsafe.cpp
|
2020-02-01 11:29:53 -04:00
|
|
|
void failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool on);
|
2018-03-01 23:30:40 -04:00
|
|
|
void handle_battery_failsafe(const char* type_str, const int8_t action);
|
2017-08-11 23:05:45 -03:00
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
|
|
|
void afs_fs_check(void);
|
|
|
|
#endif
|
|
|
|
|
2017-08-16 07:02:56 -03:00
|
|
|
// fence.cpp
|
|
|
|
void fence_check();
|
|
|
|
|
2017-08-11 23:05:45 -03:00
|
|
|
// GCS_Mavlink.cpp
|
2019-01-25 22:14:00 -04:00
|
|
|
void send_wheel_encoder_distance(mavlink_channel_t chan);
|
2015-08-06 09:50:52 -03:00
|
|
|
|
2017-08-11 23:05:45 -03:00
|
|
|
// Log.cpp
|
2018-06-11 08:10:32 -03:00
|
|
|
void Log_Write_Attitude();
|
2018-06-11 08:10:53 -03:00
|
|
|
void Log_Write_Depth();
|
2018-06-11 08:10:32 -03:00
|
|
|
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
|
|
|
|
void Log_Write_Nav_Tuning();
|
2018-09-25 10:09:47 -03:00
|
|
|
void Log_Write_Sail();
|
2018-06-11 08:10:32 -03:00
|
|
|
void Log_Write_Steering();
|
2017-12-07 08:42:06 -04:00
|
|
|
void Log_Write_Throttle();
|
2015-05-12 01:58:56 -03:00
|
|
|
void Log_Write_RC(void);
|
2018-06-11 08:10:32 -03:00
|
|
|
void Log_Write_Vehicle_Startup_Messages();
|
2017-08-11 23:05:45 -03:00
|
|
|
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
|
2015-05-13 00:16:45 -03:00
|
|
|
void log_init(void);
|
2015-08-06 09:50:52 -03:00
|
|
|
|
2019-01-08 10:06:56 -04:00
|
|
|
// mode.cpp
|
|
|
|
Mode *mode_from_mode_num(enum Mode::Number num);
|
|
|
|
|
2017-08-11 23:05:45 -03:00
|
|
|
// Parameters.cpp
|
2020-01-16 06:40:52 -04:00
|
|
|
void load_parameters(void) override;
|
2017-08-11 23:05:45 -03:00
|
|
|
|
2022-05-30 06:26:55 -03:00
|
|
|
// precision_landing.cpp
|
|
|
|
void init_precland();
|
|
|
|
void update_precland();
|
|
|
|
|
2017-08-11 23:05:45 -03:00
|
|
|
// radio.cpp
|
2020-02-12 17:47:11 -04:00
|
|
|
void set_control_channels(void) override;
|
2015-05-12 01:58:56 -03:00
|
|
|
void init_rc_in();
|
2017-08-11 23:05:45 -03:00
|
|
|
void rudder_arm_disarm_check();
|
2015-05-12 01:58:56 -03:00
|
|
|
void read_radio();
|
2018-10-31 02:26:19 -03:00
|
|
|
void radio_failsafe_check(uint16_t pwm);
|
2017-08-11 23:05:45 -03:00
|
|
|
|
|
|
|
// sensors.cpp
|
2019-03-15 22:10:33 -03:00
|
|
|
void update_compass(void);
|
|
|
|
void compass_save(void);
|
2017-07-11 23:02:51 -03:00
|
|
|
void update_wheel_encoder();
|
2017-07-13 08:36:44 -03:00
|
|
|
void read_rangefinders(void);
|
2017-08-11 23:05:45 -03:00
|
|
|
|
|
|
|
// Steering.cpp
|
|
|
|
void set_servos(void);
|
|
|
|
|
2020-01-16 06:12:17 -04:00
|
|
|
// Rover.cpp
|
|
|
|
void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
|
|
|
|
uint8_t &task_count,
|
|
|
|
uint32_t &log_bit) override;
|
|
|
|
|
2017-08-11 23:05:45 -03:00
|
|
|
// system.cpp
|
2020-01-16 06:12:17 -04:00
|
|
|
void init_ardupilot() override;
|
2015-05-12 01:58:56 -03:00
|
|
|
void startup_ground(void);
|
2018-05-28 02:15:36 -03:00
|
|
|
void update_ahrs_flyforward();
|
2019-10-17 00:48:47 -03:00
|
|
|
bool set_mode(Mode &new_mode, ModeReason reason);
|
|
|
|
bool set_mode(const uint8_t new_mode, ModeReason reason) override;
|
2020-01-09 10:46:23 -04:00
|
|
|
uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); }
|
2023-04-12 02:23:26 -03:00
|
|
|
bool current_mode_requires_mission() const override {
|
|
|
|
return control_mode == &mode_auto;
|
|
|
|
}
|
|
|
|
|
2015-05-12 01:58:56 -03:00
|
|
|
void startup_INS_ground(void);
|
2017-11-28 22:35:33 -04:00
|
|
|
void notify_mode(const Mode *new_mode);
|
2015-05-12 01:58:56 -03:00
|
|
|
uint8_t check_digital_pin(uint8_t pin);
|
|
|
|
bool should_log(uint32_t mask);
|
2017-12-06 22:37:42 -04:00
|
|
|
bool is_boat() const;
|
2017-08-11 23:05:45 -03:00
|
|
|
|
2020-12-05 14:45:11 -04:00
|
|
|
// vehicle specific waypoint info helpers
|
|
|
|
bool get_wp_distance_m(float &distance) const override;
|
|
|
|
bool get_wp_bearing_deg(float &bearing) const override;
|
|
|
|
bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
|
2019-09-21 16:49:31 -03:00
|
|
|
|
2018-03-01 23:30:40 -04:00
|
|
|
enum Failsafe_Action {
|
|
|
|
Failsafe_Action_None = 0,
|
|
|
|
Failsafe_Action_RTL = 1,
|
|
|
|
Failsafe_Action_Hold = 2,
|
|
|
|
Failsafe_Action_SmartRTL = 3,
|
|
|
|
Failsafe_Action_SmartRTL_Hold = 4,
|
|
|
|
Failsafe_Action_Terminate = 5
|
|
|
|
};
|
|
|
|
|
2019-09-29 23:32:07 -03:00
|
|
|
enum class Failsafe_Options : uint32_t {
|
|
|
|
Failsafe_Option_Active_In_Hold = (1<<0)
|
|
|
|
};
|
|
|
|
|
2018-03-01 23:30:40 -04:00
|
|
|
static constexpr int8_t _failsafe_priorities[] = {
|
|
|
|
Failsafe_Action_Terminate,
|
|
|
|
Failsafe_Action_Hold,
|
|
|
|
Failsafe_Action_RTL,
|
|
|
|
Failsafe_Action_SmartRTL_Hold,
|
|
|
|
Failsafe_Action_SmartRTL,
|
|
|
|
Failsafe_Action_None,
|
|
|
|
-1 // the priority list must end with a sentinel of -1
|
|
|
|
};
|
|
|
|
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
|
|
|
|
"_failsafe_priorities is missing the sentinel");
|
|
|
|
|
|
|
|
|
2015-05-12 04:00:25 -03:00
|
|
|
public:
|
|
|
|
void failsafe_check();
|
2017-07-14 23:59:28 -03:00
|
|
|
// Motor test
|
|
|
|
void motor_test_output();
|
2019-08-17 21:05:10 -03:00
|
|
|
bool mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc, AP_MotorsUGV::motor_test_order motor_instance, uint8_t throttle_type, int16_t throttle_value);
|
|
|
|
MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, AP_MotorsUGV::motor_test_order motor_instance, uint8_t throttle_type, int16_t throttle_value, float timeout_sec);
|
2017-07-14 23:59:28 -03:00
|
|
|
void motor_test_stop();
|
2018-05-31 06:26:07 -03:00
|
|
|
|
|
|
|
// frame type
|
2021-02-01 12:26:24 -04:00
|
|
|
uint8_t get_frame_type() const { return g2.frame_type.get(); }
|
2018-08-08 00:48:30 -03:00
|
|
|
AP_WheelRateControl& get_wheel_rate_control() { return g2.wheel_rate_control; }
|
2018-07-02 04:21:37 -03:00
|
|
|
|
|
|
|
// Simple mode
|
|
|
|
float simple_sin_yaw;
|
2015-05-12 01:58:56 -03:00
|
|
|
};
|
2015-05-12 04:00:25 -03:00
|
|
|
|
2015-05-13 00:16:45 -03:00
|
|
|
extern Rover rover;
|
|
|
|
|
2015-11-19 23:04:16 -04:00
|
|
|
using AP_HAL::millis;
|
|
|
|
using AP_HAL::micros;
|