mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
GCS_MAVLink: move common mavlink camera handling up
This commit is contained in:
parent
bb06b02df6
commit
b4688bd3d4
@ -18,6 +18,7 @@
|
||||
#include <AP_HAL/utility/RingBuffer.h>
|
||||
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
|
||||
#include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
|
||||
#include <AP_Camera/AP_Camera.h>
|
||||
|
||||
// check if a message will fit in the payload space available
|
||||
#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN)
|
||||
@ -222,6 +223,7 @@ protected:
|
||||
virtual AP_Mission *get_mission() = 0;
|
||||
virtual AP_Rally *get_rally() const = 0;
|
||||
virtual Compass *get_compass() const = 0;
|
||||
virtual class AP_Camera *get_camera() const = 0;
|
||||
virtual AP_ServoRelayEvents *get_servorelayevents() const = 0;
|
||||
virtual AP_GPS *get_gps() const = 0;
|
||||
|
||||
@ -258,7 +260,7 @@ protected:
|
||||
void handle_common_rally_message(mavlink_message_t *msg);
|
||||
void handle_rally_fetch_point(mavlink_message_t *msg);
|
||||
void handle_rally_point(mavlink_message_t *msg);
|
||||
|
||||
void handle_common_camera_message(const mavlink_message_t *msg);
|
||||
void handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const;
|
||||
void handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio);
|
||||
void handle_serial_control(mavlink_message_t *msg, AP_GPS &gps);
|
||||
@ -280,6 +282,7 @@ protected:
|
||||
MAV_RESULT handle_command_preflight_set_sensor_offsets(const mavlink_command_long_t &packet);
|
||||
MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet);
|
||||
MAV_RESULT handle_command_long_message(mavlink_command_long_t &packet);
|
||||
MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet);
|
||||
|
||||
private:
|
||||
|
||||
|
@ -1744,6 +1744,64 @@ void GCS_MAVLINK::handle_common_gps_message(mavlink_message_t *msg)
|
||||
gps->handle_msg(msg);
|
||||
}
|
||||
|
||||
|
||||
void GCS_MAVLINK::handle_common_camera_message(const mavlink_message_t *msg)
|
||||
{
|
||||
AP_Camera *camera = get_camera();
|
||||
if (camera == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: // MAV ID: 202
|
||||
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
|
||||
break;
|
||||
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
|
||||
//deprecated. Use MAV_CMD_DO_DIGICAM_CONTROL
|
||||
camera->control_msg(msg);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &packet)
|
||||
{
|
||||
AP_Camera *camera = get_camera();
|
||||
if (camera == nullptr) {
|
||||
return MAV_RESULT_UNSUPPORTED;
|
||||
}
|
||||
|
||||
MAV_RESULT result = MAV_RESULT_FAILED;
|
||||
switch (packet.command) {
|
||||
case MAV_CMD_DO_DIGICAM_CONFIGURE:
|
||||
camera->configure(packet.param1,
|
||||
packet.param2,
|
||||
packet.param3,
|
||||
packet.param4,
|
||||
packet.param5,
|
||||
packet.param6,
|
||||
packet.param7);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
break;
|
||||
case MAV_CMD_DO_DIGICAM_CONTROL:
|
||||
camera->control(packet.param1,
|
||||
packet.param2,
|
||||
packet.param3,
|
||||
packet.param4,
|
||||
packet.param5,
|
||||
packet.param6);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
break;
|
||||
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
|
||||
camera->set_trigger_distance(packet.param1);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
break;
|
||||
default:
|
||||
result = MAV_RESULT_UNSUPPORTED;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/*
|
||||
handle messages which don't require vehicle specific data
|
||||
*/
|
||||
@ -1778,6 +1836,13 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
|
||||
break;
|
||||
|
||||
|
||||
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:
|
||||
/* fall through */
|
||||
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
|
||||
/* fall through */
|
||||
handle_common_camera_message(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
|
||||
/* fall through */
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
|
||||
@ -1935,6 +2000,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &pack
|
||||
result = handle_rc_bind(packet);
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_DIGICAM_CONFIGURE:
|
||||
/* fall through */
|
||||
case MAV_CMD_DO_DIGICAM_CONTROL:
|
||||
/* fall through */
|
||||
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
|
||||
result = handle_command_camera(packet);
|
||||
break;
|
||||
|
||||
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: {
|
||||
result = handle_command_preflight_set_sensor_offsets(packet);
|
||||
break;
|
||||
|
@ -18,6 +18,7 @@ protected:
|
||||
AP_Mission *get_mission() override { return nullptr; }
|
||||
AP_Rally *get_rally() const override { return nullptr; };
|
||||
AP_GPS *get_gps() const override { return nullptr; };
|
||||
AP_Camera *get_camera() const override { return nullptr; };
|
||||
AP_ServoRelayEvents *get_servorelayevents() const override { return nullptr; }
|
||||
|
||||
uint8_t sysid_my_gcs() const override { return 1; }
|
||||
|
@ -26,6 +26,7 @@ protected:
|
||||
AP_Rally *get_rally() const override { return nullptr; }
|
||||
AP_ServoRelayEvents *get_servorelayevents() const override { return nullptr; }
|
||||
AP_GPS *get_gps() const override { return nullptr; };
|
||||
AP_Camera *get_camera() const override { return nullptr; };
|
||||
uint8_t sysid_my_gcs() const override { return 1; }
|
||||
|
||||
private:
|
||||
@ -122,5 +123,10 @@ void loop(void)
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
|
||||
/* dummy methods to avoid having to link against AP_Camera */
|
||||
void AP_Camera::control_msg(mavlink_message_t const*) {}
|
||||
void AP_Camera::configure(float, float, float, float, float, float, float) {}
|
||||
void AP_Camera::control(float, float, float, float, float, float) {}
|
||||
/* end dummy methods to avoid having to link against AP_Camera */
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
Loading…
Reference in New Issue
Block a user