Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: allow SET_ATTITUDE_TARGET to set rpy and throttle

Browse Source
This commit is contained in:
Tom Pittenger 2016-06-30 08:50:17 -07:00
parent 3377714300
commit 0597dbd033
4 changed files with 100 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
ArduPlane/Attitude.cpp
View File

@ -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);

62
ArduPlane/GCS_Mavlink.cpp
View File

@ -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;
}

13
ArduPlane/Plane.h
View File

@ -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

3
ArduPlane/system.cpp
View File

@ -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