Rover: walking_height input sent to motors

Rover/AP_MotorsUGV.cpp

@@ -222,6 +222,12 @@ void AP_MotorsUGV::set_pitch(float pitch)
     _pitch = constrain_float(pitch, -1.0f, 1.0f);
 }
 
+// set walking_height input as a value from -1 to +1
+void AP_MotorsUGV::set_walking_height(float walking_height)
+{
+    _walking_height = constrain_float(walking_height, -1.0f, 1.0f);
+}
+
 // set mainsail input as a value from 0 to 100
 void AP_MotorsUGV::set_mainsail(float mainsail)
 {

Rover/AP_MotorsUGV.h

@@ -68,6 +68,10 @@ public:
     float get_pitch() const { return _pitch; }
     void set_pitch(float pitch);
 
+    // get or set walking_height as a value from -1 to 1
+    float get_walking_height() const { return _walking_height; }
+    void set_walking_height(float walking_height);
+
     // get or set lateral input as a value from -100 to +100
     float get_lateral() const { return _lateral; }
     void set_lateral(float lateral);
@@ -192,6 +196,7 @@ protected:
     float   _lateral;  // requested lateral input as a value from -100 to +100
     float   _roll;      // requested roll as a value from -1 to +1
     float   _pitch;     // requested pitch as a value from -1 to +1
+    float   _walking_height; // requested height as a value from -1 to +1   
     float   _mainsail;  // requested mainsail input as a value from 0 to 100
     float   _wingsail;  // requested wing sail input as a value in the range +- 100
 

Rover/Rover.cpp

@@ -191,6 +191,9 @@ bool Rover::get_control_output(AP_Vehicle::ControlOutput control_output, float &
     case AP_Vehicle::ControlOutput::Pitch:
         control_value = constrain_float(g2.motors.get_pitch(), -1.0f, 1.0f);
         return true;
+    case AP_Vehicle::ControlOutput::Walking_Height:
+        control_value = constrain_float(g2.motors.get_walking_height(), -1.0f, 1.0f);
+        return true;    
     case AP_Vehicle::ControlOutput::Throttle:
         control_value = constrain_float(g2.motors.get_throttle() / 100.0f, -1.0f, 1.0f);
         return true;

Rover/Rover.h

@@ -145,6 +145,7 @@ private:
     RC_Channel *channel_lateral;
     RC_Channel *channel_roll;
     RC_Channel *channel_pitch;
+    RC_Channel *channel_walking_height;
 
     AP_Logger logger;
 

Rover/mode.cpp

@@ -10,6 +10,7 @@ Mode::Mode() :
     channel_lateral(rover.channel_lateral),
     channel_roll(rover.channel_roll),
     channel_pitch(rover.channel_pitch),
+    channel_walking_height(rover.channel_walking_height),
     attitude_control(rover.g2.attitude_control)
 { }
 
@@ -189,6 +190,17 @@ void Mode::get_pilot_desired_roll_and_pitch(float &roll_out, float &pitch_out)
     }
 }
 
+// decode pilot walking_height inputs and return in walking_height_out arguments
+// outputs are in the range -1 to +1
+void Mode::get_pilot_desired_walking_height(float &walking_height_out)
+{
+    if (channel_walking_height != nullptr) {
+        walking_height_out = channel_walking_height->norm_input();
+    } else {
+        walking_height_out = 0.0f;
+    }
+}
+
 // return heading (in degrees) to target destination (aka waypoint)
 float Mode::wp_bearing() const
 {

Rover/mode.h

@@ -157,6 +157,10 @@ protected:
     // outputs are in the range -1 to +1
     void get_pilot_desired_roll_and_pitch(float &roll_out, float &pitch_out);
 
+    // decode pilot height inputs and return in height_out arguments
+    // outputs are in the range -1 to +1
+    void get_pilot_desired_walking_height(float &walking_height_out);
+
     // high level call to navigate to waypoint
     void navigate_to_waypoint();
 
@@ -203,6 +207,7 @@ protected:
     class RC_Channel *&channel_lateral;
     class RC_Channel *&channel_roll;
     class RC_Channel *&channel_pitch;
+    class RC_Channel *&channel_walking_height;
     class AR_AttitudeControl &attitude_control;
 
     // private members for waypoint navigation

Rover/mode_manual.cpp

@@ -18,11 +18,13 @@ void ModeManual::update()
         rover.balancebot_pitch_control(desired_throttle);
     }
 
-    // walking robots support roll and pitch
-    float desired_roll, desired_pitch;
+    // walking robots support roll, pitch and walking_height
+    float desired_roll, desired_pitch, desired_walking_height;
     get_pilot_desired_roll_and_pitch(desired_roll, desired_pitch);
+    get_pilot_desired_walking_height(desired_walking_height);
     g2.motors.set_roll(desired_roll);
     g2.motors.set_pitch(desired_pitch);
+    g2.motors.set_walking_height(desired_walking_height);
 
     // set sailboat sails
     float desired_mainsail;

Rover/radio.cpp

@@ -20,6 +20,7 @@ void Rover::set_control_channels(void)
     // walking robots rc input init
     channel_roll = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ROLL);
     channel_pitch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::PITCH);
+    channel_walking_height = rc().find_channel_for_option(RC_Channel::AUX_FUNC::WALKING_HEIGHT);
     if (channel_roll != nullptr) {
         channel_roll->set_angle(SERVO_MAX);
         channel_roll->set_default_dead_zone(30);
@@ -28,6 +29,10 @@ void Rover::set_control_channels(void)
         channel_pitch->set_angle(SERVO_MAX);
         channel_pitch->set_default_dead_zone(30);
     }
+    if (channel_walking_height != nullptr) {
+        channel_walking_height->set_angle(SERVO_MAX);
+        channel_walking_height->set_default_dead_zone(30);
+    }    
 
     // sailboat rc input init
     g2.sailboat.init_rc_in();

libraries/AP_Vehicle/AP_Vehicle.h

@@ -193,6 +193,7 @@ public:
         Lateral = 5,
         MainSail = 6,
         WingSail = 7,
+        Walking_Height = 8,
         Last_ControlOutput  // place new values before this
     };