mirror of https://github.com/ArduPilot/ardupilot
AP_Mission: add DO_GIMBAL_MANAGER_PITCHYAW support
This commit is contained in:
parent
640a4b1a5f
commit
589102b7d7
|
@ -363,6 +363,8 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
|
||||||
return start_command_do_sprayer(cmd);
|
return start_command_do_sprayer(cmd);
|
||||||
case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
|
case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
|
||||||
return command_do_set_repeat_dist(cmd);
|
return command_do_set_repeat_dist(cmd);
|
||||||
|
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
|
||||||
|
return start_command_do_gimbal_manager_pitchyaw(cmd);
|
||||||
default:
|
default:
|
||||||
return _cmd_start_fn(cmd);
|
return _cmd_start_fn(cmd);
|
||||||
}
|
}
|
||||||
|
@ -1202,6 +1204,15 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
|
||||||
cmd.p1 = packet.param1;
|
cmd.p1 = packet.param1;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
|
||||||
|
cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg = packet.param1;
|
||||||
|
cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg = packet.param2;
|
||||||
|
cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs = packet.param3;
|
||||||
|
cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs = packet.param4;
|
||||||
|
cmd.content.gimbal_manager_pitchyaw.flags = packet.x;
|
||||||
|
cmd.content.gimbal_manager_pitchyaw.gimbal_id = packet.z;
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
// unrecognised command
|
// unrecognised command
|
||||||
return MAV_MISSION_UNSUPPORTED;
|
return MAV_MISSION_UNSUPPORTED;
|
||||||
|
@ -1289,6 +1300,7 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const ma
|
||||||
case MAV_CMD_DO_DIGICAM_CONTROL:
|
case MAV_CMD_DO_DIGICAM_CONTROL:
|
||||||
case MAV_CMD_DO_DIGICAM_CONFIGURE:
|
case MAV_CMD_DO_DIGICAM_CONFIGURE:
|
||||||
case MAV_CMD_NAV_ATTITUDE_TIME:
|
case MAV_CMD_NAV_ATTITUDE_TIME:
|
||||||
|
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
|
||||||
mav_cmd.x = packet.x;
|
mav_cmd.x = packet.x;
|
||||||
mav_cmd.y = packet.y;
|
mav_cmd.y = packet.y;
|
||||||
break;
|
break;
|
||||||
|
@ -1331,6 +1343,7 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(const ma
|
||||||
case MAV_CMD_DO_DIGICAM_CONTROL:
|
case MAV_CMD_DO_DIGICAM_CONTROL:
|
||||||
case MAV_CMD_DO_DIGICAM_CONFIGURE:
|
case MAV_CMD_DO_DIGICAM_CONFIGURE:
|
||||||
case MAV_CMD_NAV_ATTITUDE_TIME:
|
case MAV_CMD_NAV_ATTITUDE_TIME:
|
||||||
|
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
|
||||||
item.x = item_int.x;
|
item.x = item_int.x;
|
||||||
item.y = item_int.y;
|
item.y = item_int.y;
|
||||||
break;
|
break;
|
||||||
|
@ -1678,6 +1691,15 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
|
||||||
packet.param1 = cmd.p1;
|
packet.param1 = cmd.p1;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
|
||||||
|
packet.param1 = cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg;
|
||||||
|
packet.param2 = cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg;
|
||||||
|
packet.param3 = cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs;
|
||||||
|
packet.param4 = cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs;
|
||||||
|
packet.x = cmd.content.gimbal_manager_pitchyaw.flags;
|
||||||
|
packet.z = cmd.content.gimbal_manager_pitchyaw.gimbal_id;
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
// unrecognised command
|
// unrecognised command
|
||||||
return false;
|
return false;
|
||||||
|
@ -2402,7 +2424,8 @@ const char *AP_Mission::Mission_Command::type() const
|
||||||
return "NavAttitudeTime";
|
return "NavAttitudeTime";
|
||||||
case MAV_CMD_DO_PAUSE_CONTINUE:
|
case MAV_CMD_DO_PAUSE_CONTINUE:
|
||||||
return "PauseContinue";
|
return "PauseContinue";
|
||||||
|
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
|
||||||
|
return "GimbalPitchYaw";
|
||||||
default:
|
default:
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
AP_HAL::panic("Mission command with ID %u has no string", id);
|
AP_HAL::panic("Mission command with ID %u has no string", id);
|
||||||
|
|
|
@ -239,6 +239,16 @@ public:
|
||||||
float climb_rate;
|
float climb_rate;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW support
|
||||||
|
struct PACKED gimbal_manager_pitchyaw_Command {
|
||||||
|
int8_t pitch_angle_deg;
|
||||||
|
int16_t yaw_angle_deg;
|
||||||
|
int8_t pitch_rate_degs;
|
||||||
|
int8_t yaw_rate_degs;
|
||||||
|
uint8_t flags;
|
||||||
|
uint8_t gimbal_id;
|
||||||
|
};
|
||||||
|
|
||||||
union Content {
|
union Content {
|
||||||
// jump structure
|
// jump structure
|
||||||
Jump_Command jump;
|
Jump_Command jump;
|
||||||
|
@ -315,6 +325,9 @@ public:
|
||||||
// nav attitude time
|
// nav attitude time
|
||||||
nav_attitude_time_Command nav_attitude_time;
|
nav_attitude_time_Command nav_attitude_time;
|
||||||
|
|
||||||
|
// MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW
|
||||||
|
gimbal_manager_pitchyaw_Command gimbal_manager_pitchyaw;
|
||||||
|
|
||||||
// location
|
// location
|
||||||
Location location{}; // Waypoint location
|
Location location{}; // Waypoint location
|
||||||
};
|
};
|
||||||
|
@ -760,7 +773,7 @@ private:
|
||||||
|
|
||||||
bool start_command_do_sprayer(const AP_Mission::Mission_Command& cmd);
|
bool start_command_do_sprayer(const AP_Mission::Mission_Command& cmd);
|
||||||
bool start_command_do_scripting(const AP_Mission::Mission_Command& cmd);
|
bool start_command_do_scripting(const AP_Mission::Mission_Command& cmd);
|
||||||
|
bool start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd);
|
||||||
};
|
};
|
||||||
|
|
||||||
namespace AP
|
namespace AP
|
||||||
|
|
|
@ -8,6 +8,7 @@
|
||||||
#include <AC_Sprayer/AC_Sprayer.h>
|
#include <AC_Sprayer/AC_Sprayer.h>
|
||||||
#include <AP_Scripting/AP_Scripting.h>
|
#include <AP_Scripting/AP_Scripting.h>
|
||||||
#include <RC_Channel/RC_Channel.h>
|
#include <RC_Channel/RC_Channel.h>
|
||||||
|
#include <AP_Mount/AP_Mount.h>
|
||||||
|
|
||||||
bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd)
|
bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd)
|
||||||
{
|
{
|
||||||
|
@ -204,3 +205,40 @@ bool AP_Mission::start_command_do_scripting(const AP_Mission::Mission_Command& c
|
||||||
return false;
|
return false;
|
||||||
#endif // AP_SCRIPTING_ENABLED
|
#endif // AP_SCRIPTING_ENABLED
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd)
|
||||||
|
{
|
||||||
|
#if HAL_MOUNT_ENABLED
|
||||||
|
AP_Mount *mount = AP::mount();
|
||||||
|
if (mount == nullptr) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
// check flags for change to RETRACT
|
||||||
|
if ((cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
|
||||||
|
mount->set_mode(MAV_MOUNT_MODE_RETRACT);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
// check flags for change to NEUTRAL
|
||||||
|
if ((cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
|
||||||
|
mount->set_mode(MAV_MOUNT_MODE_NEUTRAL);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// To-Do: handle earth-frame vs body-frame angles
|
||||||
|
//bool earth_frame = cmd.content.gimbal_manager_pitchyaw.flags.GIMBAL_MANAGER_FLAGS_ROLL_LOCK |
|
||||||
|
// cmd.content.gimbal_manager_pitchyaw.flags.GIMBAL_MANAGER_FLAGS_PITCH_LOCK |
|
||||||
|
// cmd.content.gimbal_manager_pitchyaw.flags.GIMBAL_MANAGER_FLAGS_YAW_LOCK;
|
||||||
|
// To-Do: handle pitch and yaw rates
|
||||||
|
// To-Do: handle gimbal device id
|
||||||
|
|
||||||
|
if (!isnan(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) && !isnan(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg)) {
|
||||||
|
mount->set_angle_targets(0, cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg, cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// if we got this far then message is not handled
|
||||||
|
return false;
|
||||||
|
#else
|
||||||
|
return false;
|
||||||
|
#endif // HAL_MOUNT_ENABLED
|
||||||
|
}
|
||||||
|
|
Loading…
Reference in New Issue