2015-05-13 00:16:45 -03:00
|
|
|
#include "Rover.h"
|
|
|
|
|
2018-05-07 23:35:08 -03:00
|
|
|
#include "RC_Channel.h"
|
|
|
|
|
|
|
|
// defining these two macros and including the RC_Channels_VarInfo
|
|
|
|
// header defines the parameter information common to all vehicle
|
|
|
|
// types
|
|
|
|
#define RC_CHANNELS_SUBCLASS RC_Channels_Rover
|
|
|
|
#define RC_CHANNEL_SUBCLASS RC_Channel_Rover
|
|
|
|
|
|
|
|
#include <RC_Channel/RC_Channels_VarInfo.h>
|
2017-02-20 10:26:18 -04:00
|
|
|
|
2018-05-07 23:35:08 -03:00
|
|
|
int8_t RC_Channels_Rover::flight_mode_channel_number() const
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2018-05-07 23:35:08 -03:00
|
|
|
return rover.g.mode_channel;
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2018-05-07 23:35:08 -03:00
|
|
|
void RC_Channel_Rover::mode_switch_changed(modeswitch_pos_t new_pos)
|
|
|
|
{
|
|
|
|
if (new_pos < 0 || (uint8_t)new_pos > rover.num_modes) {
|
|
|
|
// should not have been called
|
|
|
|
return;
|
2016-12-20 09:32:57 -04:00
|
|
|
}
|
2018-05-07 23:35:08 -03:00
|
|
|
Mode *new_mode = rover.mode_from_mode_num((Mode::Number)rover.modes[new_pos].get());
|
|
|
|
if (new_mode != nullptr) {
|
2019-10-17 00:48:47 -03:00
|
|
|
rover.set_mode(*new_mode, ModeReason::RC_COMMAND);
|
2016-12-20 09:32:57 -04:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2018-05-07 23:35:08 -03:00
|
|
|
// init_aux_switch_function - initialize aux functions
|
2020-06-02 23:21:51 -03:00
|
|
|
void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2018-05-07 23:35:08 -03:00
|
|
|
// init channel options
|
2019-09-22 20:09:02 -03:00
|
|
|
switch (ch_option) {
|
|
|
|
// the following functions do not need initialising:
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::ACRO:
|
|
|
|
case AUX_FUNC::AUTO:
|
2019-11-09 01:25:03 -04:00
|
|
|
case AUX_FUNC::FOLLOW:
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::GUIDED:
|
2019-11-09 01:25:03 -04:00
|
|
|
case AUX_FUNC::HOLD:
|
|
|
|
case AUX_FUNC::LEARN_CRUISE:
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::LOITER:
|
2019-05-27 11:28:08 -03:00
|
|
|
case AUX_FUNC::MAINSAIL:
|
2019-11-09 01:25:03 -04:00
|
|
|
case AUX_FUNC::MANUAL:
|
2020-01-26 13:24:51 -04:00
|
|
|
case AUX_FUNC::PITCH:
|
|
|
|
case AUX_FUNC::ROLL:
|
2020-08-28 15:07:43 -03:00
|
|
|
case AUX_FUNC::WALKING_HEIGHT:
|
2019-11-09 01:25:03 -04:00
|
|
|
case AUX_FUNC::RTL:
|
|
|
|
case AUX_FUNC::SAILBOAT_TACK:
|
|
|
|
case AUX_FUNC::SAVE_TRIM:
|
|
|
|
case AUX_FUNC::SAVE_WP:
|
|
|
|
case AUX_FUNC::SIMPLE:
|
|
|
|
case AUX_FUNC::SMART_RTL:
|
|
|
|
case AUX_FUNC::STEERING:
|
2019-05-27 11:28:08 -03:00
|
|
|
break;
|
|
|
|
case AUX_FUNC::SAILBOAT_MOTOR_3POS:
|
|
|
|
do_aux_function_sailboat_motor_3pos(ch_flag);
|
2018-05-07 23:35:08 -03:00
|
|
|
break;
|
|
|
|
default:
|
|
|
|
RC_Channel::init_aux_function(ch_option, ch_flag);
|
|
|
|
break;
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2017-08-22 04:06:23 -03:00
|
|
|
|
2018-05-07 23:35:08 -03:00
|
|
|
bool RC_Channels_Rover::has_valid_input() const
|
2017-08-22 04:06:23 -03:00
|
|
|
{
|
2018-05-07 23:35:08 -03:00
|
|
|
if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return true;
|
2017-08-22 04:06:23 -03:00
|
|
|
}
|
|
|
|
|
2020-04-02 17:44:54 -03:00
|
|
|
RC_Channel * RC_Channels_Rover::get_arming_channel(void) const
|
|
|
|
{
|
|
|
|
return rover.channel_steer;
|
|
|
|
}
|
|
|
|
|
2018-05-07 23:35:08 -03:00
|
|
|
void RC_Channel_Rover::do_aux_function_change_mode(Mode &mode,
|
2020-06-02 23:21:51 -03:00
|
|
|
const AuxSwitchPos ch_flag)
|
2018-06-06 04:02:01 -03:00
|
|
|
{
|
2019-09-22 20:09:02 -03:00
|
|
|
switch (ch_flag) {
|
2020-06-02 23:21:51 -03:00
|
|
|
case AuxSwitchPos::HIGH:
|
2019-10-17 00:48:47 -03:00
|
|
|
rover.set_mode(mode, ModeReason::RC_COMMAND);
|
2018-06-06 04:02:01 -03:00
|
|
|
break;
|
2020-06-02 23:21:51 -03:00
|
|
|
case AuxSwitchPos::MIDDLE:
|
2018-06-06 04:02:01 -03:00
|
|
|
// do nothing
|
|
|
|
break;
|
2020-06-02 23:21:51 -03:00
|
|
|
case AuxSwitchPos::LOW:
|
2018-05-07 23:35:08 -03:00
|
|
|
if (rover.control_mode == &mode) {
|
|
|
|
rc().reset_mode_switch();
|
2018-06-06 04:02:01 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-08-16 23:39:30 -03:00
|
|
|
void RC_Channel_Rover::add_waypoint_for_current_loc()
|
|
|
|
{
|
|
|
|
// create new mission command
|
|
|
|
AP_Mission::Mission_Command cmd = {};
|
|
|
|
|
|
|
|
// set new waypoint to current location
|
|
|
|
cmd.content.location = rover.current_loc;
|
|
|
|
|
|
|
|
// make the new command to a waypoint
|
|
|
|
cmd.id = MAV_CMD_NAV_WAYPOINT;
|
|
|
|
|
|
|
|
// save command
|
|
|
|
if (rover.mode_auto.mission.add_cmd(cmd)) {
|
|
|
|
hal.console->printf("Added waypoint %u", (unsigned)rover.mode_auto.mission.num_commands());
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-06-02 23:21:51 -03:00
|
|
|
void RC_Channel_Rover::do_aux_function_sailboat_motor_3pos(const AuxSwitchPos ch_flag)
|
2019-05-27 11:28:08 -03:00
|
|
|
{
|
2019-09-22 20:09:02 -03:00
|
|
|
switch (ch_flag) {
|
2020-06-02 23:21:51 -03:00
|
|
|
case AuxSwitchPos::HIGH:
|
2019-08-21 03:19:06 -03:00
|
|
|
rover.g2.sailboat.set_motor_state(Sailboat::UseMotor::USE_MOTOR_ALWAYS);
|
2019-05-27 11:28:08 -03:00
|
|
|
break;
|
2020-06-02 23:21:51 -03:00
|
|
|
case AuxSwitchPos::MIDDLE:
|
2019-08-21 03:19:06 -03:00
|
|
|
rover.g2.sailboat.set_motor_state(Sailboat::UseMotor::USE_MOTOR_ASSIST);
|
2019-05-27 11:28:08 -03:00
|
|
|
break;
|
2020-06-02 23:21:51 -03:00
|
|
|
case AuxSwitchPos::LOW:
|
2019-08-21 03:19:06 -03:00
|
|
|
rover.g2.sailboat.set_motor_state(Sailboat::UseMotor::USE_MOTOR_NEVER);
|
2019-05-27 11:28:08 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-02-24 05:48:16 -04:00
|
|
|
bool RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
|
2017-08-22 04:06:23 -03:00
|
|
|
{
|
2018-05-07 23:35:08 -03:00
|
|
|
switch (ch_option) {
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::DO_NOTHING:
|
2013-02-07 18:21:22 -04:00
|
|
|
break;
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::SAVE_WP:
|
2020-06-02 23:21:51 -03:00
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) {
|
2017-08-22 04:06:23 -03:00
|
|
|
// do nothing if in AUTO mode
|
2018-05-07 23:35:08 -03:00
|
|
|
if (rover.control_mode == &rover.mode_auto) {
|
2021-02-24 05:48:16 -04:00
|
|
|
break;
|
2017-08-22 04:06:23 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// if disarmed clear mission and set home to current location
|
2018-05-07 23:35:08 -03:00
|
|
|
if (!rover.arming.is_armed()) {
|
2018-12-11 20:10:20 -04:00
|
|
|
rover.mode_auto.mission.clear();
|
2018-05-29 21:52:57 -03:00
|
|
|
if (!rover.set_home_to_current_location(false)) {
|
|
|
|
// ignored
|
|
|
|
}
|
2021-02-24 05:48:16 -04:00
|
|
|
break;
|
2017-08-22 04:06:23 -03:00
|
|
|
}
|
|
|
|
|
2017-11-28 22:45:34 -04:00
|
|
|
// record the waypoint if not in auto mode
|
2018-05-07 23:35:08 -03:00
|
|
|
if (rover.control_mode != &rover.mode_auto) {
|
2018-08-16 23:39:30 -03:00
|
|
|
if (rover.mode_auto.mission.num_commands() == 0) {
|
|
|
|
// add a home location....
|
|
|
|
add_waypoint_for_current_loc();
|
2012-11-28 07:44:03 -04:00
|
|
|
}
|
2018-08-16 23:39:30 -03:00
|
|
|
add_waypoint_for_current_loc();
|
2012-11-28 07:44:03 -04:00
|
|
|
}
|
|
|
|
}
|
2013-02-07 18:21:22 -04:00
|
|
|
break;
|
2017-08-22 08:53:36 -03:00
|
|
|
|
|
|
|
// learn cruise speed and throttle
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::LEARN_CRUISE:
|
2020-06-02 23:21:51 -03:00
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) {
|
2018-05-07 23:35:08 -03:00
|
|
|
rover.cruise_learn_start();
|
2017-08-22 08:53:36 -03:00
|
|
|
}
|
|
|
|
break;
|
2017-11-30 23:15:50 -04:00
|
|
|
|
2017-11-30 23:48:49 -04:00
|
|
|
// set mode to Manual
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::MANUAL:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_manual, ch_flag);
|
2017-11-30 23:48:49 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
// set mode to Acro
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::ACRO:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_acro, ch_flag);
|
2017-11-30 23:48:49 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
// set mode to Steering
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::STEERING:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_steering, ch_flag);
|
2017-11-30 23:48:49 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
// set mode to Hold
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::HOLD:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_hold, ch_flag);
|
2017-11-30 23:48:49 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
// set mode to Auto
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::AUTO:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_auto, ch_flag);
|
2017-11-30 23:48:49 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
// set mode to RTL
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::RTL:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_rtl, ch_flag);
|
2017-11-30 23:48:49 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
// set mode to SmartRTL
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::SMART_RTL:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_smartrtl, ch_flag);
|
2017-11-30 23:48:49 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
// set mode to Guided
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::GUIDED:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_guided, ch_flag);
|
2017-11-30 23:48:49 -04:00
|
|
|
break;
|
2018-05-24 05:13:12 -03:00
|
|
|
|
|
|
|
// Set mode to LOITER
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::LOITER:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_loiter, ch_flag);
|
2018-05-24 05:13:12 -03:00
|
|
|
break;
|
2018-05-24 01:47:07 -03:00
|
|
|
|
|
|
|
// Set mode to Follow
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::FOLLOW:
|
2018-05-07 23:35:08 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_follow, ch_flag);
|
|
|
|
break;
|
|
|
|
|
2018-07-02 04:21:37 -03:00
|
|
|
// set mode to Simple
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::SIMPLE:
|
2018-07-02 04:21:37 -03:00
|
|
|
do_aux_function_change_mode(rover.mode_simple, ch_flag);
|
|
|
|
break;
|
|
|
|
|
2018-09-14 04:09:07 -03:00
|
|
|
// trigger sailboat tack
|
2019-04-03 13:24:17 -03:00
|
|
|
case AUX_FUNC::SAILBOAT_TACK:
|
2018-11-01 03:11:37 -03:00
|
|
|
// any switch movement interpreted as request to tack
|
|
|
|
rover.control_mode->handle_tack_request();
|
2018-09-14 04:09:07 -03:00
|
|
|
break;
|
|
|
|
|
2019-05-27 11:28:08 -03:00
|
|
|
// sailboat motor state 3pos
|
|
|
|
case AUX_FUNC::SAILBOAT_MOTOR_3POS:
|
|
|
|
do_aux_function_sailboat_motor_3pos(ch_flag);
|
|
|
|
break;
|
|
|
|
|
2019-09-22 20:09:02 -03:00
|
|
|
// save steering trim
|
|
|
|
case AUX_FUNC::SAVE_TRIM:
|
|
|
|
if (!rover.g2.motors.have_skid_steering() && rover.arming.is_armed() &&
|
|
|
|
(rover.control_mode != &rover.mode_loiter)
|
2020-06-02 23:21:51 -03:00
|
|
|
&& (rover.control_mode != &rover.mode_hold) && ch_flag == AuxSwitchPos::HIGH) {
|
2019-09-22 20:09:02 -03:00
|
|
|
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_steering);
|
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "Steering trim saved!");
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
2020-01-26 13:24:51 -04:00
|
|
|
// manual input, nothing to do
|
|
|
|
case AUX_FUNC::MAINSAIL:
|
|
|
|
case AUX_FUNC::PITCH:
|
|
|
|
case AUX_FUNC::ROLL:
|
2020-08-28 15:07:43 -03:00
|
|
|
case AUX_FUNC::WALKING_HEIGHT:
|
2020-01-26 13:24:51 -04:00
|
|
|
break;
|
|
|
|
|
2018-05-07 23:35:08 -03:00
|
|
|
default:
|
2021-02-24 05:48:16 -04:00
|
|
|
return RC_Channel::do_aux_function(ch_option, ch_flag);
|
2018-05-24 01:47:07 -03:00
|
|
|
|
2012-11-28 07:44:03 -04:00
|
|
|
}
|
2021-02-24 05:48:16 -04:00
|
|
|
|
|
|
|
return true;
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|