diff --git a/Rover/AP_MotorsUGV.cpp b/Rover/AP_MotorsUGV.cpp index 51c1b478a2..a738fd7dd5 100644 --- a/Rover/AP_MotorsUGV.cpp +++ b/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) { diff --git a/Rover/AP_MotorsUGV.h b/Rover/AP_MotorsUGV.h index 12da6507ff..c963308b13 100644 --- a/Rover/AP_MotorsUGV.h +++ b/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 diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 6f3d4049b7..22c22b091b 100644 --- a/Rover/Rover.cpp +++ b/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; diff --git a/Rover/Rover.h b/Rover/Rover.h index e6f587fb73..aa487edbef 100644 --- a/Rover/Rover.h +++ b/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; diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 9354e3aa5b..6c6f31c64a 100644 --- a/Rover/mode.cpp +++ b/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 { diff --git a/Rover/mode.h b/Rover/mode.h index eb8a1b8332..b62752f7fe 100644 --- a/Rover/mode.h +++ b/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 diff --git a/Rover/mode_manual.cpp b/Rover/mode_manual.cpp index 00eafa3b67..404a687e00 100644 --- a/Rover/mode_manual.cpp +++ b/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; diff --git a/Rover/radio.cpp b/Rover/radio.cpp index c839ababeb..2e89b651e7 100644 --- a/Rover/radio.cpp +++ b/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(); diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 4cc08ec554..8a1f0f344c 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/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 };