2013-08-26 03:52:24 -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/>.
|
|
|
|
*/
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2015-05-24 19:59:16 -03:00
|
|
|
/*
|
2020-03-26 20:14:17 -03:00
|
|
|
This is the ArduRover firmware. It was originally derived from
|
2013-08-26 03:52:24 -03:00
|
|
|
ArduPlane by Jean-Louis Naudin (JLN), and then rewritten after the
|
|
|
|
AP_HAL merge by Andrew Tridgell
|
|
|
|
|
2019-08-06 05:11:20 -03:00
|
|
|
Maintainer: Randy Mackay, Grant Morphett
|
2013-02-28 21:32:48 -04:00
|
|
|
|
2015-08-05 23:32:42 -03:00
|
|
|
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin, Grant Morphett
|
2013-02-28 21:32:48 -04:00
|
|
|
|
2019-09-21 16:49:31 -03:00
|
|
|
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier
|
2012-05-14 12:47:08 -03:00
|
|
|
|
2015-05-24 19:59:16 -03:00
|
|
|
APMrover alpha version tester: Franco Borasio, Daniel Chapelat...
|
2013-08-26 03:52:24 -03:00
|
|
|
|
2021-05-26 23:33:17 -03:00
|
|
|
Please contribute your ideas! See https://ardupilot.org/dev for details
|
2013-08-26 03:52:24 -03:00
|
|
|
*/
|
2013-02-28 21:32:48 -04:00
|
|
|
|
2015-05-12 02:03:23 -03:00
|
|
|
#include "Rover.h"
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2019-07-10 21:05:55 -03:00
|
|
|
#define FORCE_VERSION_H_INCLUDE
|
|
|
|
#include "version.h"
|
|
|
|
#undef FORCE_VERSION_H_INCLUDE
|
2012-11-21 02:25:11 -04:00
|
|
|
|
2018-12-27 02:35:13 -04:00
|
|
|
#include "AP_Gripper/AP_Gripper.h"
|
|
|
|
|
2019-07-10 21:05:55 -03:00
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
2013-06-03 21:37:05 -03:00
|
|
|
|
2021-11-13 23:49:45 -04:00
|
|
|
#define SCHED_TASK(func, _interval_ticks, _max_time_micros, _priority) SCHED_TASK_CLASS(Rover, &rover, func, _interval_ticks, _max_time_micros, _priority)
|
2015-05-12 04:00:25 -03:00
|
|
|
|
|
|
|
/*
|
2021-11-13 23:49:45 -04:00
|
|
|
scheduler table - all regular tasks should be listed here.
|
|
|
|
|
|
|
|
All entries in this table must be ordered by priority.
|
|
|
|
|
|
|
|
This table is interleaved with the table in AP_Vehicle to determine
|
|
|
|
the order in which tasks are run. Convenience methods SCHED_TASK
|
|
|
|
and SCHED_TASK_CLASS are provided to build entries in this structure:
|
|
|
|
|
|
|
|
SCHED_TASK arguments:
|
|
|
|
- name of static function to call
|
|
|
|
- rate (in Hertz) at which the function should be called
|
|
|
|
- expected time (in MicroSeconds) that the function should take to run
|
|
|
|
- priority (0 through 255, lower number meaning higher priority)
|
|
|
|
|
|
|
|
SCHED_TASK_CLASS arguments:
|
|
|
|
- class name of method to be called
|
|
|
|
- instance on which to call the method
|
|
|
|
- method to call on that instance
|
|
|
|
- rate (in Hertz) at which the method should be called
|
|
|
|
- expected time (in MicroSeconds) that the method should take to run
|
|
|
|
- priority (0 through 255, lower number meaning higher priority)
|
|
|
|
|
2018-04-02 08:47:34 -03:00
|
|
|
scheduler table - all regular tasks are listed here, along with how
|
|
|
|
often they should be called (in Hz) and the maximum time
|
|
|
|
they are expected to take (in microseconds)
|
|
|
|
*/
|
2015-10-25 14:03:46 -03:00
|
|
|
const AP_Scheduler::Task Rover::scheduler_tasks[] = {
|
2017-06-19 12:55:14 -03:00
|
|
|
// Function name, Hz, us,
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK(read_radio, 50, 200, 3),
|
|
|
|
SCHED_TASK(ahrs_update, 400, 400, 6),
|
|
|
|
SCHED_TASK(read_rangefinders, 50, 200, 9),
|
2022-09-07 03:49:51 -03:00
|
|
|
#if AP_OPTICALFLOW_ENABLED
|
|
|
|
SCHED_TASK_CLASS(AP_OpticalFlow, &rover.optflow, update, 200, 160, 11),
|
|
|
|
#endif
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK(update_current_mode, 400, 200, 12),
|
|
|
|
SCHED_TASK(set_servos, 400, 200, 15),
|
|
|
|
SCHED_TASK_CLASS(AP_GPS, &rover.gps, update, 50, 300, 18),
|
|
|
|
SCHED_TASK_CLASS(AP_Baro, &rover.barometer, update, 10, 200, 21),
|
|
|
|
SCHED_TASK_CLASS(AP_Beacon, &rover.g2.beacon, update, 50, 200, 24),
|
2021-03-25 05:47:29 -03:00
|
|
|
#if HAL_PROXIMITY_ENABLED
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 50, 200, 27),
|
2021-03-25 05:47:29 -03:00
|
|
|
#endif
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK_CLASS(AP_WindVane, &rover.g2.windvane, update, 20, 100, 30),
|
|
|
|
SCHED_TASK(update_wheel_encoder, 50, 200, 36),
|
|
|
|
SCHED_TASK(update_compass, 10, 200, 39),
|
|
|
|
SCHED_TASK(update_logging1, 10, 200, 45),
|
|
|
|
SCHED_TASK(update_logging2, 10, 200, 48),
|
|
|
|
SCHED_TASK_CLASS(GCS, (GCS*)&rover._gcs, update_receive, 400, 500, 51),
|
|
|
|
SCHED_TASK_CLASS(GCS, (GCS*)&rover._gcs, update_send, 400, 1000, 54),
|
|
|
|
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&rover.g2.rc_channels, read_mode_switch, 7, 200, 57),
|
|
|
|
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&rover.g2.rc_channels, read_aux_all, 10, 200, 60),
|
|
|
|
SCHED_TASK_CLASS(AP_BattMonitor, &rover.battery, read, 10, 300, 63),
|
|
|
|
SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events, 50, 200, 66),
|
2022-09-20 04:37:48 -03:00
|
|
|
#if AP_GRIPPER_ENABLED
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Gripper, &rover.g2.gripper, update, 10, 75, 69),
|
2023-03-20 16:24:41 -03:00
|
|
|
#endif
|
2023-03-22 05:45:41 -03:00
|
|
|
#if AC_PRECLAND_ENABLED
|
2022-05-30 06:26:55 -03:00
|
|
|
SCHED_TASK(update_precland, 400, 50, 70),
|
|
|
|
#endif
|
2022-07-15 08:53:42 -03:00
|
|
|
#if AP_RPM_ENABLED
|
2022-01-05 07:23:45 -04:00
|
|
|
SCHED_TASK_CLASS(AP_RPM, &rover.rpm_sensor, update, 10, 100, 72),
|
2022-07-15 08:53:42 -03:00
|
|
|
#endif
|
2020-07-24 14:23:45 -03:00
|
|
|
#if HAL_MOUNT_ENABLED
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Mount, &rover.camera_mount, update, 50, 200, 75),
|
2018-02-12 01:24:29 -04:00
|
|
|
#endif
|
2022-06-02 05:28:26 -03:00
|
|
|
#if AP_CAMERA_ENABLED
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Camera, &rover.camera, update, 50, 200, 78),
|
2018-02-12 01:24:29 -04:00
|
|
|
#endif
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK(gcs_failsafe_check, 10, 200, 81),
|
|
|
|
SCHED_TASK(fence_check, 10, 200, 84),
|
|
|
|
SCHED_TASK(ekf_check, 10, 100, 87),
|
|
|
|
SCHED_TASK_CLASS(ModeSmartRTL, &rover.mode_smartrtl, save_position, 3, 200, 90),
|
|
|
|
SCHED_TASK_CLASS(AP_Notify, &rover.notify, update, 50, 300, 93),
|
|
|
|
SCHED_TASK(one_second_loop, 1, 1500, 96),
|
2020-01-17 20:36:11 -04:00
|
|
|
#if HAL_SPRAYER_ENABLED
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK_CLASS(AC_Sprayer, &rover.g2.sprayer, update, 3, 90, 99),
|
2020-01-17 20:36:11 -04:00
|
|
|
#endif
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK(compass_save, 0.1, 200, 105),
|
2018-04-20 05:57:14 -03:00
|
|
|
#if LOGGING_ENABLED == ENABLED
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Logger, &rover.logger, periodic_tasks, 50, 300, 108),
|
2018-04-20 05:57:14 -03:00
|
|
|
#endif
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK_CLASS(AP_InertialSensor, &rover.ins, periodic, 400, 200, 111),
|
|
|
|
SCHED_TASK_CLASS(AP_Scheduler, &rover.scheduler, update_logging, 0.1, 200, 114),
|
2021-08-12 00:38:23 -03:00
|
|
|
#if HAL_BUTTON_ENABLED
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Button, &rover.button, update, 5, 200, 117),
|
2021-08-12 00:38:23 -03:00
|
|
|
#endif
|
2018-02-28 08:23:09 -04:00
|
|
|
#if STATS_ENABLED == ENABLED
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK(stats_update, 1, 200, 120),
|
2018-02-28 08:23:09 -04:00
|
|
|
#endif
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK(crash_check, 10, 200, 123),
|
|
|
|
SCHED_TASK(cruise_learn_update, 50, 200, 126),
|
2017-01-30 10:21:55 -04:00
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
2021-11-13 23:49:45 -04:00
|
|
|
SCHED_TASK(afs_fs_check, 10, 200, 129),
|
2017-01-30 10:21:55 -04:00
|
|
|
#endif
|
2015-05-12 04:00:25 -03:00
|
|
|
};
|
|
|
|
|
2020-01-16 06:12:17 -04:00
|
|
|
|
|
|
|
void Rover::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
|
|
|
|
uint8_t &task_count,
|
|
|
|
uint32_t &log_bit)
|
|
|
|
{
|
|
|
|
tasks = &scheduler_tasks[0];
|
|
|
|
task_count = ARRAY_SIZE(scheduler_tasks);
|
|
|
|
log_bit = MASK_LOG_PM;
|
|
|
|
}
|
|
|
|
|
2018-03-01 23:30:40 -04:00
|
|
|
constexpr int8_t Rover::_failsafe_priorities[7];
|
|
|
|
|
2019-07-10 21:05:55 -03:00
|
|
|
Rover::Rover(void) :
|
2018-12-27 02:35:13 -04:00
|
|
|
AP_Vehicle(),
|
2019-07-10 21:05:55 -03:00
|
|
|
param_loader(var_info),
|
|
|
|
logger{g.log_bitmask},
|
|
|
|
modes(&g.mode1),
|
2020-01-27 21:34:01 -04:00
|
|
|
control_mode(&mode_initializing)
|
2019-07-10 21:05:55 -03:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
2021-11-15 01:08:35 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2020-02-18 00:11:45 -04:00
|
|
|
// set target location (for use by scripting)
|
|
|
|
bool Rover::set_target_location(const Location& target_loc)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!control_mode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2022-02-19 22:06:59 -04:00
|
|
|
return mode_guided.set_desired_location(target_loc);
|
2020-02-18 00:11:45 -04:00
|
|
|
}
|
|
|
|
|
2020-03-09 00:12:45 -03:00
|
|
|
// set target velocity (for use by scripting)
|
|
|
|
bool Rover::set_target_velocity_NED(const Vector3f& vel_ned)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!control_mode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// convert vector length into speed
|
|
|
|
const float target_speed_m = safe_sqrt(sq(vel_ned.x) + sq(vel_ned.y));
|
|
|
|
|
|
|
|
// convert vector direction to target yaw
|
|
|
|
const float target_yaw_cd = degrees(atan2f(vel_ned.y, vel_ned.x)) * 100.0f;
|
|
|
|
|
|
|
|
// send target heading and speed
|
|
|
|
mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_speed_m);
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2020-06-15 04:29:00 -03:00
|
|
|
// set steering and throttle (-1 to +1) (for use by scripting)
|
|
|
|
bool Rover::set_steering_and_throttle(float steering, float throttle)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!control_mode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// set steering and throttle
|
|
|
|
mode_guided.set_steering_and_throttle(steering, throttle);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2022-01-16 06:08:34 -04:00
|
|
|
// set desired turn rate (degrees/sec) and speed (m/s). Used for scripting
|
|
|
|
bool Rover::set_desired_turn_rate_and_speed(float turn_rate, float speed)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!control_mode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// set turn rate and speed. Turn rate is expected in centidegrees/s and speed in meters/s
|
|
|
|
mode_guided.set_desired_turn_rate_and_speed(turn_rate * 100.0f, speed);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2022-06-30 15:45:14 -03:00
|
|
|
// set desired nav speed (m/s). Used for scripting.
|
|
|
|
bool Rover::set_desired_speed(float speed)
|
|
|
|
{
|
|
|
|
return control_mode->set_desired_speed(speed);
|
|
|
|
}
|
|
|
|
|
2020-08-07 20:05:11 -03:00
|
|
|
// get control output (for use in scripting)
|
|
|
|
// returns true on success and control_value is set to a value in the range -1 to +1
|
|
|
|
bool Rover::get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value)
|
|
|
|
{
|
|
|
|
switch (control_output) {
|
2020-07-22 10:42:56 -03:00
|
|
|
case AP_Vehicle::ControlOutput::Roll:
|
|
|
|
control_value = constrain_float(g2.motors.get_roll(), -1.0f, 1.0f);
|
|
|
|
return true;
|
|
|
|
case AP_Vehicle::ControlOutput::Pitch:
|
|
|
|
control_value = constrain_float(g2.motors.get_pitch(), -1.0f, 1.0f);
|
|
|
|
return true;
|
2020-08-28 15:51:26 -03:00
|
|
|
case AP_Vehicle::ControlOutput::Walking_Height:
|
|
|
|
control_value = constrain_float(g2.motors.get_walking_height(), -1.0f, 1.0f);
|
2022-02-14 08:09:10 -04:00
|
|
|
return true;
|
2020-08-07 20:05:11 -03:00
|
|
|
case AP_Vehicle::ControlOutput::Throttle:
|
|
|
|
control_value = constrain_float(g2.motors.get_throttle() / 100.0f, -1.0f, 1.0f);
|
|
|
|
return true;
|
|
|
|
case AP_Vehicle::ControlOutput::Yaw:
|
|
|
|
control_value = constrain_float(g2.motors.get_steering() / 4500.0f, -1.0f, 1.0f);
|
|
|
|
return true;
|
|
|
|
case AP_Vehicle::ControlOutput::Lateral:
|
|
|
|
control_value = constrain_float(g2.motors.get_lateral() / 100.0f, -1.0f, 1.0f);
|
|
|
|
return true;
|
|
|
|
case AP_Vehicle::ControlOutput::MainSail:
|
|
|
|
control_value = constrain_float(g2.motors.get_mainsail() / 100.0f, -1.0f, 1.0f);
|
|
|
|
return true;
|
|
|
|
case AP_Vehicle::ControlOutput::WingSail:
|
|
|
|
control_value = constrain_float(g2.motors.get_wingsail() / 100.0f, -1.0f, 1.0f);
|
|
|
|
return true;
|
|
|
|
default:
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
2022-02-19 01:36:35 -04:00
|
|
|
|
|
|
|
// returns true if mode supports NAV_SCRIPT_TIME mission commands
|
|
|
|
bool Rover::nav_scripting_enable(uint8_t mode)
|
|
|
|
{
|
|
|
|
return mode == (uint8_t)mode_auto.mode_number();
|
|
|
|
}
|
|
|
|
|
|
|
|
// lua scripts use this to retrieve the contents of the active command
|
2022-10-13 21:37:52 -03:00
|
|
|
bool Rover::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4)
|
2022-02-19 01:36:35 -04:00
|
|
|
{
|
|
|
|
if (control_mode != &mode_auto) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2022-10-13 21:37:52 -03:00
|
|
|
return mode_auto.nav_script_time(id, cmd, arg1, arg2, arg3, arg4);
|
2022-02-19 01:36:35 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// lua scripts use this to indicate when they have complete the command
|
|
|
|
void Rover::nav_script_time_done(uint16_t id)
|
|
|
|
{
|
|
|
|
if (control_mode != &mode_auto) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
return mode_auto.nav_script_time_done(id);
|
|
|
|
}
|
2021-11-15 01:08:35 -04:00
|
|
|
#endif // AP_SCRIPTING_ENABLED
|
2020-08-07 20:05:11 -03:00
|
|
|
|
2018-02-28 08:23:09 -04:00
|
|
|
#if STATS_ENABLED == ENABLED
|
2016-10-25 23:19:28 -03:00
|
|
|
/*
|
|
|
|
update AP_Stats
|
|
|
|
*/
|
|
|
|
void Rover::stats_update(void)
|
|
|
|
{
|
2018-06-06 03:18:24 -03:00
|
|
|
g2.stats.set_flying(g2.motors.active());
|
2016-10-25 23:19:28 -03:00
|
|
|
g2.stats.update();
|
|
|
|
}
|
2018-02-28 08:23:09 -04:00
|
|
|
#endif
|
2016-10-25 23:19:28 -03:00
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2013-10-27 20:33:52 -03:00
|
|
|
// update AHRS system
|
2015-05-12 02:03:23 -03:00
|
|
|
void Rover::ahrs_update()
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2019-05-03 09:32:42 -03:00
|
|
|
arming.update_soft_armed();
|
2014-02-18 19:54:04 -04:00
|
|
|
|
2018-05-28 02:15:36 -03:00
|
|
|
// AHRS may use movement to calculate heading
|
|
|
|
update_ahrs_flyforward();
|
2013-11-17 19:58:22 -04:00
|
|
|
|
2013-10-27 20:33:52 -03:00
|
|
|
ahrs.update();
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2018-05-23 02:34:47 -03:00
|
|
|
// update position
|
2022-01-20 19:42:41 -04:00
|
|
|
have_position = ahrs.get_location(current_loc);
|
2018-05-23 02:34:47 -03:00
|
|
|
|
2018-11-07 18:41:48 -04:00
|
|
|
// set home from EKF if necessary and possible
|
|
|
|
if (!ahrs.home_is_set()) {
|
2018-05-29 21:52:57 -03:00
|
|
|
if (!set_home_to_current_location(false)) {
|
|
|
|
// ignore this failure
|
|
|
|
}
|
2018-11-07 18:41:48 -04:00
|
|
|
}
|
2017-06-05 04:55:24 -03:00
|
|
|
|
2014-05-11 08:39:06 -03:00
|
|
|
// if using the EKF get a speed update now (from accelerometers)
|
|
|
|
Vector3f velocity;
|
|
|
|
if (ahrs.get_velocity_NED(velocity)) {
|
2021-09-11 07:35:52 -03:00
|
|
|
ground_speed = velocity.xy().length();
|
2017-08-02 23:06:39 -03:00
|
|
|
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
|
|
|
ground_speed = ahrs.groundspeed();
|
2014-05-11 08:39:06 -03:00
|
|
|
}
|
|
|
|
|
2016-12-20 09:26:57 -04:00
|
|
|
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
2013-06-03 22:57:59 -03:00
|
|
|
Log_Write_Attitude();
|
2018-09-25 10:09:47 -03:00
|
|
|
Log_Write_Sail();
|
2016-12-20 09:26:57 -04:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2015-07-13 08:34:58 -03:00
|
|
|
if (should_log(MASK_LOG_IMU)) {
|
2021-01-22 01:28:07 -04:00
|
|
|
AP::ins().Write_IMU();
|
2015-07-13 08:34:58 -03:00
|
|
|
}
|
2021-12-15 15:09:29 -04:00
|
|
|
|
|
|
|
if (should_log(MASK_LOG_VIDEO_STABILISATION)) {
|
|
|
|
ahrs.write_video_stabilisation();
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2013-06-03 21:37:05 -03:00
|
|
|
/*
|
|
|
|
check for GCS failsafe - 10Hz
|
|
|
|
*/
|
2015-05-12 02:03:23 -03:00
|
|
|
void Rover::gcs_failsafe_check(void)
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2023-01-29 18:49:35 -04:00
|
|
|
if (g.fs_gcs_enabled == FS_GCS_DISABLED) {
|
2018-10-31 02:27:56 -03:00
|
|
|
// gcs failsafe disabled
|
|
|
|
return;
|
2013-09-15 19:23:21 -03:00
|
|
|
}
|
2018-10-31 02:27:56 -03:00
|
|
|
|
2021-02-04 22:28:08 -04:00
|
|
|
const uint32_t gcs_last_seen_ms = gcs().sysid_myggcs_last_seen_time_ms();
|
|
|
|
if (gcs_last_seen_ms == 0) {
|
|
|
|
// we've never seen the GCS, so we never failsafe for not seeing it
|
2023-01-29 18:49:35 -04:00
|
|
|
return;
|
2021-02-04 22:28:08 -04:00
|
|
|
}
|
|
|
|
|
2023-01-29 18:49:35 -04:00
|
|
|
// calc time since last gcs update
|
|
|
|
// note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
|
|
|
|
const uint32_t last_gcs_update_ms = millis() - gcs_last_seen_ms;
|
|
|
|
const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
|
|
|
|
|
|
|
|
const bool do_failsafe = last_gcs_update_ms >= gcs_timeout_ms ? true : false;
|
|
|
|
|
2021-02-04 22:28:08 -04:00
|
|
|
failsafe_trigger(FAILSAFE_EVENT_GCS, "GCS", do_failsafe);
|
2013-06-03 21:37:05 -03:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2013-06-03 21:37:05 -03:00
|
|
|
/*
|
|
|
|
log some key data - 10Hz
|
|
|
|
*/
|
2015-05-12 02:03:23 -03:00
|
|
|
void Rover::update_logging1(void)
|
2013-06-03 21:37:05 -03:00
|
|
|
{
|
2016-12-20 09:26:57 -04:00
|
|
|
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
|
2013-06-03 21:37:05 -03:00
|
|
|
Log_Write_Attitude();
|
2018-09-25 10:09:47 -03:00
|
|
|
Log_Write_Sail();
|
2016-12-20 09:26:57 -04:00
|
|
|
}
|
2015-05-24 19:59:16 -03:00
|
|
|
|
2017-12-07 08:42:06 -04:00
|
|
|
if (should_log(MASK_LOG_THR)) {
|
|
|
|
Log_Write_Throttle();
|
2022-03-03 21:31:26 -04:00
|
|
|
g2.beacon.log();
|
2016-12-20 09:26:57 -04:00
|
|
|
}
|
2013-06-03 21:37:05 -03:00
|
|
|
|
2016-12-20 09:26:57 -04:00
|
|
|
if (should_log(MASK_LOG_NTUN)) {
|
2013-06-03 21:37:05 -03:00
|
|
|
Log_Write_Nav_Tuning();
|
2021-11-18 23:38:12 -04:00
|
|
|
if (g2.pos_control.is_active()) {
|
|
|
|
g2.pos_control.write_log();
|
|
|
|
logger.Write_PID(LOG_PIDN_MSG, g2.pos_control.get_vel_pid().get_pid_info_x());
|
|
|
|
logger.Write_PID(LOG_PIDE_MSG, g2.pos_control.get_vel_pid().get_pid_info_y());
|
|
|
|
}
|
2016-12-20 09:26:57 -04:00
|
|
|
}
|
2018-12-10 02:25:25 -04:00
|
|
|
|
2021-03-25 05:47:29 -03:00
|
|
|
#if HAL_PROXIMITY_ENABLED
|
2018-12-10 02:25:25 -04:00
|
|
|
if (should_log(MASK_LOG_RANGEFINDER)) {
|
2022-03-03 21:12:18 -04:00
|
|
|
g2.proximity.log();
|
2018-12-10 02:25:25 -04:00
|
|
|
}
|
2021-03-25 05:47:29 -03:00
|
|
|
#endif
|
2013-12-29 19:24:01 -04:00
|
|
|
}
|
2013-12-15 20:14:58 -04:00
|
|
|
|
2013-12-29 19:24:01 -04:00
|
|
|
/*
|
|
|
|
log some key data - 10Hz
|
|
|
|
*/
|
2015-05-12 02:03:23 -03:00
|
|
|
void Rover::update_logging2(void)
|
2013-12-29 19:24:01 -04:00
|
|
|
{
|
2014-01-14 00:10:13 -04:00
|
|
|
if (should_log(MASK_LOG_STEERING)) {
|
2017-12-07 21:59:52 -04:00
|
|
|
Log_Write_Steering();
|
2013-12-15 20:14:58 -04:00
|
|
|
}
|
2013-12-29 19:24:01 -04:00
|
|
|
|
2016-12-20 09:26:57 -04:00
|
|
|
if (should_log(MASK_LOG_RC)) {
|
2013-12-29 19:24:01 -04:00
|
|
|
Log_Write_RC();
|
2019-02-05 01:00:15 -04:00
|
|
|
g2.wheel_encoder.Log_Write();
|
2016-12-20 09:26:57 -04:00
|
|
|
}
|
2016-04-04 20:59:09 -03:00
|
|
|
|
|
|
|
if (should_log(MASK_LOG_IMU)) {
|
2021-01-22 01:28:07 -04:00
|
|
|
AP::ins().Write_Vibration();
|
2022-10-21 21:29:54 -03:00
|
|
|
#if HAL_GYROFFT_ENABLED
|
|
|
|
gyro_fft.write_log_messages();
|
|
|
|
#endif
|
2016-04-04 20:59:09 -03:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2013-07-14 20:57:00 -03:00
|
|
|
|
2013-06-03 21:37:05 -03:00
|
|
|
/*
|
|
|
|
once a second events
|
|
|
|
*/
|
2015-05-12 02:03:23 -03:00
|
|
|
void Rover::one_second_loop(void)
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2013-06-03 06:33:59 -03:00
|
|
|
set_control_channels();
|
2013-06-03 21:37:05 -03:00
|
|
|
|
|
|
|
// cope with changes to aux functions
|
2018-11-21 18:09:33 -04:00
|
|
|
SRV_Channels::enable_aux_servos();
|
2013-06-03 21:37:05 -03:00
|
|
|
|
2015-10-30 02:56:41 -03:00
|
|
|
// update notify flags
|
|
|
|
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
|
|
|
|
AP_Notify::flags.pre_arm_gps_check = true;
|
2020-03-30 03:07:41 -03:00
|
|
|
AP_Notify::flags.armed = arming.is_armed();
|
2019-03-01 02:33:00 -04:00
|
|
|
AP_Notify::flags.flying = hal.util->get_soft_armed();
|
2015-10-30 02:56:41 -03:00
|
|
|
|
2013-06-03 21:37:05 -03:00
|
|
|
// cope with changes to mavlink system ID
|
|
|
|
mavlink_system.sysid = g.sysid_this_mav;
|
|
|
|
|
2018-11-07 18:41:48 -04:00
|
|
|
// attempt to update home position and baro calibration if not armed:
|
|
|
|
if (!hal.util->get_soft_armed()) {
|
2017-05-04 06:10:05 -03:00
|
|
|
update_home();
|
|
|
|
}
|
|
|
|
|
2018-10-19 02:36:07 -03:00
|
|
|
// need to set "likely flying" when armed to allow for compass
|
|
|
|
// learning to run
|
2020-01-06 19:05:52 -04:00
|
|
|
set_likely_flying(hal.util->get_soft_armed());
|
2019-04-29 03:31:45 -03:00
|
|
|
|
|
|
|
// send latest param values to wp_nav
|
2021-05-02 13:37:13 -03:00
|
|
|
g2.wp_nav.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering());
|
2021-11-18 23:38:12 -04:00
|
|
|
g2.pos_control.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering());
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2015-05-12 02:03:23 -03:00
|
|
|
void Rover::update_current_mode(void)
|
2015-05-24 19:59:16 -03:00
|
|
|
{
|
2019-01-25 14:58:50 -04:00
|
|
|
// check for emergency stop
|
|
|
|
if (SRV_Channels::get_emergency_stop()) {
|
|
|
|
// relax controllers, motor stopping done at output level
|
|
|
|
g2.attitude_control.relax_I();
|
|
|
|
}
|
|
|
|
|
2017-07-18 23:19:08 -03:00
|
|
|
control_mode->update();
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2020-12-05 14:45:11 -04:00
|
|
|
// vehicle specific waypoint info helpers
|
|
|
|
bool Rover::get_wp_distance_m(float &distance) const
|
2019-09-21 16:49:31 -03:00
|
|
|
{
|
2020-12-05 14:45:11 -04:00
|
|
|
// see GCS_MAVLINK_Rover::send_nav_controller_output()
|
|
|
|
if (!rover.control_mode->is_autopilot_mode()) {
|
|
|
|
return false;
|
2019-09-21 16:49:31 -03:00
|
|
|
}
|
2020-12-05 14:45:11 -04:00
|
|
|
distance = control_mode->get_distance_to_destination();
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// vehicle specific waypoint info helpers
|
|
|
|
bool Rover::get_wp_bearing_deg(float &bearing) const
|
|
|
|
{
|
|
|
|
// see GCS_MAVLINK_Rover::send_nav_controller_output()
|
|
|
|
if (!rover.control_mode->is_autopilot_mode()) {
|
|
|
|
return false;
|
2019-09-21 16:49:31 -03:00
|
|
|
}
|
2020-12-05 14:45:11 -04:00
|
|
|
bearing = control_mode->wp_bearing();
|
|
|
|
return true;
|
2019-09-21 16:49:31 -03:00
|
|
|
}
|
2020-12-05 14:45:11 -04:00
|
|
|
|
|
|
|
// vehicle specific waypoint info helpers
|
|
|
|
bool Rover::get_wp_crosstrack_error_m(float &xtrack_error) const
|
|
|
|
{
|
|
|
|
// see GCS_MAVLINK_Rover::send_nav_controller_output()
|
|
|
|
if (!rover.control_mode->is_autopilot_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
xtrack_error = control_mode->crosstrack_error();
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2019-09-21 16:49:31 -03:00
|
|
|
|
2019-07-10 21:05:55 -03:00
|
|
|
Rover rover;
|
2019-12-01 13:02:18 -04:00
|
|
|
AP_Vehicle& vehicle = rover;
|
2019-07-10 21:05:55 -03:00
|
|
|
|
2016-01-19 01:26:14 -04:00
|
|
|
AP_HAL_MAIN_CALLBACKS(&rover);
|