diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 248b17a100..8db5b0d35b 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -429,7 +429,16 @@ void Plane::calc_throttle() return; } - channel_throttle->set_servo_out(SpdHgt_Controller->get_throttle_demand()); + int32_t commanded_throttle = SpdHgt_Controller->get_throttle_demand(); + + // Received an external msg that guides throttle in the last 3 seconds? + if (control_mode == GUIDED && + plane.guided_state.last_forced_throttle_ms > 0 && + millis() - plane.guided_state.last_forced_throttle_ms < 3000) { + commanded_throttle = plane.guided_state.forced_throttle; + } + + channel_throttle->set_servo_out(commanded_throttle); } /***************************************** @@ -445,12 +454,23 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler) if (control_mode == STABILIZE && rudder_input != 0) { disable_integrator = true; } - steering_control.rudder = yawController.get_servo_out(speed_scaler, disable_integrator); - // add in rudder mixing from roll - steering_control.rudder += channel_roll->get_servo_out() * g.kff_rudder_mix; - steering_control.rudder += rudder_input; - steering_control.rudder = constrain_int16(steering_control.rudder, -4500, 4500); + int16_t commanded_rudder; + + // Received an external msg that guides yaw in the last 3 seconds? + if (control_mode == GUIDED && + plane.guided_state.last_forced_rpy_ms.z > 0 && + millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) { + commanded_rudder = plane.guided_state.forced_rpy_cd.z; + } else { + commanded_rudder = yawController.get_servo_out(speed_scaler, disable_integrator); + + // add in rudder mixing from roll + commanded_rudder += channel_roll->get_servo_out() * g.kff_rudder_mix; + commanded_rudder += rudder_input; + } + + steering_control.rudder = constrain_int16(commanded_rudder, -4500, 4500); } /* @@ -519,8 +539,16 @@ void Plane::calc_nav_pitch() { // Calculate the Pitch of the plane // -------------------------------- - nav_pitch_cd = SpdHgt_Controller->get_pitch_demand(); - nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); + int32_t commanded_pitch = SpdHgt_Controller->get_pitch_demand(); + + // Received an external msg that guides roll in the last 3 seconds? + if (control_mode == GUIDED && + plane.guided_state.last_forced_rpy_ms.y > 0 && + millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { + commanded_pitch = plane.guided_state.forced_rpy_cd.y; + } + + nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); } @@ -532,9 +560,10 @@ void Plane::calc_nav_roll() int32_t commanded_roll = nav_controller->nav_roll_cd(); // Received an external msg that guides roll in the last 3 seconds? - if (plane.guided_state.last_roll_ms > 0 && - AP_HAL::millis() - plane.guided_state.last_roll_ms < 3000) { - commanded_roll = plane.guided_state.roll_cd; + if (control_mode == GUIDED && + plane.guided_state.last_forced_rpy_ms.x > 0 && + millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { + commanded_roll = plane.guided_state.forced_rpy_cd.x; } nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 8aad24c95d..89c2ad1c29 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -2110,28 +2110,66 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) // in e.g., RTL, CICLE. Specifying a single mode for companion // computer control is more safe (even more so when using // FENCE_ACTION = 4 for geofence failures). - if (plane.control_mode != GUIDED) { //don't screw up failsafes + if (plane.control_mode != GUIDED) { // don't screw up failsafes break; } mavlink_set_attitude_target_t att_target; mavlink_msg_set_attitude_target_decode(msg, &att_target); - // Unexepectedly, the mask is expecting "ones" for dimensions that should - // be IGNORNED rather than INCLUDED. See mavlink documentation of the - // SET_ATTITUDE_TARGET message, type_mask field. - const uint16_t roll_mask = 0b11111110; // (roll mask at bit 1) - if (att_target.type_mask & roll_mask) { - // Extract the Euler roll angle from the Quaternion, - Quaternion q(att_target.q[0], att_target.q[1], - att_target.q[2], att_target.q[3]); - float roll_rad = q.get_euler_roll(); + // Mappings: If any of these bits are set, the corresponding input should be ignored. + // NOTE, when parsing the bits we invert them for easier interpretation but transport has them inverted + // bit 1: body roll rate + // bit 2: body pitch rate + // bit 3: body yaw rate + // bit 4: unknown + // bit 5: unknown + // bit 6: reserved + // bit 7: throttle + // bit 8: attitude - plane.guided_state.roll_cd = degrees(roll_rad) * 100.0f; + // if not setting all Quaternion values, use _rate flags to indicate which fields. + + // Extract the Euler roll angle from the Quaternion. + Quaternion q(att_target.q[0], att_target.q[1], + att_target.q[2], att_target.q[3]); + + // NOTE: att_target.type_mask is inverted for easier interpretation + att_target.type_mask = att_target.type_mask ^ 0xFF; + + uint8_t attitude_mask = att_target.type_mask & 0b10000111; // q plus rpy + + uint32_t now = AP_HAL::millis(); + if ((attitude_mask & 0b10000001) || // partial, including roll + (attitude_mask == 0b10000000)) { // all angles + plane.guided_state.forced_rpy_cd.x = degrees(q.get_euler_roll()) * 100.0f; // Update timer for external roll to the nav control - plane.guided_state.last_roll_ms = AP_HAL::millis(); + plane.guided_state.last_forced_rpy_ms.x = now; } + + if ((attitude_mask & 0b10000010) || // partial, including pitch + (attitude_mask == 0b10000000)) { // all angles + plane.guided_state.forced_rpy_cd.y = degrees(q.get_euler_pitch()) * 100.0f; + + // Update timer for external pitch to the nav control + plane.guided_state.last_forced_rpy_ms.y = now; + } + + if ((attitude_mask & 0b10000100) || // partial, including yaw + (attitude_mask == 0b10000000)) { // all angles + plane.guided_state.forced_rpy_cd.z = degrees(q.get_euler_yaw()) * 100.0f; + + // Update timer for external yaw to the nav control + plane.guided_state.last_forced_rpy_ms.z = now; + } + if (att_target.type_mask & 0b01000000) { // throttle + plane.guided_state.forced_throttle = att_target.thrust * 100.0f; + + // Update timer for external throttle + plane.guided_state.last_forced_throttle_ms = now; + } + break; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 145a032955..dd049fabef 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -527,12 +527,15 @@ private: } auto_state; struct { - // roll commanded from external controller in centidegrees - int32_t roll_cd; - + // roll pitch yaw commanded from external controller in centidegrees + Vector3l forced_rpy_cd; // last time we heard from the external controller - uint32_t last_roll_ms; - } guided_state; + Vector3l last_forced_rpy_ms; + + // throttle commanded from external controller in percent + float forced_throttle; + uint32_t last_forced_throttle_ms; +} guided_state; struct { // on hard landings, only check once after directly a landing so you diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 203ca13146..b2e67bb95b 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -337,7 +337,8 @@ void Plane::set_mode(enum FlightMode mode) crash_state.impact_detected = false; // reset external attitude guidance - guided_state.last_roll_ms = 0; + guided_state.last_forced_rpy_ms.zero(); + guided_state.last_forced_throttle_ms = 0; // always reset this because we don't know who called set_mode. In evasion // behavior you should set this flag after set_mode so you know the evasion