diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index e25560085c..4999569acb 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -42,13 +42,13 @@ public: _hal->rc.push_back( new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100, - 1500, 1900)); + 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540, - 1900)); + 1900, RC_MODE_INOUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500, - 1900)); + 1900, RC_MODE_INOUT, false)); } virtual MAV_MODE getMode() { return (MAV_MODE) _mode.get(); diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 5a846df99b..8bb7e9afb5 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -42,13 +42,13 @@ public: _hal->rc.push_back( new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100, - 1500, 1900)); + 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540, - 1900)); + 1900, RC_MODE_INOUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500, - 1900)); + 1900, RC_MODE_INOUT, false)); } virtual MAV_MODE getMode() { return (MAV_MODE) _mode.get(); diff --git a/ArduRover/ControllerTank.h b/ArduRover/ControllerTank.h index b0bb3f1ef7..ad8a294a2c 100644 --- a/ArduRover/ControllerTank.h +++ b/ArduRover/ControllerTank.h @@ -42,19 +42,19 @@ public: _hal->rc.push_back( new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, - 1500, 1900, RC_MODE_IN)); + 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, 1500, - 1900, RC_MODE_OUT)); + 1900, RC_MODE_OUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, 1500, - 1900, RC_MODE_OUT)); + 1900, RC_MODE_OUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1500, - 1900, RC_MODE_IN)); + 1900, RC_MODE_IN, false)); _hal->rc.push_back( new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500, - 1900, RC_MODE_IN)); + 1900, RC_MODE_IN, false)); } virtual MAV_MODE getMode() { return (MAV_MODE) _mode.get(); diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h index 539253f8d9..2120ead7fb 100644 --- a/apo/ControllerPlane.h +++ b/apo/ControllerPlane.h @@ -87,19 +87,19 @@ public: _hal->rc.push_back( new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100, - 1500, 1900, RC_MODE_IN)); + 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( new AP_RcChannel(k_chRoll, PSTR("roll_"), APM_RC, 0, 1200, - 1500, 1800, RC_MODE_INOUT)); + 1500, 1800, RC_MODE_INOUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chPitch, PSTR("pitch_"), APM_RC, 1, 1200, - 1500, 1800, RC_MODE_INOUT)); + 1500, 1800, RC_MODE_INOUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chThr, PSTR("thr_"), APM_RC, 2, 1100, 1100, - 1900, RC_MODE_INOUT)); + 1900, RC_MODE_INOUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chYaw, PSTR("yaw_"), APM_RC, 3, 1200, 1500, - 1800, RC_MODE_INOUT)); + 1800, RC_MODE_INOUT, false)); } virtual MAV_MODE getMode() { return (MAV_MODE) _mode.get(); diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 3f74739b00..9e12bbe0b0 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -82,31 +82,31 @@ public: */ _hal->rc.push_back( new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, - 1500, 1900, RC_MODE_IN)); + 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, - 1100, 1900, RC_MODE_OUT)); + 1100, 1900, RC_MODE_OUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, - 1100, 1900, RC_MODE_OUT)); + 1100, 1900, RC_MODE_OUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, - 1100, 1900, RC_MODE_OUT)); + 1100, 1900, RC_MODE_OUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100, - 1100, 1900, RC_MODE_OUT)); + 1100, 1900, RC_MODE_OUT, false)); _hal->rc.push_back( new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100, - 1500, 1900, RC_MODE_IN)); + 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, - 1500, 1900, RC_MODE_IN)); + 1500, 1900, RC_MODE_IN, false)); _hal->rc.push_back( new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 2, 1100, 1500, - 1900, RC_MODE_IN)); + 1900, RC_MODE_IN, false)); _hal->rc.push_back( new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100, - 1100, 1900, RC_MODE_IN)); + 1100, 1900, RC_MODE_IN, false)); } virtual void update(const float & dt) { diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h index 6cb10557e3..58335abe53 100644 --- a/libraries/APO/AP_MavlinkCommand.h +++ b/libraries/APO/AP_MavlinkCommand.h @@ -131,6 +131,7 @@ public: return getX(); break; case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_ENU: case MAV_FRAME_MISSION: default: return 0; @@ -144,6 +145,7 @@ public: setX(val); break; case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_ENU: case MAV_FRAME_MISSION: default: break; @@ -156,6 +158,7 @@ public: return getY(); break; case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_ENU: case MAV_FRAME_MISSION: default: return 0; @@ -169,6 +172,7 @@ public: setY(val); break; case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_ENU: case MAV_FRAME_MISSION: default: break; @@ -205,6 +209,9 @@ public: break; case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_LOCAL: + return -getZ() + home.getAlt(); + break; + case MAV_FRAME_LOCAL_ENU: return getZ() + home.getAlt(); break; case MAV_FRAME_MISSION: @@ -223,6 +230,9 @@ public: setZ(val); break; case MAV_FRAME_LOCAL: + setZ(home.getLonDeg() - val); + break; + case MAV_FRAME_LOCAL_ENU: setZ(val - home.getLonDeg()); break; case MAV_FRAME_MISSION: @@ -241,6 +251,9 @@ public: break; case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_LOCAL: + return -getZ(); + break; + case MAV_FRAME_LOCAL_ENU: return getZ(); break; case MAV_FRAME_MISSION: @@ -250,7 +263,7 @@ public: } } /** - * set the relative altitude in meters from home + * set the relative altitude in meters from home (up) */ void setRelAlt(float val) { switch (getFrame()) { @@ -259,6 +272,9 @@ public: break; case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_LOCAL: + setZ(-val); + break; + case MAV_FRAME_LOCAL_ENU: setZ(val); break; case MAV_FRAME_MISSION: diff --git a/libraries/APO/AP_RcChannel.cpp b/libraries/APO/AP_RcChannel.cpp index c2adbc5c52..a62668a991 100644 --- a/libraries/APO/AP_RcChannel.cpp +++ b/libraries/APO/AP_RcChannel.cpp @@ -24,8 +24,9 @@ AP_RcChannel::AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, AP_Var_group(keyValue, name), _ch(this, 1, ch, PSTR("ch")), _pwmMin(this, 2, pwmMin, PSTR("pMin")), _pwmNeutral(this, 3, pwmNeutral, PSTR("pNtrl")), - _pwmMax(this, 4, pwmMax, PSTR("pMax")), _rcMode(rcMode), - _reverse(this, 5, reverse, PSTR("rev")), _rc(rc), _pwm(pwmNeutral) { + _pwmMax(this, 4, pwmMax, PSTR("pMax")), + _reverse(this, 5, reverse, PSTR("rev")), + _rcMode(rcMode), _rc(rc), _pwm(pwmNeutral) { //Serial.print("pwm after ctor: "); Serial.println(pwmNeutral); if (rcMode == RC_MODE_IN) return; @@ -43,10 +44,6 @@ uint16_t AP_RcChannel::getRadioPwm() { return _rc.InputCh(_ch); } -void AP_RcChannel::setUsingRadio() { - setPwm(getRadioPwm()); -} - void AP_RcChannel::setPwm(uint16_t pwm) { //Serial.printf("pwm in setPwm: %d\n", pwm); //Serial.printf("reverse: %s\n", (reverse)?"true":"false"); @@ -69,14 +66,6 @@ void AP_RcChannel::setPwm(uint16_t pwm) { _rc.OutputCh(_ch, _pwm); } -void AP_RcChannel::setPosition(float position) { - if (position > 1.0) - position = 1.0; - else if (position < -1.0) - position = -1.0; - setPwm(_positionToPwm(position)); -} - uint16_t AP_RcChannel::_positionToPwm(const float & position) { uint16_t pwm; //Serial.printf("position: %f\n", position); @@ -94,6 +83,8 @@ uint16_t AP_RcChannel::_positionToPwm(const float & position) { float AP_RcChannel::_pwmToPosition(const uint16_t & pwm) { float position; + // note a piece-wise linear mapping occurs if the pwm ranges + // are not symmetric about pwmNeutral if (pwm < uint8_t(_pwmNeutral)) position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( _pwmNeutral - _pwmMin); diff --git a/libraries/APO/AP_RcChannel.h b/libraries/APO/AP_RcChannel.h index b12489a257..91790dd3bd 100644 --- a/libraries/APO/AP_RcChannel.h +++ b/libraries/APO/AP_RcChannel.h @@ -24,20 +24,20 @@ class AP_RcChannel: public AP_Var_group { public: /// Constructor - AP_RcChannel(AP_Var::Key key, const prog_char_t * name, APM_RC_Class & rc, + AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, APM_RC_Class & rc, const uint8_t & ch, const uint16_t & pwmMin, const uint16_t & pwmNeutral, const uint16_t & pwmMax, - const rcMode_t & rcMode = RC_MODE_INOUT, - const bool & reverse = false); + const rcMode_t & rcMode, + const bool & reverse); // configuration - AP_Uint8 _ch;AP_Uint16 _pwmMin;AP_Uint16 _pwmNeutral;AP_Uint16 _pwmMax; - rcMode_t _rcMode;AP_Bool _reverse; - - // set - void setUsingRadio(); - void setPwm(uint16_t pwm); - void setPosition(float position); + AP_Uint8 _ch; + AP_Uint16 _pwmMin; + AP_Uint16 _pwmNeutral; + AP_Uint16 _pwmMax; + AP_Uint8 _deg2mPwm; + rcMode_t _rcMode; + AP_Bool _reverse; // get uint16_t getPwm() { @@ -45,13 +45,22 @@ public: } uint16_t getRadioPwm(); float getPosition() { - return _pwmToPosition(_pwm); + return _pwmToPosition(getPwm()); } float getRadioPosition() { return _pwmToPosition(getRadioPwm()); } -private: + // set + void setUsingRadio() { + setPwm(getRadioPwm()); + } + void setPwm(uint16_t pwm); + void setPosition(float position) { + setPwm(_positionToPwm(position)); + } + +protected: // configuration APM_RC_Class & _rc; @@ -64,6 +73,27 @@ private: float _pwmToPosition(const uint16_t & pwm); }; +class AP_RcChannel_Scaled : public AP_RcChannel { +public: + AP_RcChannel_Scaled(AP_Var::Key keyValue, const prog_char_t * name, APM_RC_Class & rc, + const uint8_t & ch, const uint16_t & pwmMin, + const uint16_t & pwmNeutral, const uint16_t & pwmMax, + const rcMode_t & rcMode, + const bool & reverse, + const float & scale) : + AP_RcChannel(keyValue,name,rc,ch,pwmMin,pwmNeutral,pwmMax,rcMode,reverse), + _scale(this, 6, pwmNeutral, PSTR("scale")) + { + } + AP_Float _scale; + void setScaled(float val) { + setPwm(val/_scale); + } + float getScaled() { + return _scale*getPwm(); + } +}; + } // apo #endif // AP_RCCHANNEL_H