aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Camera: Added feedback message support

Browse Source 
Now the autopilot can publish the state of the camera via the mavlink_msg_camera_feedback message.
This commit is contained in:
Arthur Benemann 2014-10-31 14:40:27 +09:00 committed by Randy Mackay
parent 8eb866adef
commit 2e4812afb5
2 changed files with 27 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Camera/AP_Camera.cpp
View File

@ -161,6 +161,30 @@ AP_Camera::control_msg(mavlink_message_t* msg)
}
/*
Send camera feedback to the GCS
*/
void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location &current_loc)
{
float altitude, altitude_rel;
if (current_loc.flags.relative_alt) {
altitude = current_loc.alt+ahrs.get_home().alt;
altitude_rel = current_loc.alt;
} else {
altitude = current_loc.alt;
altitude_rel = current_loc.alt - ahrs.get_home().alt;
}
mavlink_msg_camera_feedback_send(chan,
gps.time_epoch_usec(),
0, 0, 0,
current_loc.lat, current_loc.lng,
altitude/100.0, altitude_rel/100.0,
ahrs.roll_sensor/100.0, ahrs.pitch_sensor/100.0, ahrs.yaw_sensor/100.0,
0.0,0);
}
/* update location, for triggering by GPS distance moved
This function returns true if a picture should be taken
The caller is responsible for taking the picture based on the return value of this function.

3
libraries/AP_Camera/AP_Camera.h
View File

@ -10,6 +10,8 @@
#include <AP_Common.h>
#include <GCS_MAVLink.h>
#include <AP_Relay.h>
#include <AP_GPS.h>
#include <AP_AHRS.h>
#define AP_CAMERA_TRIGGER_TYPE_SERVO 0
#define AP_CAMERA_TRIGGER_TYPE_RELAY 1
@ -45,6 +47,7 @@ public:
// MAVLink methods
void configure_msg(mavlink_message_t* msg);
void control_msg(mavlink_message_t* msg);
void send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location &current_loc);
// set camera trigger distance in a mission
void set_trigger_distance(uint32_t distance_m) { _trigg_dist.set(distance_m); }