Skip to content

Commit

Permalink
Control/UAV/Ardupilot: Plane: Add support for GUIDED mode; sending de…
Browse files Browse the repository at this point in the history 
…sired roll/pitch/throttle through GUIDED mode on AP 3.7 and up
  • Loading branch information
@krisgry
krisgry committed Jul 4, 2018
1 parent dae4156 commit c1a6fcc
Showing 1 changed file with 193 additions and 57 deletions.
250 changes: 193 additions & 57 deletions src/Control/UAV/Ardupilot/Task.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,13 @@
// MAVLink headers.
#include <mavlink/ardupilotmega/mavlink.h>

// Not yet included in MAVLINK :(
#define MAVLINK_MSG_ID_SET_ATTITUDE_R (1<<1)
#define MAVLINK_MSG_ID_SET_ATTITUDE_P (1<<2)
#define MAVLINK_MSG_ID_SET_ATTITUDE_Y (1<<3)
#define MAVLINK_MSG_ID_SET_ATTITUDE_THR (1<<7)
#define MAVLINK_MSG_ID_SET_ATTITUDE_ATT (1<<8)

namespace Control
{
namespace UAV
Expand Down Expand Up @@ -223,7 +230,7 @@ namespace Control
//! Vehicle is on ground
bool m_ground;
//! Desired control
float m_droll, m_dclimb, m_dspeed;
float m_droll, m_dclimb, m_dspeed, m_dthrottle,m_dpitch;
//! Type of system to be controlled
APM_Vehicle m_vehicle_type;
//! Check if is in service
Expand Down Expand Up @@ -260,6 +267,8 @@ namespace Control
m_droll(0),
m_dclimb(0),
m_dspeed(20),
m_dthrottle(60),
m_dpitch(0.0),
m_vehicle_type(VEHICLE_UNKNOWN),
m_service(false),
m_last_wp(0)
Expand Down Expand Up @@ -439,6 +448,8 @@ namespace Control
// Setup processing of IMC messages
bind<DesiredPath>(this);
bind<DesiredRoll>(this);
bind<DesiredPitch>(this);
bind<DesiredThrottle>(this);
bind<DesiredZ>(this);
bind<DesiredVerticalRate>(this);
bind<DesiredSpeed>(this);
Expand Down Expand Up @@ -624,53 +635,113 @@ namespace Control

if (cloops->enable)
{
m_cloops |= cloops->mask;
if ((!m_args.ardu_tracker) && (cloops->mask & IMC::CL_PATH))
trace("CLoops enabled");

if (!m_args.ardu_tracker)
{
inf("Ardupilot tracker is NOT enabled");
m_cloops &= ~IMC::CL_PATH;
m_cloops |= cloops->mask;
if (cloops->mask & IMC::CL_ROLL)
{
onUpdateParameters();
if((m_mode!=PL_MODE_FBWB) && !(m_cloops & IMC::CL_PITCH) )
{ // current mode is not GUIDED and neither current or next control loop contains pitch
activateFBWB();
}
}
if(cloops->mask & IMC::CL_PITCH)
{//Pitch has just been activated
deactivateFBWB();
activateGuided();
}
}

if (!(m_args.ardu_tracker) && (cloops->mask & IMC::CL_ROLL))
activateFBW();
}
else
{
trace("CLoops disabled");
m_cloops &= ~cloops->mask;

if ((cloops->mask & IMC::CL_ROLL) && !m_ground && m_vehicle_type == VEHICLE_FIXEDWING)
if(!m_ground && m_vehicle_type == VEHICLE_FIXEDWING)
{
mavlink_message_t msg;
uint8_t buf[512];

//! Sending value 0 disables RC override for that channel
mavlink_msg_rc_channels_override_pack(255, 0, &msg,
1,
1,
0, //! RC Channel 1 (roll)
0, //! RC Channel 2 (vertical rate)
0, //! RC Channel 3 (speed)
0, //! RC Channel 4 (rudder)
0, //! RC Channel 5 (not used)
0, //! RC Channel 6 (not used)
0, //! RC Channel 7 (not used)
0);//! RC Channel 8 (mode)
uint16_t n = mavlink_msg_to_send_buffer(buf, &msg);
sendData(buf, n);
if ((cloops->mask & IMC::CL_ROLL))
{
deactivateFBWB();
}
else if ( (m_cloops & IMC::CL_ROLL) && (cloops->mask & IMC::CL_PITCH) )
{// ROLL is still active, but deactivating pitch. Going from GUIDED to FBWB
activateFBWB();
}
}
}

info(prev, m_cloops, IMC::CL_SPEED, "speed control");
info(prev, m_cloops, IMC::CL_THROTTLE, "throttle control");
info(prev, m_cloops, IMC::CL_ALTITUDE, "altitude control");
info(prev, m_cloops, IMC::CL_VERTICAL_RATE, "vertical rate control");
info(prev, m_cloops, IMC::CL_ROLL, "bank control");
info(prev, m_cloops, IMC::CL_YAW, "heading control");
info(prev, m_cloops, IMC::CL_PATH, "path control");
info(prev, m_cloops, IMC::CL_PITCH, "pitch control");
}

//! Messages for FBWB control (using DUNE's controllers)
void
activateFBW(void)
activateFBWB(void)
{
if (m_vehicle_type == VEHICLE_FIXEDWING)
{
uint8_t buf[512];
mavlink_message_t msg;

mavlink_msg_set_mode_pack(255, 0, &msg,
m_sysid,
1,
PL_MODE_FBWB);

uint16_t n = mavlink_msg_to_send_buffer(buf, &msg);
sendData(buf, n);
trace("Activating FBWB");
}
else
{
debug("Tried to set FBWB on a non-supported vehicle!");
}
}

//! Messages for FBWB control (using DUNE's controllers)
void
deactivateFBWB(void)
{
if (m_vehicle_type == VEHICLE_FIXEDWING)
{
inf("Deactivating RC override (FBWB)");
mavlink_message_t msg;
uint8_t buf[512];

//! Sending value 0 disables RC override for that channel
mavlink_msg_rc_channels_override_pack(255, 0, &msg,
1,
1,
0, //! RC Channel 1 (roll)
0, //! RC Channel 2 (vertical rate/pitch)
0, //! RC Channel 3 (speed)
0, //! RC Channel 4 (rudder)
0, //! RC Channel 5 (not used)
0, //! RC Channel 6 (not used)
0, //! RC Channel 7 (not used)
0);//! RC Channel 8 (mode)
uint16_t n = mavlink_msg_to_send_buffer(buf, &msg);
sendData(buf, n);
}
else
{
debug("Tried to disable FBWB on a non-supported vehicle!");
}
}

//! Messages for GUIDED control (using DUNE's controllers)
void
activateGuided(void)
{
if (m_vehicle_type == VEHICLE_FIXEDWING)
{
Expand All @@ -680,14 +751,15 @@ namespace Control
mavlink_msg_set_mode_pack(255, 0, &msg,
m_sysid,
1,
6); //! FBWB is mode 6
PL_MODE_GUIDED);

uint16_t n = mavlink_msg_to_send_buffer(buf, &msg);
sendData(buf, n);
trace("Activating GUIDED");
}
else
{
debug("Tried to set FBW on a non-supported vehicle!");
debug("Tried to set GUIDED on a non-supported vehicle!");
}
}

Expand Down Expand Up @@ -716,37 +788,76 @@ namespace Control

//! Convert references to PWM and send all

int pwm_roll = map2PWM(m_args.rc1.pwm_min, m_args.rc1.pwm_max,
m_args.rc1.val_min, m_args.rc1.val_max,
m_droll, m_args.rc1.reverse);
if (m_mode == PL_MODE_GUIDED)
{
debug("V1: %f, V2: %f, V3: %f", d_roll->value, Angles::radians(m_dpitch), m_dthrottle);

int pwm_climb = map2PWM(m_args.rc2.pwm_min, m_args.rc2.pwm_max,
m_args.rc2.val_min, m_args.rc2.val_max,
m_dclimb, m_args.rc2.reverse);
uint8_t buf[512];

int pwm_speed = map2PWM(m_args.rc3.pwm_min, m_args.rc3.pwm_max,
m_args.rc3.val_min, m_args.rc3.val_max,
m_dspeed, m_args.rc3.reverse);
mavlink_message_t msg;
// Ignore everything but thr and attitude, interpret att as roll and pitch
uint8_t type_mask = !(MAVLINK_MSG_ID_SET_ATTITUDE_THR & MAVLINK_MSG_ID_SET_ATTITUDE_ATT & MAVLINK_MSG_ID_SET_ATTITUDE_R & MAVLINK_MSG_ID_SET_ATTITUDE_P);
debug("type mask: %d", type_mask);
float d_quat[4];
mavlink_euler_to_quaternion(d_roll->value, Angles::radians(m_dpitch), 0.0, d_quat);
debug("d_quat = [%f, %f, %f, %f]", d_quat[0], d_quat[1], d_quat[2], d_quat[3]);
float roll, pitch, yaw;
mavlink_quaternion_to_euler(d_quat, &roll, &pitch, &yaw);
debug("Back-calc rpy: %f, %f, %f",roll,pitch,yaw);


mavlink_msg_set_attitude_target_pack(255, 0, &msg,
Clock::get(),
m_sysid, //@param target_system System ID
0, //@param target_component Component ID
type_mask,
d_quat,
0, 0, 0, // rpy rate
m_dthrottle*0.01);// percent to 0..1
//sysid = 255? component_id = 0? target_system = target_component = 1?
uint16_t n = mavlink_msg_to_send_buffer(buf, &msg);
sendData(buf, n);
}
else if ( (m_mode == PL_MODE_FBWB) && !(m_cloops & IMC::CL_PITCH) )
{
//FBWB. Pitch might be active, if we are transitioning from FBWB to GUIDED.
// Sending RC override then will result in lock of pitch to LIM_PITCH_MIN/MAX
int pwm_roll = map2PWM(m_args.rc1.pwm_min, m_args.rc1.pwm_max,
m_args.rc1.val_min, m_args.rc1.val_max,
m_droll, m_args.rc1.reverse);

debug("V1: %f, V2: %f, V3: %f", m_droll, m_dclimb, m_dspeed);
debug("RC1: %d, RC2: %d, RC3: %d", pwm_roll, pwm_climb, pwm_speed);
int pwm_climb = map2PWM(m_args.rc2.pwm_min, m_args.rc2.pwm_max,
m_args.rc2.val_min, m_args.rc2.val_max,
m_dclimb, m_args.rc2.reverse);

uint8_t buf[512];
int pwm_speed = map2PWM(m_args.rc3.pwm_min, m_args.rc3.pwm_max,
m_args.rc3.val_min, m_args.rc3.val_max,
m_dspeed, m_args.rc3.reverse);

mavlink_message_t msg;
mavlink_msg_rc_channels_override_pack(255, 0, &msg,
1,
1,
pwm_roll, //! RC Channel 1 (roll)
pwm_climb, //! RC Channel 2 (vertical rate)
pwm_speed, //! RC Channel 3 (speed)
1500, //! RC Channel 4 (rudder)
0, //! RC Channel 5 (not used)
0, //! RC Channel 6 (not used)
0, //! RC Channel 7 (not used)
0);//! RC Channel 8 (mode - do not override)
uint16_t n = mavlink_msg_to_send_buffer(buf, &msg);
sendData(buf, n);
debug("V1: %f, V2: %f, V3: %f", m_droll, m_dclimb, m_dspeed);
debug("RC1: %d, RC2: %d, RC3: %d", pwm_roll, pwm_climb, pwm_speed);

uint8_t buf[512];

mavlink_message_t msg;
mavlink_msg_rc_channels_override_pack(255, 0, &msg,
1,
1,
pwm_roll, //! RC Channel 1 (roll)
pwm_climb, //! RC Channel 2 (vertical rate)
pwm_speed, //! RC Channel 3 (speed)
1500, //! RC Channel 4 (rudder)
0, //! RC Channel 5 (not used)
0, //! RC Channel 6 (not used)
0, //! RC Channel 7 (not used)
0);//! RC Channel 8 (mode - do not override)
uint16_t n = mavlink_msg_to_send_buffer(buf, &msg);
sendData(buf, n);
}
else
{
inf("Received DesiredRoll, but not in FBWB or GUIDED");
}
}

void
Expand Down Expand Up @@ -873,6 +984,32 @@ namespace Control
m_dspeed = d_speed->value;
}

void
consume(const IMC::DesiredPitch* d_pitch)
{
if(!(m_cloops & IMC::CL_PITCH))
{
inf(DTR("pitch control is NOT active"));
return;
}

//! Saving value
m_dpitch = Angles::degrees(d_pitch->value);
}

void
consume(const IMC::DesiredThrottle* d_throttle)
{
if(!(m_cloops & IMC::CL_THROTTLE))
{
inf(DTR("throttle control is NOT active"));
return;
}

//! Saving value
m_dthrottle = d_throttle->value;
}

//! Converts value in range min_value:max_value to a value_pwm in range min_pwm:max_pwm
int
map2PWM(int min_pwm, int max_pwm, float min_value, float max_value, float value, bool reverse)
Expand All @@ -898,7 +1035,7 @@ namespace Control
return;
}

if (!((m_cloops & IMC::CL_PATH) && m_args.ardu_tracker))
if (!m_args.ardu_tracker)
{
inf(DTR("path control is NOT active"));
return;
Expand All @@ -907,7 +1044,6 @@ namespace Control
//! In Auto mode but still in ground, performing takeoff first
if (m_ground)
{

inf(DTR("ArduPilot in Auto mode but still in ground, performing takeoff first."));
if (m_vehicle_type == VEHICLE_COPTER)
{
Expand Down Expand Up @@ -2171,7 +2307,7 @@ namespace Control
if (m_vehicle_type == VEHICLE_COPTER)
is_valid_mode = (m_mode == CP_MODE_GUIDED || (m_mode == CP_MODE_AUTO )) ? true : false;
else
is_valid_mode = (m_mode == 15 || (m_mode == 10 && m_current_wp == 3)) ? true : false;
is_valid_mode = (m_mode == PL_MODE_GUIDED || (m_mode == PL_MODE_AUTO && m_current_wp == 3)) ? true : false;

// Check Loiter tolerance
if (m_vehicle_type == VEHICLE_COPTER)
Expand Down

0 comments on commit c1a6fcc

Please sign in to comment.