mirror of https://github.com/ArduPilot/ardupilot
Rover: add sailboat support
This commit is contained in:
IamPete1
Randy Mackay
parent
100c494127
commit
4366bae96d
|
|
@ -95,6 +95,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
|
|||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
SCHED_TASK(afs_fs_check, 10, 200),
|
||||
#endif
|
||||
SCHED_TASK(windvane_update, 10, 200),
|
||||
};
|
||||
|
||||
void Rover::read_mode_switch()
|
||||
|
|
@ -181,6 +182,7 @@ void Rover::ahrs_update()
|
|||
|
||||
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||
Log_Write_Attitude();
|
||||
Log_Write_Sail();
|
||||
}
|
||||
|
||||
if (should_log(MASK_LOG_IMU)) {
|
||||
|
|
@ -219,6 +221,7 @@ void Rover::update_logging1(void)
|
|||
{
|
||||
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||
Log_Write_Attitude();
|
||||
Log_Write_Sail();
|
||||
}
|
||||
|
||||
if (should_log(MASK_LOG_THR)) {
|
||||
|
|
|
|||
|
|
@ -257,6 +257,21 @@ void Rover::send_pid_tuning(mavlink_channel_t chan)
|
|||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// sailboat heel to mainsail pid
|
||||
if (g.gcs_pid_mask & 32) {
|
||||
pid_info = &g2.attitude_control.get_sailboat_heel_pid().get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, 9,
|
||||
pid_info->desired,
|
||||
pid_info->actual,
|
||||
pid_info->FF,
|
||||
pid_info->P,
|
||||
pid_info->I,
|
||||
pid_info->D);
|
||||
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Rover::send_fence_status(mavlink_channel_t chan)
|
||||
|
|
@ -264,6 +279,21 @@ void Rover::send_fence_status(mavlink_channel_t chan)
|
|||
fence_send_mavlink_status(chan);
|
||||
}
|
||||
|
||||
void Rover::send_wind(mavlink_channel_t chan)
|
||||
{
|
||||
// exit immediately if no wind vane
|
||||
if (!rover.g2.windvane.enabled()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// send wind
|
||||
mavlink_msg_wind_send(
|
||||
chan,
|
||||
degrees(rover.g2.windvane.get_apparent_wind_direction_rad()),
|
||||
0,
|
||||
0);
|
||||
}
|
||||
|
||||
void Rover::send_wheel_encoder(mavlink_channel_t chan)
|
||||
{
|
||||
// send wheel encoder data using rpm message
|
||||
|
|
@ -347,6 +377,9 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
|
|||
rover.send_fence_status(chan);
|
||||
break;
|
||||
|
||||
case MSG_WIND:
|
||||
CHECK_PAYLOAD_SIZE(WIND);
|
||||
rover.send_wind(chan);
|
||||
break;
|
||||
|
||||
case MSG_PID_TUNING:
|
||||
|
|
@ -492,6 +525,7 @@ static const ap_message STREAM_EXTRA2_msgs[] = {
|
|||
static const ap_message STREAM_EXTRA3_msgs[] = {
|
||||
MSG_AHRS,
|
||||
MSG_HWSTATUS,
|
||||
MSG_WIND,
|
||||
MSG_RANGEFINDER,
|
||||
MSG_SYSTEM_TIME,
|
||||
MSG_BATTERY2,
|
||||
|
|
|
|||
|
|
@ -44,6 +44,11 @@ void Rover::Log_Write_Attitude()
|
|||
if (is_balancebot()) {
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info());
|
||||
}
|
||||
|
||||
// log heel to sail control for sailboats
|
||||
if (g2.motors.has_sail()) {
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info());
|
||||
}
|
||||
}
|
||||
|
||||
// Write a range finder depth message
|
||||
|
|
@ -56,7 +61,7 @@ void Rover::Log_Write_Depth()
|
|||
|
||||
// get position
|
||||
Location loc;
|
||||
if (!rover.ahrs.get_position(loc)) {
|
||||
if (!ahrs.get_position(loc)) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -154,6 +159,26 @@ void Rover::Log_Write_Proximity()
|
|||
DataFlash.Log_Write_Proximity(g2.proximity);
|
||||
}
|
||||
|
||||
void Rover::Log_Write_Sail()
|
||||
{
|
||||
// only log sail if present
|
||||
if (!g2.motors.has_sail()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// get wind direction
|
||||
float wind_dir_rel = 0.0f;
|
||||
if (rover.g2.windvane.enabled()) {
|
||||
wind_dir_rel = degrees(g2.windvane.get_apparent_wind_direction_rad());
|
||||
}
|
||||
DataFlash.Log_Write("SAIL", "TimeUS,WindDirRel,SailOut,VMG",
|
||||
"sh%n", "F000", "Qfff",
|
||||
AP_HAL::micros64(),
|
||||
(double)wind_dir_rel,
|
||||
(double)g2.motors.get_mainsail(),
|
||||
(double)sailboat_get_VMG());
|
||||
}
|
||||
|
||||
struct PACKED log_Steering {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
|
|
@ -356,6 +381,7 @@ void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
|
|||
void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
|
||||
void Rover::Log_Write_Nav_Tuning() {}
|
||||
void Rover::Log_Write_Proximity() {}
|
||||
void Rover::Log_Write_Sail() {}
|
||||
void Rover::Log_Write_Startup(uint8_t type) {}
|
||||
void Rover::Log_Write_Throttle() {}
|
||||
void Rover::Log_Write_Rangefinder() {}
|
||||
|
|
|
|||
|
|
@ -64,8 +64,8 @@ const AP_Param::Info Rover::var_info[] = {
|
|||
// @DisplayName: GCS PID tuning mask
|
||||
// @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for
|
||||
// @User: Advanced
|
||||
// @Values: 0:None,1:Steering,2:Throttle,4:Pitch,8:Left Wheel,16:Right Wheel
|
||||
// @Bitmask: 0:Steering,1:Throttle,2:Pitch,3:Left Wheel,4:Right Wheel
|
||||
// @Values: 0:None,1:Steering,2:Throttle,4:Pitch,8:Left Wheel,16:Right Wheel,32:Sailboat Heel
|
||||
// @Bitmask: 0:Steering,1:Throttle,2:Pitch,3:Left Wheel,4:Right Wheel,5:Sailboat Heel
|
||||
GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0),
|
||||
|
||||
// @Param: MAG_ENABLE
|
||||
|
|
@ -608,6 +608,45 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("LOIT_RADIUS", 30, ParametersG2, loit_radius, 2),
|
||||
|
||||
// @Group: WNDVN_
|
||||
// @Path: ../libraries/AP_WindVane/AP_WindVane.cpp
|
||||
AP_SUBGROUPINFO(windvane, "WNDVN_", 31, ParametersG2, AP_WindVane),
|
||||
|
||||
// @Param: SAIL_ANGLE_MIN
|
||||
// @DisplayName: Sail min angle
|
||||
// @Description: Mainsheet tight, angle between centerline and boom
|
||||
// @Units: deg
|
||||
// @Range: 0 90
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SAIL_ANGLE_MIN", 32, ParametersG2, sail_angle_min, 0),
|
||||
|
||||
// @Param: SAIL_ANGLE_MAX
|
||||
// @DisplayName: Sail max angle
|
||||
// @Description: Mainsheet loose, angle between centerline and boom
|
||||
// @Units: deg
|
||||
// @Range: 0 90
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SAIL_ANGLE_MAX", 33, ParametersG2, sail_angle_max, 90),
|
||||
|
||||
// @Param: SAIL_ANGLE_IDEAL
|
||||
// @DisplayName: Sail ideal angle
|
||||
// @Description: Ideal angle between sail and apparent wind
|
||||
// @Units: deg
|
||||
// @Range: 0 90
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SAIL_ANGLE_IDEAL", 34, ParametersG2, sail_angle_ideal, 25),
|
||||
|
||||
// @Param: SAIL_HEEL_MAX
|
||||
// @DisplayName: Sailing maximum heel angle
|
||||
// @Description: When in auto sail trim modes the heel will be limited to this value using PID control
|
||||
// @Units: deg
|
||||
// @Range: 0 90
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SAIL_HEEL_MAX", 35, ParametersG2, sail_heel_angle_max, 15),
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
@ -639,7 +678,8 @@ ParametersG2::ParametersG2(void)
|
|||
proximity(rover.serial_manager),
|
||||
avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon),
|
||||
follow(),
|
||||
rally(rover.ahrs)
|
||||
rally(rover.ahrs),
|
||||
windvane()
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
|
@ -704,6 +744,14 @@ void Rover::load_parameters(void)
|
|||
g2.crash_angle.set_default(30);
|
||||
}
|
||||
|
||||
// sailboat defaults
|
||||
if (g2.motors.has_sail()) {
|
||||
g2.crash_angle.set_default(0);
|
||||
g2.loit_type.set_default(1);
|
||||
g2.loit_radius.set_default(5);
|
||||
g.waypoint_overshoot.set_default(10);
|
||||
}
|
||||
|
||||
const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old, Parameters::k_param_rc_2_old,
|
||||
Parameters::k_param_rc_3_old, Parameters::k_param_rc_4_old,
|
||||
Parameters::k_param_rc_5_old, Parameters::k_param_rc_6_old,
|
||||
|
|
|
|||
|
|
@ -371,6 +371,15 @@ public:
|
|||
|
||||
// Simple mode types
|
||||
AP_Int8 simple_type;
|
||||
|
||||
// sailboat parameters
|
||||
AP_Float sail_angle_min;
|
||||
AP_Float sail_angle_max;
|
||||
AP_Float sail_angle_ideal;
|
||||
AP_Float sail_heel_angle_max;
|
||||
|
||||
// windvane
|
||||
AP_WindVane windvane;
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
|
|||
|
|
@ -79,6 +79,7 @@
|
|||
#include <AC_Avoidance/AC_Avoid.h>
|
||||
#include <AP_Follow/AP_Follow.h>
|
||||
#include <AP_OSD/AP_OSD.h>
|
||||
#include <AP_WindVane/AP_WindVane.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
#include <SITL/SITL.h>
|
||||
#endif
|
||||
|
|
@ -472,6 +473,7 @@ private:
|
|||
void send_pid_tuning(mavlink_channel_t chan);
|
||||
void send_wheel_encoder(mavlink_channel_t chan);
|
||||
void send_fence_status(mavlink_channel_t chan);
|
||||
void send_wind(mavlink_channel_t chan);
|
||||
void gcs_data_stream_send(void);
|
||||
void gcs_update(void);
|
||||
void gcs_retry_deferred(void);
|
||||
|
|
@ -484,6 +486,7 @@ private:
|
|||
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
|
||||
void Log_Write_Nav_Tuning();
|
||||
void Log_Write_Proximity();
|
||||
void Log_Write_Sail();
|
||||
void Log_Write_Startup(uint8_t type);
|
||||
void Log_Write_Steering();
|
||||
void Log_Write_Throttle();
|
||||
|
|
@ -506,6 +509,10 @@ private:
|
|||
void control_failsafe(uint16_t pwm);
|
||||
bool trim_radio();
|
||||
|
||||
// sailboat.cpp
|
||||
void sailboat_update_mainsail(float desired_speed);
|
||||
float sailboat_get_VMG() const;
|
||||
|
||||
// sensors.cpp
|
||||
void init_compass(void);
|
||||
void init_compass_location(void);
|
||||
|
|
@ -518,6 +525,7 @@ private:
|
|||
void accel_cal_update(void);
|
||||
void read_rangefinders(void);
|
||||
void init_proximity();
|
||||
void windvane_update();
|
||||
void update_sensor_status_flags(void);
|
||||
|
||||
// Steering.cpp
|
||||
|
|
|
|||
|
|
@ -285,6 +285,9 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_
|
|||
rover.balancebot_pitch_control(throttle_out);
|
||||
}
|
||||
|
||||
// update mainsail position if present
|
||||
rover.sailboat_update_mainsail(target_speed);
|
||||
|
||||
// send to motor
|
||||
g2.motors.set_throttle(throttle_out);
|
||||
}
|
||||
|
|
@ -301,6 +304,9 @@ bool Mode::stop_vehicle()
|
|||
rover.balancebot_pitch_control(throttle_out);
|
||||
}
|
||||
|
||||
// relax mainsail if present
|
||||
g2.motors.set_mainsail(100.0f);
|
||||
|
||||
// send to motor
|
||||
g2.motors.set_throttle(throttle_out);
|
||||
|
||||
|
|
|
|||
|
|
@ -10,6 +10,9 @@ void ModeHold::update()
|
|||
rover.balancebot_pitch_control(throttle);
|
||||
}
|
||||
|
||||
// relax mainsail
|
||||
g2.motors.set_mainsail(100.0f);
|
||||
|
||||
// hold position - stop motors and center steering
|
||||
g2.motors.set_throttle(throttle);
|
||||
g2.motors.set_steering(0.0f);
|
||||
|
|
|
|||
|
|
@ -25,7 +25,9 @@ void ModeLoiter::update()
|
|||
|
||||
// if within waypoint radius slew desired speed towards zero and use existing desired heading
|
||||
if (_distance_to_destination <= g2.loit_radius) {
|
||||
_desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt);
|
||||
// sailboats do not stop
|
||||
const float desired_speed_within_radius = g2.motors.has_sail() ? 0.1f : 0.0f;
|
||||
_desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
|
||||
_yaw_error_cd = 0.0f;
|
||||
} else {
|
||||
// P controller with hard-coded gain to convert distance to desired speed
|
||||
|
|
|
|||
|
|
@ -18,6 +18,9 @@ void ModeManual::update()
|
|||
rover.balancebot_pitch_control(desired_throttle);
|
||||
}
|
||||
|
||||
// set sailboat mainsail from throttle position
|
||||
g2.motors.set_mainsail(desired_throttle);
|
||||
|
||||
// copy RC scaled inputs to outputs
|
||||
g2.motors.set_throttle(desired_throttle);
|
||||
g2.motors.set_steering(desired_steering, false);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,69 @@
|
|||
#include "Rover.h"
|
||||
|
||||
/*
|
||||
To Do List
|
||||
- Improve tacking in light winds and bearing away in strong wings
|
||||
- consider drag vs lift sailing differences, ie upwind sail is like wing, dead down wind sail is like parachute
|
||||
- max speed paramiter and contoller, for maping you may not want to go too fast
|
||||
- mavlink sailing messages
|
||||
- motor sailing, some boats may also have motor, we need to decide at what point we would be better of just motoring in low wind, or for a tight loiter, or to hit waypoint exactly, or if stuck head to wind, or to reverse...
|
||||
- smart decision making, ie tack on windshifts, what to do if stuck head to wind
|
||||
- some sailing codes track waves to try and 'surf' and to allow tacking on a flat bit, not sure if there is much gain to be had here
|
||||
- add some sort of pitch monitoring to prevent nose diving in heavy weather
|
||||
- pitch PID for hydrofoils
|
||||
- more advanced sail control, ie twist
|
||||
- independent sheeting for main and jib
|
||||
- wing type sails with 'elevator' control
|
||||
- tack on depth sounder info to stop sailing into shallow water on indirect sailing routes
|
||||
- add option to do proper tacks, ie tacking on flat spot in the waves, or only try once at a certain speed, or some better method than just changing the desired heading suddenly
|
||||
*/
|
||||
|
||||
// update mainsail's desired angle based on wind speed and direction and desired speed (in m/s)
|
||||
void Rover::sailboat_update_mainsail(float desired_speed)
|
||||
{
|
||||
if (!g2.motors.has_sail()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// relax sail if desired speed is zero
|
||||
if (!is_positive(desired_speed)) {
|
||||
g2.motors.set_mainsail(100.0f);
|
||||
return;
|
||||
}
|
||||
|
||||
// + is wind over starboard side, - is wind over port side, but as the sails are sheeted the same on each side it makes no difference so take abs
|
||||
float wind_dir_apparent = fabsf(g2.windvane.get_apparent_wind_direction_rad());
|
||||
wind_dir_apparent = degrees(wind_dir_apparent);
|
||||
|
||||
// set the main sail to the ideal angle to the wind
|
||||
float mainsail_angle = wind_dir_apparent - g2.sail_angle_ideal;
|
||||
|
||||
// make sure between allowable range
|
||||
mainsail_angle = constrain_float(mainsail_angle, g2.sail_angle_min, g2.sail_angle_max);
|
||||
|
||||
// linear interpolate mainsail value (0 to 100) from wind angle mainsail_angle
|
||||
float mainsail = linear_interpolate(0.0f, 100.0f, mainsail_angle, g2.sail_angle_min, g2.sail_angle_max);
|
||||
|
||||
// use PID controller to sheet out
|
||||
const float pid_offset = g2.attitude_control.get_sail_out_from_heel(radians(g2.sail_heel_angle_max), G_Dt) * 100.0f;
|
||||
|
||||
mainsail = constrain_float((mainsail+pid_offset), 0.0f ,100.0f);
|
||||
g2.motors.set_mainsail(mainsail);
|
||||
}
|
||||
|
||||
// Velocity Made Good, this is the speed we are traveling towards the desired destination
|
||||
// only for logging at this stage
|
||||
// https://en.wikipedia.org/wiki/Velocity_made_good
|
||||
float Rover::sailboat_get_VMG() const
|
||||
{
|
||||
// return 0 if not heading towards waypoint
|
||||
if (!control_mode->is_autopilot_mode()) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float speed;
|
||||
if (!g2.attitude_control.get_forward_speed(speed)) {
|
||||
return 0.0f;
|
||||
}
|
||||
return (speed * cosf(wrap_PI(radians(nav_controller->target_bearing_cd()) - ahrs.yaw)));
|
||||
}
|
||||
|
|
@ -241,6 +241,11 @@ void Rover::init_proximity(void)
|
|||
g2.proximity.set_rangefinder(&rangefinder);
|
||||
}
|
||||
|
||||
void Rover::windvane_update()
|
||||
{
|
||||
g2.windvane.update();
|
||||
}
|
||||
|
||||
// update error mask of sensors and subsystems. The mask
|
||||
// uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
|
||||
// then it indicates that the sensor or subsystem is present but
|
||||
|
|
|
|||
|
|
@ -64,6 +64,8 @@ void Rover::init_ardupilot()
|
|||
|
||||
rssi.init();
|
||||
|
||||
g2.windvane.init();
|
||||
|
||||
// init baro before we start the GCS, so that the CLI baro test works
|
||||
barometer.init();
|
||||
|
||||
|
|
@ -335,6 +337,9 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method)
|
|||
// initialize simple mode heading
|
||||
rover.mode_simple.init_heading();
|
||||
|
||||
// save home heading for use in sail vehicles
|
||||
rover.g2.windvane.record_home_headng();
|
||||
|
||||
change_arm_state();
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue