Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: move handling of gimbal messages up

Browse Source 
Copter: move handling of mav_cmd_do_mount_control up

Copter: move handling of deprecated mount messages up

Copter: move handling of command_do_set_roi up

Copter: move handling of command_do_set_roi up

Copter: mount no longer takes ahrs in constructor
This commit is contained in:
Peter Barker 2018-10-14 09:24:43 +11:00 committed by Peter Barker
parent 178d26f8e3
commit 859f4828a1
3 changed files with 53 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -504,7 +504,7 @@ private:
// Camera/Antenna mount tracking and stabilisation stuff
#if MOUNT == ENABLED
// current_loc uses the baro/gps solution for altitude rather than gps only.
AP_Mount camera_mount{ahrs, current_loc};
AP_Mount camera_mount{current_loc};
#endif
// AC_Fence library to reduce fly-aways

144
ArduCopter/GCS_Mavlink.cpp
View File

@ -322,13 +322,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
#endif
break;
case MSG_MOUNT_STATUS:
#if MOUNT == ENABLED
CHECK_PAYLOAD_SIZE(MOUNT_STATUS);
copter.camera_mount.status_msg(chan);
#endif // MOUNT == ENABLED
break;
case MSG_OPTICAL_FLOW:
#if OPTFLOW == ENABLED
CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
@ -336,13 +329,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
#endif
break;
case MSG_GIMBAL_REPORT:
#if MOUNT == ENABLED
CHECK_PAYLOAD_SIZE(GIMBAL_REPORT);
copter.camera_mount.send_gimbal_report(chan);
#endif
break;
case MSG_WIND:
case MSG_SERVO_OUT:
case MSG_AOA_SSA:
@ -602,6 +588,15 @@ MAV_RESULT GCS_MAVLINK_Copter::_handle_command_preflight_calibration(const mavli
}
MAV_RESULT GCS_MAVLINK_Copter::handle_command_do_set_roi(const Location &roi_loc)
{
if (!check_latlng(roi_loc)) {
return MAV_RESULT_FAILED;
}
copter.flightmode->auto_yaw.set_roi(roi_loc);
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_int_t &packet)
{
switch(packet.command) {
@ -657,30 +652,30 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
return MAV_RESULT_ACCEPTED;
}
case MAV_CMD_DO_SET_ROI: {
// param1 : /* Region of interest mode (not used)*/
// param2 : /* MISSION index/ target ID (not used)*/
// param3 : /* ROI index (not used)*/
// param4 : /* empty */
// x : lat
// y : lon
// z : alt
// sanity check location
if (!check_latlng(packet.x, packet.y)) {
return MAV_RESULT_FAILED;
}
Location roi_loc;
roi_loc.lat = packet.x;
roi_loc.lng = packet.y;
roi_loc.alt = (int32_t)(packet.z * 100.0f);
copter.flightmode->auto_yaw.set_roi(roi_loc);
return MAV_RESULT_ACCEPTED;
}
default:
return GCS_MAVLINK::handle_command_int_packet(packet);
}
}
MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t &packet)
{
// if the mount doesn't do pan control then yaw the entire vehicle instead:
switch (packet.command) {
#if MOUNT == ENABLED
case MAV_CMD_DO_MOUNT_CONTROL:
if(!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
(float)packet.param3 / 100.0f,
0.0f,
0,0);
}
break;
#endif
default:
break;
}
return GCS_MAVLINK::handle_command_mount(packet);
}
MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet)
{
@ -791,36 +786,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
}
return MAV_RESULT_FAILED;
case MAV_CMD_DO_SET_ROI:
// param1 : regional of interest mode (not supported)
// param2 : mission index/ target id (not supported)
// param3 : ROI index (not supported)
// param5 : x / lat
// param6 : y / lon
// param7 : z / alt
// sanity check location
if (!check_latlng(packet.param5, packet.param6)) {
return MAV_RESULT_FAILED;
}
Location roi_loc;
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
copter.flightmode->auto_yaw.set_roi(roi_loc);
return MAV_RESULT_ACCEPTED;
#if MOUNT == ENABLED
case MAV_CMD_DO_MOUNT_CONTROL:
if(!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
(float)packet.param3 / 100.0f,
0.0f,
0,0);
}
copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
return MAV_RESULT_ACCEPTED;
#endif
#if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_MISSION_START:
if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
@ -1018,6 +983,25 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
}
}
void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t* msg)
{
switch (msg->msgid) {
#if MOUNT == ENABLED
case MAVLINK_MSG_ID_MOUNT_CONTROL:
if(!copter.camera_mount.has_pan_control()) {
// if the mount doesn't do pan control then yaw the entire vehicle instead:
copter.flightmode->auto_yaw.set_fixed_yaw(
mavlink_msg_mount_control_get_input_c(msg)/100.0f,
0.0f,
0,
0);
break;
}
#endif
}
GCS_MAVLINK::handle_mount_message(msg);
}
void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
{
@ -1031,22 +1015,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE:
{
#if MOUNT == ENABLED
copter.camera_mount.handle_param_value(msg);
#endif
break;
}
case MAVLINK_MSG_ID_GIMBAL_REPORT:
{
#if MOUNT == ENABLED
handle_gimbal_report(copter.camera_mount, msg);
#endif
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: // MAV ID: 70
{
// allow override of RC channel values for HIL
@ -1414,24 +1382,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
break;
#endif // AC_FENCE == ENABLED
#if MOUNT == ENABLED
//deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // MAV ID: 204
copter.camera_mount.configure_msg(msg);
break;
//deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
case MAVLINK_MSG_ID_MOUNT_CONTROL:
if(!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
mavlink_msg_mount_control_get_input_c(msg)/100.0f,
0.0f,
0,
0);
}
copter.camera_mount.control_msg(msg);
break;
#endif // MOUNT == ENABLED
case MAVLINK_MSG_ID_TERRAIN_DATA:
case MAVLINK_MSG_ID_TERRAIN_CHECK:
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN

5
ArduCopter/GCS_Mavlink.h
View File

@ -33,9 +33,14 @@ protected:
void send_position_target_global_int() override;
MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;
MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override;
MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override;
MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;
void handle_mount_message(const mavlink_message_t* msg) override;
private:
void handleMessage(mavlink_message_t * msg) override;