Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : #include "RC_Channel_Rover.h"
       4             : 
       5             : // defining these two macros and including the RC_Channels_VarInfo
       6             : // header defines the parameter information common to all vehicle
       7             : // types
       8             : #define RC_CHANNELS_SUBCLASS RC_Channels_Rover
       9             : #define RC_CHANNEL_SUBCLASS RC_Channel_Rover
      10             : 
      11             : #include <RC_Channel/RC_Channels_VarInfo.h>
      12             : 
      13       89921 : int8_t RC_Channels_Rover::flight_mode_channel_number() const
      14             : {
      15       89921 :     return rover.g.mode_channel;
      16             : }
      17             : 
      18         314 : void RC_Channel_Rover::mode_switch_changed(modeswitch_pos_t new_pos)
      19             : {
      20         314 :     if (new_pos < 0 || (uint8_t)new_pos >= ARRAY_SIZE(rover.g.modes)) {
      21             :         // should not have been called
      22           0 :         return;
      23             :     }
      24             : 
      25         314 :     rover.set_mode((Mode::Number)rover.g.modes[new_pos].get(), ModeReason::RC_COMMAND);
      26             : }
      27             : 
      28             : // init_aux_switch_function - initialize aux functions
      29        4528 : void RC_Channel_Rover::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag)
      30             : {
      31             :     // init channel options
      32        4528 :     switch (ch_option) {
      33             :     // the following functions do not need initialising:
      34           0 :     case AUX_FUNC::ACRO:
      35             :     case AUX_FUNC::AUTO:
      36             :     case AUX_FUNC::CIRCLE:
      37             :     case AUX_FUNC::FOLLOW:
      38             :     case AUX_FUNC::GUIDED:
      39             :     case AUX_FUNC::HOLD:
      40             :     case AUX_FUNC::LEARN_CRUISE:
      41             :     case AUX_FUNC::LOITER:
      42             :     case AUX_FUNC::MAINSAIL:
      43             :     case AUX_FUNC::MANUAL:
      44             :     case AUX_FUNC::PITCH:
      45             :     case AUX_FUNC::ROLL:
      46             :     case AUX_FUNC::WALKING_HEIGHT:
      47             :     case AUX_FUNC::RTL:
      48             :     case AUX_FUNC::SAILBOAT_TACK:
      49             :     case AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC:
      50             :     case AUX_FUNC::SAVE_WP:
      51             :     case AUX_FUNC::SIMPLE:
      52             :     case AUX_FUNC::SMART_RTL:
      53             :     case AUX_FUNC::STEERING:
      54             :     case AUX_FUNC::WIND_VANE_DIR_OFSSET:
      55           0 :         break;
      56           1 :     case AUX_FUNC::SAILBOAT_MOTOR_3POS:
      57           1 :         do_aux_function_sailboat_motor_3pos(ch_flag);
      58           1 :         break;
      59        4527 :     default:
      60        4527 :         RC_Channel::init_aux_function(ch_option, ch_flag);
      61        4527 :         break;
      62             :     }
      63        4528 : }
      64             : 
      65             : 
      66      580921 : bool RC_Channels_Rover::in_rc_failsafe() const
      67             : {
      68      580921 :     return rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE;
      69             : }
      70             : 
      71      335478 : bool RC_Channels_Rover::has_valid_input() const
      72             : {
      73      335478 :     if (in_rc_failsafe()) {
      74         737 :         return false;
      75             :     }
      76      334741 :     return RC_Channels::has_valid_input();
      77             : }
      78             : 
      79      617122 : RC_Channel * RC_Channels_Rover::get_arming_channel(void) const
      80             : {
      81      617122 :     return rover.channel_steer;
      82             : }
      83             : 
      84         306 : bool RC_Channels_Rover::has_pilot_input_for_override_clear()
      85             : {
      86         306 :     if (channel_outside_trim_dz(get_roll_channel())) {  // steering
      87           1 :         return true;
      88             :     }
      89             :     // throttle may be non-centered (e.g. forward-only), so use movement-since-override-start
      90         305 :     if (throttle_moved_since_override_start()) {
      91           1 :         return true;
      92             :     }
      93         304 :     if (rover.g2.motors.is_omni() && channel_outside_trim_dz(get_lateral_channel())) {
      94           0 :         return true;
      95             :     }
      96         304 :     return false;
      97             : }
      98             : 
      99          12 : void RC_Channel_Rover::do_aux_function_change_mode(Mode &mode,
     100             :         const AuxSwitchPos ch_flag)
     101             : {
     102          12 :     switch (ch_flag) {
     103           5 :     case AuxSwitchPos::HIGH:
     104           5 :         rover.set_mode(mode, ModeReason::AUX_FUNCTION);
     105           5 :         break;
     106           0 :     case AuxSwitchPos::MIDDLE:
     107             :         // do nothing
     108           0 :         break;
     109           7 :     case AuxSwitchPos::LOW:
     110           7 :         if (rover.control_mode == &mode) {
     111           3 :             rc().reset_mode_switch();
     112             :         }
     113             :     }
     114          12 : }
     115             : 
     116           6 : void RC_Channel_Rover::add_waypoint_for_current_loc()
     117             : {
     118             :     // create new mission command
     119           6 :     AP_Mission::Mission_Command cmd = {};
     120             : 
     121             :     // set new waypoint to current location
     122           6 :     cmd.content.location = rover.current_loc;
     123             : 
     124             :     // make the new command to a waypoint
     125           6 :     cmd.id = MAV_CMD_NAV_WAYPOINT;
     126             : 
     127             :     // save command
     128           6 :     if (rover.mode_auto.mission.add_cmd(cmd)) {
     129           6 :         hal.console->printf("Added waypoint %u", (unsigned)rover.mode_auto.mission.num_commands());
     130             :     }
     131           6 : }
     132             : 
     133           2 : void RC_Channel_Rover::do_aux_function_sailboat_motor_3pos(const AuxSwitchPos ch_flag)
     134             : {
     135           2 :     switch (ch_flag) {
     136           0 :     case AuxSwitchPos::HIGH:
     137           0 :         rover.g2.sailboat.set_motor_state(Sailboat::UseMotor::USE_MOTOR_ALWAYS);
     138           0 :         break;
     139           1 :     case AuxSwitchPos::MIDDLE:
     140           1 :         rover.g2.sailboat.set_motor_state(Sailboat::UseMotor::USE_MOTOR_ASSIST);
     141           1 :         break;
     142           1 :     case AuxSwitchPos::LOW:
     143           1 :         rover.g2.sailboat.set_motor_state(Sailboat::UseMotor::USE_MOTOR_NEVER);
     144           1 :         break;
     145             :     }
     146           2 : }
     147             : 
     148         111 : bool RC_Channel_Rover::do_aux_function(const AuxFuncTrigger &trigger)
     149             : {
     150         111 :     const AUX_FUNC &ch_option = trigger.func;
     151         111 :     const AuxSwitchPos &ch_flag = trigger.pos;
     152             : 
     153         111 :     switch (ch_option) {
     154           0 :     case AUX_FUNC::DO_NOTHING:
     155           0 :         break;
     156          14 :     case AUX_FUNC::SAVE_WP:
     157          14 :         if (ch_flag == AuxSwitchPos::HIGH) {
     158             :             // do nothing if in AUTO mode
     159           6 :             if (rover.control_mode == &rover.mode_auto) {
     160           0 :                 break;
     161             :             }
     162             : 
     163             :             // if disarmed clear mission and set home to current location
     164           6 :             if (!rover.arming.is_armed()) {
     165           0 :                 rover.mode_auto.mission.clear();
     166           0 :                 GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "SaveWP: Mission cleared!");
     167           0 :                 if (!rover.set_home_to_current_location(false)) {
     168             :                     // ignored
     169             :                 }
     170           0 :                 break;
     171             :             }
     172           6 :             add_waypoint_for_current_loc();
     173             :         }
     174          14 :         break;
     175             : 
     176             :     // learn cruise speed and throttle
     177           0 :     case AUX_FUNC::LEARN_CRUISE:
     178           0 :         if (ch_flag == AuxSwitchPos::HIGH) {
     179           0 :             rover.cruise_learn_start();
     180             :         }
     181           0 :         break;
     182             : 
     183             :     // set mode to Manual
     184           0 :     case AUX_FUNC::MANUAL:
     185           0 :         do_aux_function_change_mode(rover.mode_manual, ch_flag);
     186           0 :         break;
     187             : 
     188             :     // set mode to Acro
     189           0 :     case AUX_FUNC::ACRO:
     190           0 :         do_aux_function_change_mode(rover.mode_acro, ch_flag);
     191           0 :         break;
     192             : 
     193             :     // set mode to Steering
     194           7 :     case AUX_FUNC::STEERING:
     195           7 :         do_aux_function_change_mode(rover.mode_steering, ch_flag);
     196           7 :         break;
     197             : 
     198             :     // set mode to Hold
     199           5 :     case AUX_FUNC::HOLD:
     200           5 :         do_aux_function_change_mode(rover.mode_hold, ch_flag);
     201           5 :         break;
     202             : 
     203             :     // set mode to Auto
     204           0 :     case AUX_FUNC::AUTO:
     205           0 :         do_aux_function_change_mode(rover.mode_auto, ch_flag);
     206           0 :         break;
     207             : 
     208             :     // set mode to RTL
     209           0 :     case AUX_FUNC::RTL:
     210           0 :         do_aux_function_change_mode(rover.mode_rtl, ch_flag);
     211           0 :         break;
     212             : 
     213             :     // set mode to SmartRTL
     214           0 :     case AUX_FUNC::SMART_RTL:
     215           0 :         do_aux_function_change_mode(rover.mode_smartrtl, ch_flag);
     216           0 :         break;
     217             : 
     218             :     // set mode to Guided
     219           0 :     case AUX_FUNC::GUIDED:
     220           0 :         do_aux_function_change_mode(rover.mode_guided, ch_flag);
     221           0 :         break;
     222             : 
     223             :     // Set mode to LOITER
     224           0 :     case AUX_FUNC::LOITER:
     225           0 :         do_aux_function_change_mode(rover.mode_loiter, ch_flag);
     226           0 :         break;
     227             : 
     228             : #if MODE_FOLLOW_ENABLED
     229             :     // Set mode to Follow
     230           0 :     case AUX_FUNC::FOLLOW:
     231           0 :         do_aux_function_change_mode(rover.mode_follow, ch_flag);
     232           0 :         break;
     233             : #endif
     234             : 
     235             :     // set mode to Simple
     236           0 :     case AUX_FUNC::SIMPLE:
     237           0 :         do_aux_function_change_mode(rover.mode_simple, ch_flag);
     238           0 :         break;
     239             : 
     240           0 :     case AUX_FUNC::CIRCLE:
     241           0 :         do_aux_function_change_mode(rover.g2.mode_circle, ch_flag);
     242           0 :         break;
     243             : 
     244             :     // trigger sailboat tack
     245           0 :     case AUX_FUNC::SAILBOAT_TACK:
     246             :         // any switch movement interpreted as request to tack
     247           0 :         rover.control_mode->handle_tack_request();
     248           0 :         break;
     249             : 
     250             :     // sailboat motor state 3pos
     251           1 :     case AUX_FUNC::SAILBOAT_MOTOR_3POS:
     252           1 :         do_aux_function_sailboat_motor_3pos(ch_flag);
     253           1 :         break;
     254             : 
     255             :     // save steering trim
     256           0 :     case AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC:
     257           0 :         if (!rover.g2.motors.have_skid_steering() && rover.arming.is_armed() &&
     258           0 :             (rover.control_mode != &rover.mode_loiter)
     259           0 :             && (rover.control_mode != &rover.mode_hold) && ch_flag == AuxSwitchPos::HIGH) {
     260           0 :             SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_steering);
     261           0 :             GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Steering trim saved!");
     262             :         }
     263           0 :         break;
     264             : 
     265             :     // manual input, nothing to do
     266           0 :     case AUX_FUNC::MAINSAIL:
     267             :     case AUX_FUNC::PITCH:
     268             :     case AUX_FUNC::ROLL:
     269             :     case AUX_FUNC::WALKING_HEIGHT:
     270             :     case AUX_FUNC::WIND_VANE_DIR_OFSSET:
     271           0 :         break;
     272             : 
     273          84 :     default:
     274          84 :         return RC_Channel::do_aux_function(trigger);
     275             : 
     276             :     }
     277             : 
     278          27 :     return true;
     279             : }