GCS_MAVLink: move common mavlink camera handling up

This commit is contained in:
Peter Barker 2017-07-26 15:48:01 +10:00 committed by Francisco Ferreira
parent bb06b02df6
commit b4688bd3d4
4 changed files with 84 additions and 1 deletions

View File

@ -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:

View File

@ -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;

View File

@ -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; }

View File

@ -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();