aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: remove guided roll flag, handle it with timer=0

Browse Source
This commit is contained in:
Tom Pittenger 2016-06-30 07:48:37 -07:00
parent 8ac433f991
commit 85afd9e245
4 changed files with 18 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/Attitude.cpp
View File

@ -531,10 +531,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.guiding_roll &&
AP_HAL::millis() - plane.guided_state.last_guided_ms < 3000) {
commanded_roll = plane.guided_state.guided_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;
}
nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd);

25
ArduPlane/GCS_Mavlink.cpp
View File

@ -2105,20 +2105,20 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
{
//Only allow companion computer (or other external controller) to
//control attitude in GUIDED mode. We DON'T want external control
//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).
// Only allow companion computer (or other external controller) to
// control attitude in GUIDED mode. We DON'T want external control
// 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
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.
// 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) {
@ -2127,13 +2127,10 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
att_target.q[2], att_target.q[3]);
float roll_rad = q.get_euler_roll();
plane.guided_state.guided_roll_cd = degrees(roll_rad) * 100.f;
// Set the flag for external roll to the nav control
plane.guided_state.guiding_roll = true;
// Update timer:
plane.guided_state.last_guided_ms = AP_HAL::millis();
plane.guided_state.roll_cd = degrees(roll_rad) * 100.0f;
// Update timer for external roll to the nav control
plane.guided_state.last_roll_ms = AP_HAL::millis();
}
break;
}

6
ArduPlane/Plane.h
View File

@ -527,13 +527,11 @@ private:
} auto_state;
struct {
// guiding roll from an external controller? (e.g., companion computer)
bool guiding_roll:1;
// roll commanded from external controller in centidegrees
int32_t guided_roll_cd;
int32_t roll_cd;
// last time we heard from the external controller
uint32_t last_guided_ms;
uint32_t last_roll_ms;
} guided_state;
struct {

2
ArduPlane/system.cpp
View File

@ -337,7 +337,7 @@ void Plane::set_mode(enum FlightMode mode)
crash_state.impact_detected = false;
// reset external attitude guidance
guided_state.guiding_roll = false;
guided_state.last_roll_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