Rover: roll and pitch input sent to motors

added get desired roll and pitch
Motors library adds set and get methods for roll and pitch
added roll and pitch to get_control_output
get_control_output returns roll and pitch values

Rover/AP_MotorsUGV.cpp

@@ -210,6 +210,18 @@ void AP_MotorsUGV::set_lateral(float lateral)
     _lateral = constrain_float(lateral, -100.0f, 100.0f);
 }
 
+// set roll input as a value from -1 to +1
+void AP_MotorsUGV::set_roll(float roll)
+{
+    _roll = constrain_float(roll, -1.0f, 1.0f);
+}
+
+// set pitch input as a value from -1 to +1
+void AP_MotorsUGV::set_pitch(float pitch)
+{
+    _pitch = constrain_float(pitch, -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

@@ -60,6 +60,14 @@ public:
     float get_throttle() const { return _throttle; }
     void set_throttle(float throttle);
 
+    // get or set roll as a value from -1 to 1
+    float get_roll() const { return _roll; }
+    void set_roll(float roll);
+
+    // get or set pitch as a value from -1 to 1
+    float get_pitch() const { return _pitch; }
+    void set_pitch(float pitch);
+
     // get or set lateral input as a value from -100 to +100
     float get_lateral() const { return _lateral; }
     void set_lateral(float lateral);
@@ -182,6 +190,8 @@ protected:
     float   _throttle_prev; // throttle input from previous iteration
     bool    _scale_steering = true; // true if we should scale steering by speed or angle
     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   _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

@@ -185,6 +185,12 @@ bool Rover::set_steering_and_throttle(float steering, float throttle)
 bool Rover::get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value)
 {
     switch (control_output) {
+    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;
     case AP_Vehicle::ControlOutput::Throttle:
         control_value = constrain_float(g2.motors.get_throttle() / 100.0f, -1.0f, 1.0f);
         return true;

Rover/Rover.h

@@ -143,6 +143,8 @@ private:
     RC_Channel *channel_steer;
     RC_Channel *channel_throttle;
     RC_Channel *channel_lateral;
+    RC_Channel *channel_roll;
+    RC_Channel *channel_pitch;
 
     AP_Logger logger;
 

Rover/mode.cpp

@@ -8,6 +8,8 @@ Mode::Mode() :
     channel_steer(rover.channel_steer),
     channel_throttle(rover.channel_throttle),
     channel_lateral(rover.channel_lateral),
+    channel_roll(rover.channel_roll),
+    channel_pitch(rover.channel_pitch),
     attitude_control(rover.g2.attitude_control)
 { }
 
@@ -171,6 +173,22 @@ void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_
     speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
 }
 
+// decode pilot roll and pitch inputs and return in roll_out and pitch_out arguments
+// outputs are in the range -1 to +1
+void Mode::get_pilot_desired_roll_and_pitch(float &roll_out, float &pitch_out)
+{
+    if (channel_roll != nullptr) {
+        roll_out = channel_roll->norm_input();
+    } else {
+        roll_out = 0.0f;
+    }
+    if (channel_pitch != nullptr) {
+        pitch_out = channel_pitch->norm_input();
+    } else {
+        pitch_out = 0.0f;
+    }
+}
+
 // return heading (in degrees) to target destination (aka waypoint)
 float Mode::wp_bearing() const
 {

Rover/mode.h

@@ -153,6 +153,10 @@ protected:
     // decode pilot's input and return heading_out (in cd) and speed_out (in m/s)
     void get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out);
 
+    // decode pilot roll and pitch inputs and return in roll_out and pitch_out arguments
+    // outputs are in the range -1 to +1
+    void get_pilot_desired_roll_and_pitch(float &roll_out, float &pitch_out);
+
     // high level call to navigate to waypoint
     void navigate_to_waypoint();
 
@@ -197,6 +201,8 @@ protected:
     class RC_Channel *&channel_steer;
     class RC_Channel *&channel_throttle;
     class RC_Channel *&channel_lateral;
+    class RC_Channel *&channel_roll;
+    class RC_Channel *&channel_pitch;
     class AR_AttitudeControl &attitude_control;
 
     // private members for waypoint navigation

Rover/mode_manual.cpp

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

Rover/radio.cpp

@@ -17,6 +17,18 @@ void Rover::set_control_channels(void)
         channel_lateral->set_angle(100);
     }
 
+    // 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);
+    if (channel_roll != nullptr) {
+        channel_roll->set_angle(SERVO_MAX);
+        channel_roll->set_default_dead_zone(30);
+    }
+    if (channel_pitch != nullptr) {
+        channel_pitch->set_angle(SERVO_MAX);
+        channel_pitch->set_default_dead_zone(30);
+    }
+
     // sailboat rc input init
     g2.sailboat.init_rc_in();