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

AP_Camera: frontend-backend split

Browse Source 
logging gets instance and shorten Pitch field name to Pit
This commit is contained in:
Randy Mackay 2023-02-11 09:27:39 +09:00 committed by Andrew Tridgell
parent 349dd5089c
commit 50bcf1f278
18 changed files with 1301 additions and 653 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

702
libraries/AP_Camera/AP_Camera.cpp
View File

@ -13,319 +13,190 @@
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include <AP_Mount/AP_Mount.h> #include <AP_Mount/AP_Mount.h>
#include "AP_Camera_Backend.h"
#include "AP_Camera_Servo.h"
#include "AP_Camera_Relay.h"
#include "AP_Camera_Mount.h"
#include "AP_Camera_MAVLink.h"
#include "AP_Camera_SoloGimbal.h" #include "AP_Camera_SoloGimbal.h"
// ------------------------------
#define CAM_DEBUG DISABLED
const AP_Param::GroupInfo AP_Camera::var_info[] = { const AP_Param::GroupInfo AP_Camera::var_info[] = {
// @Param: TRIGG_TYPE
// @DisplayName: Camera shutter (trigger) type
// @Description: how to trigger the camera to take a picture
// @Values: 0:Servo,1:Relay, 2:GoPro in Solo Gimbal, 3:Mount (Siyi)
// @User: Standard
AP_GROUPINFO("TRIGG_TYPE", 0, AP_Camera, _trigger_type, 0),
// @Param: DURATION // @Param: _MAX_ROLL
// @DisplayName: Duration that shutter is held open
// @Description: How long the shutter will be held open in 10ths of a second (i.e. enter 10 for 1second, 50 for 5seconds)
// @Units: ds
// @Range: 0 50
// @User: Standard
AP_GROUPINFO("DURATION", 1, AP_Camera, _trigger_duration, AP_CAMERA_TRIGGER_DEFAULT_DURATION),
// @Param: SERVO_ON
// @DisplayName: Servo ON PWM value
// @Description: PWM value in microseconds to move servo to when shutter is activated
// @Units: PWM
// @Range: 1000 2000
// @User: Standard
AP_GROUPINFO("SERVO_ON", 2, AP_Camera, _servo_on_pwm, AP_CAMERA_SERVO_ON_PWM),
// @Param: SERVO_OFF
// @DisplayName: Servo OFF PWM value
// @Description: PWM value in microseconds to move servo to when shutter is deactivated
// @Units: PWM
// @Range: 1000 2000
// @User: Standard
AP_GROUPINFO("SERVO_OFF", 3, AP_Camera, _servo_off_pwm, AP_CAMERA_SERVO_OFF_PWM),
// @Param: TRIGG_DIST
// @DisplayName: Camera trigger distance
// @Description: Distance in meters between camera triggers. If this value is non-zero then the camera will trigger whenever the position changes by this number of meters regardless of what mode the APM is in. Note that this parameter can also be set in an auto mission using the DO_SET_CAM_TRIGG_DIST command, allowing you to enable/disable the triggering of the camera during the flight.
// @User: Standard
// @Units: m
// @Range: 0 1000
AP_GROUPINFO("TRIGG_DIST", 4, AP_Camera, _trigg_dist, 0),
// @Param: RELAY_ON
// @DisplayName: Relay ON value
// @Description: This sets whether the relay goes high or low when it triggers. Note that you should also set RELAY_DEFAULT appropriately for your camera
// @Values: 0:Low,1:High
// @User: Standard
AP_GROUPINFO("RELAY_ON", 5, AP_Camera, _relay_on, 1),
// @Param: MIN_INTERVAL
// @DisplayName: Minimum time between photos
// @Description: Postpone shooting if previous picture was taken less than preset time(ms) ago.
// @User: Standard
// @Units: ms
// @Range: 0 10000
AP_GROUPINFO("MIN_INTERVAL", 6, AP_Camera, _min_interval, 0),
// @Param: MAX_ROLL
// @DisplayName: Maximum photo roll angle. // @DisplayName: Maximum photo roll angle.
// @Description: Postpone shooting if roll is greater than limit. (0=Disable, will shoot regardless of roll). // @Description: Postpone shooting if roll is greater than limit. (0=Disable, will shoot regardless of roll).
// @User: Standard // @User: Standard
// @Units: deg // @Units: deg
// @Range: 0 180 // @Range: 0 180
AP_GROUPINFO("MAX_ROLL", 7, AP_Camera, _max_roll, 0), AP_GROUPINFO("_MAX_ROLL", 7, AP_Camera, _max_roll, 0),
// @Param: FEEDBACK_PIN // @Param: _AUTO_ONLY
// @DisplayName: Camera feedback pin
// @Description: pin number to use for save accurate camera feedback messages. If set to -1 then don't use a pin flag for this, otherwise this is a pin number which if held high after a picture trigger order, will save camera messages when camera really takes a picture. A universal camera hot shoe is needed. The pin should be held high for at least 2 milliseconds for reliable trigger detection. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. See also the CAM_FEEDBACK_POL option.
// @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("FEEDBACK_PIN", 8, AP_Camera, _feedback_pin, AP_CAMERA_FEEDBACK_DEFAULT_FEEDBACK_PIN),
// @Param: FEEDBACK_POL
// @DisplayName: Camera feedback pin polarity
// @Description: Polarity for feedback pin. If this is 1 then the feedback pin should go high on trigger. If set to 0 then it should go low
// @Values: 0:TriggerLow,1:TriggerHigh
// @User: Standard
AP_GROUPINFO("FEEDBACK_POL", 9, AP_Camera, _feedback_polarity, 1),
// @Param: AUTO_ONLY
// @DisplayName: Distance-trigging in AUTO mode only // @DisplayName: Distance-trigging in AUTO mode only
// @Description: When enabled, trigging by distance is done in AUTO mode only. // @Description: When enabled, trigging by distance is done in AUTO mode only.
// @Values: 0:Always,1:Only when in AUTO // @Values: 0:Always,1:Only when in AUTO
// @User: Standard // @User: Standard
AP_GROUPINFO("AUTO_ONLY", 10, AP_Camera, _auto_mode_only, 0), AP_GROUPINFO("_AUTO_ONLY", 10, AP_Camera, _auto_mode_only, 0),
// @Param: TYPE // @Group: 1
// @DisplayName: Type of camera (0: None, 1: BMMCC) // @Path: AP_Camera_Params.cpp
// @Description: Set the camera type that is being used, certain cameras have custom functions that need further configuration, this enables that. AP_SUBGROUPINFO(_params[0], "1", 12, AP_Camera, AP_Camera_Params),
// @Values: 0:Default,1:BMMCC
// @User: Standard #if AP_CAMERA_MAX_INSTANCES > 1
AP_GROUPINFO("TYPE", 11, AP_Camera, _type, 0), // @Group: 2
// @Path: AP_Camera_Params.cpp
AP_SUBGROUPINFO(_params[1], "2", 13, AP_Camera, AP_Camera_Params),
#endif
AP_GROUPEND AP_GROUPEND
}; };
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
/// Servo operated camera AP_Camera::AP_Camera(uint32_t _log_camera_bit) :
void log_camera_bit(_log_camera_bit)
AP_Camera::servo_pic()
{ {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _servo_on_pwm); AP_Param::setup_object_defaults(this, var_info);
_singleton = this;
// leave a message that it should be active for this many loops (assumes 50hz loops)
_trigger_counter = constrain_int16(_trigger_duration*5,0,255);
} }
/// basic relay activation // returns camera type of the given instance
void AP_Camera::CameraType AP_Camera::get_type(uint8_t instance) const
AP_Camera::relay_pic()
{ {
AP_Relay *_apm_relay = AP::relay(); if (instance >= AP_CAMERA_MAX_INSTANCES) {
if (_apm_relay == nullptr) { return CameraType::NONE;
}
return (CameraType)_params[instance].type.get();
}
// detect and initialise backends
void AP_Camera::init()
{
// check init has not been called before
if (_num_instances != 0) {
return; return;
} }
if (_relay_on) {
_apm_relay->on(0);
} else {
_apm_relay->off(0);
}
// leave a message that it should be active for this many loops (assumes 50hz loops) // perform any required parameter conversion
_trigger_counter = constrain_int16(_trigger_duration*5,0,255); convert_params();
}
/// single entry point to take pictures // create each instance
void AP_Camera::trigger_pic() bool primary_set = false;
{ for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
setup_feedback_callback(); CameraType camera_type = get_type(instance);
// check for servo camera
if (camera_type == CameraType::SERVO) {
_backends[instance] = new AP_Camera_Servo(*this, _params[instance], instance);
_num_instances++;
// check for relay camera
} else if (camera_type == CameraType::RELAY) {
_backends[instance] = new AP_Camera_Relay(*this, _params[instance], instance);
_num_instances++;
_image_index++;
switch (get_trigger_type()) {
case CamTrigType::servo:
servo_pic(); // Servo operated camera
break;
case CamTrigType::relay:
relay_pic(); // basic relay activation
break;
#if HAL_SOLO_GIMBAL_ENABLED #if HAL_SOLO_GIMBAL_ENABLED
case CamTrigType::gopro: // gopro in Solo Gimbal // check for GoPro in Solo camera
AP_Camera_SoloGimbal::gopro_shutter_toggle(); } else if (camera_type == CameraType::SOLOGIMBAL) {
break; _backends[instance] = new AP_Camera_SoloGimbal(*this, _params[instance], instance);
_num_instances++;
#endif #endif
#if HAL_MOUNT_ENABLED
case CamTrigType::mount: {
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
mount->take_picture(0);
}
break;
}
#endif
default:
break;
}
log_picture(); // check for Mount camera
} } else if (camera_type == CameraType::MOUNT) {
_backends[instance] = new AP_Camera_Mount(*this, _params[instance], instance);
_num_instances++;
/// de-activate the trigger after some delay, but without using a delay() function // check for MAVLink enabled camera driver
/// should be called at 50hz } else if (camera_type == CameraType::MAVLINK) {
void _backends[instance] = new AP_Camera_MAVLink(*this, _params[instance], instance);
AP_Camera::trigger_pic_cleanup() _num_instances++;
{
if (_trigger_counter) {
_trigger_counter--;
} else {
switch (get_trigger_type()) {
case CamTrigType::servo:
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _servo_off_pwm);
break;
case CamTrigType::relay: {
AP_Relay *_apm_relay = AP::relay();
if (_apm_relay == nullptr) {
break;
}
if (_relay_on) {
_apm_relay->off(0);
} else {
_apm_relay->on(0);
}
break;
} }
case CamTrigType::gopro:
case CamTrigType::mount: // set primary to first non-null index
// nothing to do if (!primary_set && (_backends[instance] != nullptr)) {
break; primary = instance;
primary_set = true;
} }
} }
if (_trigger_counter_cam_function) { // init each instance, do it after all instances were created, so that they all know things
_trigger_counter_cam_function--; for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
} else { if (_backends[instance] != nullptr) {
switch (_type) { _backends[instance]->init();
case AP_Camera::CAMERA_TYPE_BMMCC:
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_iso, _servo_off_pwm);
break;
} }
} }
} }
// handle incoming mavlink messages
void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
{ {
switch (msg.msgid) { if (msg.msgid == MAVLINK_MSG_ID_DIGICAM_CONTROL) {
case MAVLINK_MSG_ID_DIGICAM_CONTROL: // decode deprecated MavLink message that controls camera.
control_msg(msg); __mavlink_digicam_control_t packet;
break; mavlink_msg_digicam_control_decode(&msg, &packet);
#if HAL_SOLO_GIMBAL_ENABLED control(packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id);
case MAVLINK_MSG_ID_GOPRO_HEARTBEAT: return;
// heartbeat from the Solo gimbal with a GoPro }
if (get_trigger_type() == CamTrigType::gopro) {
AP_Camera_SoloGimbal::handle_gopro_heartbeat(chan, msg); // call each instance
break; for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
if (_backends[instance] != nullptr) {
_backends[instance]->handle_message(chan, msg);
} }
break;
#endif
} }
} }
/// momentary switch to cycle camera modes // set camera trigger distance in a mission
void AP_Camera::cam_mode_toggle() void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m)
{ {
switch (get_trigger_type()) { if (!is_valid_instance(instance)) {
#if HAL_SOLO_GIMBAL_ENABLED return;
case CamTrigType::gopro:
AP_Camera_SoloGimbal::gopro_capture_mode_toggle();
break;
#endif
default:
// no other cameras use this yet
break;
} }
// call backend
_backends[instance]->set_trigger_distance(distance_m);
} }
/// decode deprecated MavLink message that controls camera. // momentary switch to change camera between picture and video modes
void void AP_Camera::cam_mode_toggle(uint8_t instance)
AP_Camera::control_msg(const mavlink_message_t &msg)
{ {
__mavlink_digicam_control_t packet; if (!is_valid_instance(instance)) {
mavlink_msg_digicam_control_decode(&msg, &packet); return;
}
control(packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id); // call backend
_backends[instance]->cam_mode_toggle();
} }
void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
{ {
// we cannot process the configure command so convert to mavlink message configure(primary, shooting_mode, shutter_speed, aperture, ISO, exposure_type, cmd_id, engine_cutoff_time);
// and send to all components in case they and process it }
mavlink_command_long_t mav_cmd_long = {}; void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
{
// convert mission command to mavlink command_long if (!is_valid_instance(instance)) {
mav_cmd_long.command = MAV_CMD_DO_DIGICAM_CONFIGURE; return;
mav_cmd_long.param1 = shooting_mode;
mav_cmd_long.param2 = shutter_speed;
mav_cmd_long.param3 = aperture;
mav_cmd_long.param4 = ISO;
mav_cmd_long.param5 = exposure_type;
mav_cmd_long.param6 = cmd_id;
mav_cmd_long.param7 = engine_cutoff_time;
// send to all components
GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long));
if (_type == AP_Camera::CAMERA_TYPE_BMMCC) {
// Set a trigger for the additional functions that are flip controlled (so far just ISO and Record Start / Stop use this method, will add others if required)
_trigger_counter_cam_function = constrain_int16(_trigger_duration*5,0,255);
// If the message contains non zero values then use them for the below functions
if (ISO > 0) {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_iso, _servo_on_pwm);
}
if (aperture > 0) {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_aperture, (int)aperture);
}
if (shutter_speed > 0) {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_shutter_speed, (int)shutter_speed);
}
// Use the shooting mode PWM value for the BMMCC as the focus control - no need to modify or create a new MAVlink message type.
if (shooting_mode > 0) {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_focus, (int)shooting_mode);
}
} }
// call backend
_backends[instance]->configure(shooting_mode, shutter_speed, aperture, ISO, exposure_type, cmd_id, engine_cutoff_time);
} }
void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
{ {
// take picture control(primary, session, zoom_pos, zoom_step, focus_lock, shooting_cmd, cmd_id);
if (is_equal(shooting_cmd,1.0f)) { }
trigger_pic();
void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
{
if (!is_valid_instance(instance)) {
return;
} }
mavlink_command_long_t mav_cmd_long = {}; // call backend
_backends[instance]->control(session, zoom_pos, zoom_step, focus_lock, shooting_cmd, cmd_id);
// convert command to mavlink command long
mav_cmd_long.command = MAV_CMD_DO_DIGICAM_CONTROL;
mav_cmd_long.param1 = session;
mav_cmd_long.param2 = zoom_pos;
mav_cmd_long.param3 = zoom_step;
mav_cmd_long.param4 = focus_lock;
mav_cmd_long.param5 = shooting_cmd;
mav_cmd_long.param6 = cmd_id;
// send to all components
GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long));
} }
/* /*
@ -333,306 +204,145 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo
*/ */
void AP_Camera::send_feedback(mavlink_channel_t chan) const void AP_Camera::send_feedback(mavlink_channel_t chan) const
{ {
// call each instance
int32_t altitude = 0; for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
if (feedback.location.initialised() && !feedback.location.get_alt_cm(Location::AltFrame::ABSOLUTE, altitude)) { if (_backends[instance] != nullptr) {
// completely ignore this failure! this is a shouldn't-happen _backends[instance]->send_camera_feedback(chan);
// as current_loc should never be in an altitude we can't }
// convert.
} }
int32_t altitude_rel = 0;
if (feedback.location.initialised() && !feedback.location.get_alt_cm(Location::AltFrame::ABOVE_HOME, altitude_rel)) {
// completely ignore this failure! this is a shouldn't-happen
// as current_loc should never be in an altitude we can't
// convert.
}
mavlink_msg_camera_feedback_send(
chan,
feedback.timestamp_us,
0, 0, _image_index,
feedback.location.lat,
feedback.location.lng,
altitude*1e-2f, altitude_rel*1e-2f,
feedback.roll_sensor*1e-2f,
feedback.pitch_sensor*1e-2f,
feedback.yaw_sensor*1e-2f,
0.0f, CAMERA_FEEDBACK_PHOTO,
feedback.camera_trigger_logged);
} }
/* /*
update; triggers by distance moved and camera trigger update; triggers by distance moved and camera trigger
*/ */
void AP_Camera::update() void AP_Camera::update()
{ {
update_trigger(); // call each instance
for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { if (_backends[instance] != nullptr) {
return; _backends[instance]->update();
} }
if (!is_positive(_trigg_dist)) {
_last_location.lat = 0;
_last_location.lng = 0;
return;
}
const AP_AHRS &ahrs = AP::ahrs();
Location current_loc;
// ignore failure - AHRS will provide its best guess
IGNORE_RETURN(ahrs.get_location(current_loc));
if (_last_location.lat == 0 && _last_location.lng == 0) {
_last_location = current_loc;
return;
}
if (_last_location.lat == current_loc.lat && _last_location.lng == current_loc.lng) {
// we haven't moved - this can happen as update() may
// be called without a new GPS fix
return;
}
if (current_loc.get_distance(_last_location) < _trigg_dist) {
return;
}
if (_max_roll > 0 && fabsf(AP::ahrs().roll_sensor*1e-2f) > _max_roll) {
return;
}
if (_is_in_auto_mode != true && _auto_mode_only != 0) {
return;
}
take_picture();
}
/*
interrupt handler for interrupt based feedback trigger
*/
void AP_Camera::feedback_pin_isr(uint8_t pin, bool high, uint32_t timestamp_us)
{
_feedback_trigger_timestamp_us = timestamp_us;
_camera_trigger_count++;
}
/*
check if feedback pin is high for timer based feedback trigger, when
attach_interrupt fails
*/
void AP_Camera::feedback_pin_timer(void)
{
uint8_t pin_state = hal.gpio->read(_feedback_pin);
uint8_t trigger_polarity = _feedback_polarity==0?0:1;
if (pin_state == trigger_polarity &&
_last_pin_state != trigger_polarity) {
_feedback_trigger_timestamp_us = AP_HAL::micros();
_camera_trigger_count++;
}
_last_pin_state = pin_state;
}
/*
setup a callback for a feedback pin. When on PX4 with the right FMU
mode we can use the microsecond timer.
*/
void AP_Camera::setup_feedback_callback(void)
{
if (_feedback_pin <= 0 || _timer_installed || _isr_installed) {
// invalid or already installed
return;
}
// ensure we are in input mode
hal.gpio->pinMode(_feedback_pin, HAL_GPIO_INPUT);
// enable pullup/pulldown
uint8_t trigger_polarity = _feedback_polarity==0?0:1;
hal.gpio->write(_feedback_pin, !trigger_polarity);
if (hal.gpio->attach_interrupt(_feedback_pin, FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_isr, void, uint8_t, bool, uint32_t),
trigger_polarity?AP_HAL::GPIO::INTERRUPT_RISING:AP_HAL::GPIO::INTERRUPT_FALLING)) {
_isr_installed = true;
} else {
// install a 1kHz timer to check feedback pin
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_timer, void));
_timer_installed = true;
}
}
// log_picture - log picture taken and send feedback to GCS
void AP_Camera::log_picture()
{
if (!using_feedback_pin()) {
// if we're using a feedback pin then when the event occurs we
// stash the feedback data. Since we're not using a feedback
// pin, we just use "now".
prep_mavlink_msg_camera_feedback(AP::gps().time_epoch_usec());
}
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) {
return;
}
if (!logger->should_log(log_camera_bit)) {
return;
}
if (!using_feedback_pin()) {
Write_Camera();
} else {
Write_Trigger();
} }
} }
// take_picture - take a picture // take_picture - take a picture
void AP_Camera::take_picture() void AP_Camera::take_picture(uint8_t instance)
{ {
uint32_t tnow = AP_HAL::millis(); if (!is_valid_instance(instance)) {
if (tnow - _last_photo_time < (unsigned) _min_interval) {
_trigger_pending = true;
return; return;
} }
_trigger_pending = false;
IGNORE_RETURN(AP::ahrs().get_location(_last_location)); // call backend
_backends[instance]->take_picture();
_last_photo_time = tnow;
// take a local picture:
trigger_pic();
// tell all of our components to take a picture:
mavlink_command_long_t cmd_msg {};
cmd_msg.command = MAV_CMD_DO_DIGICAM_CONTROL;
cmd_msg.param5 = 1;
// forward to all components
GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&cmd_msg, sizeof(cmd_msg));
} }
// start/stop recording video. returns true on success // start/stop recording video. returns true on success
// start_recording should be true to start recording, false to stop recording // start_recording should be true to start recording, false to stop recording
bool AP_Camera::record_video(bool start_recording) bool AP_Camera::record_video(uint8_t instance, bool start_recording)
{ {
#if HAL_MOUNT_ENABLED if (!is_valid_instance(instance)) {
// only mount implements recording video return false;
if (get_trigger_type() == CamTrigType::mount) {
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->record_video(0, start_recording);
}
} }
#endif
return false; // call backend
return _backends[instance]->record_video(start_recording);
} }
// zoom in, out or hold. returns true on success // zoom in, out or hold. returns true on success
// zoom out = -1, hold = 0, zoom in = 1 // zoom out = -1, hold = 0, zoom in = 1
bool AP_Camera::set_zoom_step(int8_t zoom_step) bool AP_Camera::set_zoom_step(uint8_t instance, int8_t zoom_step)
{ {
#if HAL_MOUNT_ENABLED if (!is_valid_instance(instance)) {
// only mount implements set_zoom_step return false;
if (get_trigger_type() == CamTrigType::mount) {
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->set_zoom_step(0, zoom_step);
}
} }
#endif
return false; // call each instance
return _backends[instance]->set_zoom_step(zoom_step);
} }
// focus in, out or hold. returns true on success // focus in, out or hold. returns true on success
// focus in = -1, focus hold = 0, focus out = 1 // focus in = -1, focus hold = 0, focus out = 1
bool AP_Camera::set_manual_focus_step(int8_t focus_step) bool AP_Camera::set_manual_focus_step(uint8_t instance, int8_t focus_step)
{ {
#if HAL_MOUNT_ENABLED if (!is_valid_instance(instance)) {
// only mount implements set_manual_focus_step return false;
if (get_trigger_type() == CamTrigType::mount) {
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->set_manual_focus_step(0, focus_step);
}
} }
#endif
return false; // call backend
return _backends[instance]->set_manual_focus_step(focus_step);
} }
// auto focus. returns true on success // auto focus. returns true on success
bool AP_Camera::set_auto_focus() bool AP_Camera::set_auto_focus(uint8_t instance)
{ {
#if HAL_MOUNT_ENABLED if (!is_valid_instance(instance)) {
// only mount implements set_auto_focus return false;
if (get_trigger_type() == CamTrigType::mount) {
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->set_auto_focus(0);
}
} }
#endif
return false; // call backend
return _backends[instance]->set_auto_focus();
} }
void AP_Camera::prep_mavlink_msg_camera_feedback(uint64_t timestamp_us) // check instance number is valid
bool AP_Camera::is_valid_instance(uint8_t instance) const
{ {
const AP_AHRS &ahrs = AP::ahrs(); return ((instance < AP_CAMERA_MAX_INSTANCES) && (_backends[instance] != nullptr));
if (!ahrs.get_location(feedback.location)) {
// completely ignore this failure! AHRS will provide its best guess.
}
feedback.timestamp_us = timestamp_us;
feedback.roll_sensor = ahrs.roll_sensor;
feedback.pitch_sensor = ahrs.pitch_sensor;
feedback.yaw_sensor = ahrs.yaw_sensor;
feedback.camera_trigger_logged = _camera_trigger_logged;
gcs().send_message(MSG_CAMERA_FEEDBACK);
} }
/* // perform any required parameter conversion
update camera trigger - 50Hz void AP_Camera::convert_params()
*/
void AP_Camera::update_trigger()
{ {
if (_trigger_pending) { // exit immediately if CAM1_TYPE has already been configured
take_picture(); if (_params[0].type.configured()) {
return;
} }
trigger_pic_cleanup();
if (_camera_trigger_logged != _camera_trigger_count) {
uint32_t timestamp32 = _feedback_trigger_timestamp_us;
_camera_trigger_logged = _camera_trigger_count;
// we should consider doing this inside the ISR and pin_timer // below conversions added Feb 2023 ahead of 4.4 release
prep_mavlink_msg_camera_feedback(_feedback_trigger_timestamp_us);
AP_Logger *logger = AP_Logger::get_singleton(); // convert CAM_TRIGG_TYPE to CAM1_TYPE
if (logger != nullptr) { int8_t cam_trigg_type = 0;
if (logger->should_log(log_camera_bit)) { int8_t cam1_type = 0;
uint32_t tdiff = AP_HAL::micros() - timestamp32; IGNORE_RETURN(AP_Param::get_param_by_index(this, 0, AP_PARAM_INT8, &cam_trigg_type));
uint64_t timestamp = AP_HAL::micros64(); if ((cam_trigg_type == 0) && SRV_Channels::function_assigned(SRV_Channel::k_cam_trigger)) {
Write_Camera(timestamp - tdiff); // CAM_TRIGG_TYPE was 0 (Servo) and camera trigger servo function was assigned so set CAM1_TYPE = 1 (Servo)
} cam1_type = 1;
}
} }
} if ((cam_trigg_type >= 1) && (cam_trigg_type <= 3)) {
// CAM_TRIGG_TYPE was set to Relay, GoPro or Mount
cam1_type = cam_trigg_type + 1;
}
_params[0].type.set_and_save(cam1_type);
AP_Camera::CamTrigType AP_Camera::get_trigger_type(void) // convert CAM_DURATION (in deci-seconds) to CAM1_DURATION (in seconds)
{ int8_t cam_duration = 0;
uint8_t type = _trigger_type.get(); if (AP_Param::get_param_by_index(this, 1, AP_PARAM_INT8, &cam_duration) && (cam_duration > 0)) {
_params[0].trigger_duration.set_and_save(cam_duration * 0.1);
}
switch ((CamTrigType)type) { // convert CAM_MIN_INTERVAL (in milliseconds) to CAM1__INTRVAL_MIN (in seconds)
case CamTrigType::servo: int16_t cam_min_interval = 0;
case CamTrigType::relay: if (AP_Param::get_param_by_index(this, 6, AP_PARAM_INT16, &cam_min_interval) && (cam_min_interval > 0)) {
case CamTrigType::gopro: _params[0].interval_min.set_and_save(cam_min_interval * 0.001f);
case CamTrigType::mount: }
return (CamTrigType)type;
default: // find Camera's top level key
return CamTrigType::servo; uint16_t k_param_camera_key;
if (!AP_Param::find_top_level_key_by_pointer(this, k_param_camera_key)) {
return;
}
// table parameters to convert without scaling
static const AP_Param::ConversionInfo camera_param_conversion_info[] {
{ k_param_camera_key, 2, AP_PARAM_INT16, "CAM1_SERVO_ON" },
{ k_param_camera_key, 3, AP_PARAM_INT16, "CAM1_SERVO_OFF" },
{ k_param_camera_key, 4, AP_PARAM_FLOAT, "CAM1_TRIGG_DIST" },
{ k_param_camera_key, 5, AP_PARAM_INT8, "CAM1_RELAY_ON" },
{ k_param_camera_key, 8, AP_PARAM_INT8, "CAM1_FEEDBAK_PIN" },
{ k_param_camera_key, 9, AP_PARAM_INT8, "CAM1_FEEDBAK_POL" },
};
uint8_t table_size = ARRAY_SIZE(camera_param_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&camera_param_conversion_info[i], 1.0f);
} }
} }

212
libraries/AP_Camera/AP_Camera.h
View File

@ -10,173 +10,141 @@
#include <AP_Logger/LogStructure.h> #include <AP_Logger/LogStructure.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include "AP_Camera_Params.h"
#define AP_CAMERA_TRIGGER_DEFAULT_DURATION 10 // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second) #define AP_CAMERA_MAX_INSTANCES 2 // maximum number of camera backends
#define AP_CAMERA_SERVO_ON_PWM 1300 // default PWM value to move servo to when shutter is activated // declare backend classes
#define AP_CAMERA_SERVO_OFF_PWM 1100 // default PWM value to move servo to when shutter is deactivated class AP_Camera_Backend;
class AP_Camera_Servo;
#define AP_CAMERA_FEEDBACK_DEFAULT_FEEDBACK_PIN -1 // default is to not use camera feedback pin class AP_Camera_Relay;
class AP_Camera_SoloGimbal;
class AP_Camera_Mount;
/// @class Camera /// @class Camera
/// @brief Object managing a Photo or video camera /// @brief Object managing a Photo or video camera
class AP_Camera { class AP_Camera {
// declare backends as friends
friend class AP_Camera_Backend;
friend class AP_Camera_Servo;
friend class AP_Camera_Relay;
friend class AP_Camera_SoloGimbal;
friend class AP_Camera_Mount;
friend class AP_Camera_MAVLink;
public: public:
AP_Camera(uint32_t _log_camera_bit)
: log_camera_bit(_log_camera_bit) // constructor
{ AP_Camera(uint32_t _log_camera_bit);
AP_Param::setup_object_defaults(this, var_info);
_singleton = this;
}
/* Do not allow copies */ /* Do not allow copies */
CLASS_NO_COPY(AP_Camera); CLASS_NO_COPY(AP_Camera);
// get singleton instance // get singleton instance
static AP_Camera *get_singleton() static AP_Camera *get_singleton() { return _singleton; }
{
return _singleton; // enums
} enum class CameraType {
NONE = 0, // None
SERVO = 1, // Servo/PWM controlled camera
RELAY = 2, // Relay controlled camera
SOLOGIMBAL = 3, // GoPro in Solo gimbal
MOUNT = 4, // Mount library implements camera
MAVLINK = 5, // MAVLink enabled camera
};
// returns camera type of the given instance
CameraType get_type(uint8_t instance) const;
// detect and initialise backends
void init();
// update - to be called periodically at 50Hz
void update();
// MAVLink methods // MAVLink methods
void handle_message(mavlink_channel_t chan, void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
const mavlink_message_t &msg); void send_feedback(mavlink_channel_t chan) const;
void send_feedback(mavlink_channel_t chan) const;
// configure camera
void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);
void configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);
// Command processing
void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);
// handle camera control // handle camera control
void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id); void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id);
void control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id);
// set camera trigger distance in a mission // set camera trigger distance in a mission
void set_trigger_distance(float distance_m) void set_trigger_distance(float distance_m) { set_trigger_distance(primary, distance_m); }
{ void set_trigger_distance(uint8_t instance, float distance_m);
_trigg_dist.set(distance_m);
}
// momentary switch to change camera modes // momentary switch to change camera between picture and video modes
void cam_mode_toggle(); void cam_mode_toggle() { cam_mode_toggle(primary); }
void cam_mode_toggle(uint8_t instance);
void take_picture(); // take a picture
void take_picture() { take_picture(primary); }
void take_picture(uint8_t instance);
// start/stop recording video // start/stop recording video
// start_recording should be true to start recording, false to stop recording // start_recording should be true to start recording, false to stop recording
bool record_video(bool start_recording); bool record_video(bool start_recording) { return record_video(primary, start_recording); }
bool record_video(uint8_t instance, bool start_recording);
// zoom in, out or hold // zoom in, out or hold
// zoom out = -1, hold = 0, zoom in = 1 // zoom out = -1, hold = 0, zoom in = 1
bool set_zoom_step(int8_t zoom_step); bool set_zoom_step(int8_t zoom_step) { return set_zoom_step(primary, zoom_step); }
bool set_zoom_step(uint8_t instance, int8_t zoom_step);
// focus in, out or hold // focus in, out or hold
// focus in = -1, focus hold = 0, focus out = 1 // focus in = -1, focus hold = 0, focus out = 1
bool set_manual_focus_step(int8_t focus_step); bool set_manual_focus_step(int8_t focus_step) { return set_manual_focus_step(primary, focus_step); }
bool set_manual_focus_step(uint8_t instance, int8_t focus_step);
// auto focus // auto focus
bool set_auto_focus(); bool set_auto_focus() { return set_auto_focus(primary); }
bool set_auto_focus(uint8_t instance);
// Update - to be called periodically @at least 50Hz
void update();
static const struct AP_Param::GroupInfo var_info[];
// set if vehicle is in AUTO mode // set if vehicle is in AUTO mode
void set_is_auto_mode(bool enable) void set_is_auto_mode(bool enable) { _is_in_auto_mode = enable; }
{
_is_in_auto_mode = enable;
}
enum camera_types { // parameter var table
CAMERA_TYPE_STD, static const struct AP_Param::GroupInfo var_info[];
CAMERA_TYPE_BMMCC
};
enum class CamTrigType { protected:
servo = 0,
relay = 1,
gopro = 2,
mount = 3,
};
AP_Camera::CamTrigType get_trigger_type(void); // return true if vehicle mode allows trigg dist
bool vehicle_mode_ok_for_trigg_dist() const { return (_auto_mode_only == 0) || _is_in_auto_mode; }
// return maximum acceptable vehicle roll angle (in degrees)
int16_t get_roll_max() const { return _max_roll; }
// return log bit
uint32_t get_log_camera_bit() const { return log_camera_bit; }
// parameters for backends
AP_Camera_Params _params[AP_CAMERA_MAX_INSTANCES];
private: private:
static AP_Camera *_singleton; static AP_Camera *_singleton;
void control_msg(const mavlink_message_t &msg); // parameters
AP_Int8 _auto_mode_only; // if 1: trigger by distance only if in AUTO mode.
AP_Int16 _max_roll; // Maximum acceptable roll angle when trigging camera
AP_Int8 _trigger_type; // 0:Servo,1:Relay, 2:GoPro in Solo Gimbal // check instance number is valid
AP_Int8 _trigger_duration; // duration in 10ths of a second that the camera shutter is held open bool is_valid_instance(uint8_t instance) const;
AP_Int8 _relay_on; // relay value to trigger camera
AP_Int16 _servo_on_pwm; // PWM value to move servo to when shutter is activated
AP_Int16 _servo_off_pwm; // PWM value to move servo to when shutter is deactivated
uint8_t _trigger_counter; // count of number of cycles shutter has been held open
uint8_t _trigger_counter_cam_function; // count of number of cycles alternative camera function has been held open
AP_Int8 _auto_mode_only; // if 1: trigger by distance only if in AUTO mode.
AP_Int8 _type; // Set the type of camera in use, will open additional parameters if set
bool _is_in_auto_mode; // true if in AUTO mode
void servo_pic(); // Servo operated camera // perform any required parameter conversion
void relay_pic(); // basic relay activation void convert_params();
void feedback_pin_timer();
void feedback_pin_isr(uint8_t, bool, uint32_t);
void setup_feedback_callback(void);
AP_Float _trigg_dist; // distance between trigger points (meters)
AP_Int16 _min_interval; // Minimum time between shots required by camera
AP_Int16 _max_roll; // Maximum acceptable roll angle when trigging camera
uint32_t _last_photo_time; // last time a photo was taken
bool _trigger_pending; // true when we have delayed take_picture
Location _last_location;
uint16_t _image_index; // number of pictures taken since boot
// pin number for accurate camera feedback messages
AP_Int8 _feedback_pin;
AP_Int8 _feedback_polarity;
uint32_t _camera_trigger_count;
uint32_t _camera_trigger_logged;
uint32_t _feedback_trigger_timestamp_us;
struct {
uint64_t timestamp_us;
Location location; // place where most recent image was taken
int32_t roll_sensor;
int32_t pitch_sensor;
int32_t yaw_sensor;
uint32_t camera_trigger_logged; // ID sequence number
} feedback;
void prep_mavlink_msg_camera_feedback(uint64_t timestamp_us);
bool _timer_installed;
bool _isr_installed;
uint8_t _last_pin_state;
void log_picture();
// Logging Function
void Write_Camera(uint64_t timestamp_us=0);
void Write_Trigger(void);
void Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us=0);
uint32_t log_camera_bit;
// update camera trigger - 50Hz
void update_trigger();
// entry point to trip local shutter (e.g. by relay or servo)
void trigger_pic();
// de-activate the trigger after some delay, but without using a delay() function
// should be called at 50hz from main program
void trigger_pic_cleanup();
// return true if we are using a feedback pin
bool using_feedback_pin(void) const
{
return _feedback_pin > 0;
}
uint8_t primary; // index of primary backend (normally the first one)
bool _is_in_auto_mode; // true if in AUTO mode
uint32_t log_camera_bit; // logging bit (from LOG_BITMASK) to enable camera logging
uint8_t _num_instances; // number of camera backends instantiated
AP_Camera_Backend *_backends[AP_CAMERA_MAX_INSTANCES]; // pointers to instantiated backends
}; };
namespace AP { namespace AP {

243
libraries/AP_Camera/AP_Camera_Backend.cpp Normal file
View File

@ -0,0 +1,243 @@
#include "AP_Camera_Backend.h"
#if AP_CAMERA_ENABLED
#include <GCS_MAVLink/GCS.h>
#include <AP_GPS/AP_GPS.h>
extern const AP_HAL::HAL& hal;
// Constructor
AP_Camera_Backend::AP_Camera_Backend(AP_Camera &frontend, AP_Camera_Params &params, uint8_t instance) :
_frontend(frontend),
_params(params),
_instance(instance)
{}
// update - should be called at 50hz
void AP_Camera_Backend::update()
{
// try to take picture if pending
if (trigger_pending) {
take_picture();
}
// check feedback pin
check_feedback();
// implement trigger distance
if (!is_positive(_params.trigg_dist)) {
last_location.lat = 0;
last_location.lng = 0;
return;
}
// check GPS status
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
return;
}
// check vehicle flight mode supports trigg dist
if (!_frontend.vehicle_mode_ok_for_trigg_dist()) {
return;
}
// check vehicle roll angle is less than configured maximum
const AP_AHRS &ahrs = AP::ahrs();
if ((_frontend.get_roll_max() > 0) && (fabsf(AP::ahrs().roll_sensor * 1e-2f) > _frontend.get_roll_max())) {
return;
}
// get current location. ignore failure because AHRS will provide its best guess
Location current_loc;
IGNORE_RETURN(ahrs.get_location(current_loc));
// initialise last location to current location
if (last_location.lat == 0 && last_location.lng == 0) {
last_location = current_loc;
return;
}
if (last_location.lat == current_loc.lat && last_location.lng == current_loc.lng) {
// we haven't moved - this can happen as update() may
// be called without a new GPS fix
return;
}
// check vehicle has moved at least trigg_dist meters
if (current_loc.get_distance(last_location) < _params.trigg_dist) {
return;
}
take_picture();
}
// take a picture. returns true on success
bool AP_Camera_Backend::take_picture()
{
// setup feedback pin interrupt or timer
setup_feedback_callback();
// check minimum time interval since last picture taken
uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_photo_time_ms < (uint32_t)(_params.interval_min * 1000)) {
trigger_pending = true;
return false;
}
trigger_pending = false;
// trigger actually taking picture and update image count
if (trigger_pic()) {
image_index++;
last_photo_time_ms = now_ms;
IGNORE_RETURN(AP::ahrs().get_location(last_location));
log_picture();
return true;
}
return false;
}
// handle camera control
void AP_Camera_Backend::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
{
// take picture
if (is_equal(shooting_cmd, 1.0f)) {
take_picture();
}
}
// send camera feedback message to GCS
void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan) const
{
int32_t altitude = 0;
if (camera_feedback.location.initialised() && !camera_feedback.location.get_alt_cm(Location::AltFrame::ABSOLUTE, altitude)) {
// completely ignore this failure! this is a shouldn't-happen
// as current_loc should never be in an altitude we can't
// convert.
}
int32_t altitude_rel = 0;
if (camera_feedback.location.initialised() && !camera_feedback.location.get_alt_cm(Location::AltFrame::ABOVE_HOME, altitude_rel)) {
// completely ignore this failure! this is a shouldn't-happen
// as current_loc should never be in an altitude we can't
// convert.
}
// send camera feedback message
mavlink_msg_camera_feedback_send(
chan,
camera_feedback.timestamp_us, // image timestamp
0, // target system id
_instance, // camera id
image_index, // image index
camera_feedback.location.lat, // latitude
camera_feedback.location.lng, // longitude
altitude*1e-2f, // alt MSL
altitude_rel*1e-2f, // alt relative to home
camera_feedback.roll_sensor*1e-2f, // roll angle (deg)
camera_feedback.pitch_sensor*1e-2f, // pitch angle (deg)
camera_feedback.yaw_sensor*1e-2f, // yaw angle (deg)
0.0f, // focal length
CAMERA_FEEDBACK_PHOTO, // flags
camera_feedback.feedback_trigger_logged_count); // completed image captures
}
// setup a callback for a feedback pin. When on PX4 with the right FMU
// mode we can use the microsecond timer.
void AP_Camera_Backend::setup_feedback_callback()
{
if (_params.feedback_pin <= 0 || timer_installed || isr_installed) {
// invalid or already installed
return;
}
// ensure we are in input mode
hal.gpio->pinMode(_params.feedback_pin, HAL_GPIO_INPUT);
// enable pullup/pulldown
uint8_t trigger_polarity = _params.feedback_polarity == 0 ? 0 : 1;
hal.gpio->write(_params.feedback_pin, !trigger_polarity);
if (hal.gpio->attach_interrupt(_params.feedback_pin, FUNCTOR_BIND_MEMBER(&AP_Camera_Backend::feedback_pin_isr, void, uint8_t, bool, uint32_t),
trigger_polarity?AP_HAL::GPIO::INTERRUPT_RISING:AP_HAL::GPIO::INTERRUPT_FALLING)) {
isr_installed = true;
} else {
// install a 1kHz timer to check feedback pin
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Camera_Backend::feedback_pin_timer, void));
timer_installed = true;
}
}
// interrupt handler for interrupt based feedback trigger
void AP_Camera_Backend::feedback_pin_isr(uint8_t pin, bool high, uint32_t timestamp_us)
{
feedback_trigger_timestamp_us = timestamp_us;
feedback_trigger_count++;
}
// check if feedback pin is high for timer based feedback trigger, when
// attach_interrupt fails
void AP_Camera_Backend::feedback_pin_timer()
{
uint8_t pin_state = hal.gpio->read(_params.feedback_pin);
uint8_t trigger_polarity = _params.feedback_polarity == 0 ? 0 : 1;
if (pin_state == trigger_polarity &&
last_pin_state != trigger_polarity) {
feedback_trigger_timestamp_us = AP_HAL::micros();
feedback_trigger_count++;
}
last_pin_state = pin_state;
}
// check for feedback pin update and log if necessary
void AP_Camera_Backend::check_feedback()
{
if (feedback_trigger_logged_count != feedback_trigger_count) {
const uint32_t timestamp32 = feedback_trigger_timestamp_us;
feedback_trigger_logged_count = feedback_trigger_count;
// we should consider doing this inside the ISR and pin_timer
prep_mavlink_msg_camera_feedback(feedback_trigger_timestamp_us);
// log camera message
uint32_t tdiff = AP_HAL::micros() - timestamp32;
uint64_t timestamp = AP_HAL::micros64();
Write_Camera(timestamp - tdiff);
}
}
void AP_Camera_Backend::prep_mavlink_msg_camera_feedback(uint64_t timestamp_us)
{
const AP_AHRS &ahrs = AP::ahrs();
if (!ahrs.get_location(camera_feedback.location)) {
// completely ignore this failure! AHRS will provide its best guess.
}
camera_feedback.timestamp_us = timestamp_us;
camera_feedback.roll_sensor = ahrs.roll_sensor;
camera_feedback.pitch_sensor = ahrs.pitch_sensor;
camera_feedback.yaw_sensor = ahrs.yaw_sensor;
camera_feedback.feedback_trigger_logged_count = feedback_trigger_logged_count;
gcs().send_message(MSG_CAMERA_FEEDBACK);
}
// log picture
void AP_Camera_Backend::log_picture()
{
const bool using_feedback_pin = _params.feedback_pin > 0;
if (!using_feedback_pin) {
// if we're using a feedback pin then when the event occurs we
// stash the feedback data. Since we're not using a feedback
// pin, we just use "now".
prep_mavlink_msg_camera_feedback(AP::gps().time_epoch_usec());
}
if (!using_feedback_pin) {
Write_Camera();
} else {
Write_Trigger();
}
}
#endif // AP_CAMERA_ENABLED

128
libraries/AP_Camera/AP_Camera_Backend.h Normal file
View File

@ -0,0 +1,128 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Camera driver backend class. Each supported camera type
needs to have an object derived from this class.
*/
#pragma once
#include "AP_Camera_config.h"
#if AP_CAMERA_ENABLED
#include "AP_Camera.h"
class AP_Camera_Backend
{
public:
// Constructor
AP_Camera_Backend(AP_Camera &frontend, AP_Camera_Params &params, uint8_t instance);
/* Do not allow copies */
CLASS_NO_COPY(AP_Camera_Backend);
// init - performs any required initialisation
virtual void init() {};
// update - should be called at 50hz
virtual void update();
// return true if healthy
virtual bool healthy() const { return true; }
// momentary switch to change camera between picture and video modes
virtual void cam_mode_toggle() {}
// take a picture. returns true on success
bool take_picture();
// entry point to actually take a picture. returns true on success
virtual bool trigger_pic() = 0;
// start or stop video recording. returns true on success
// set start_recording = true to start record, false to stop recording
virtual bool record_video(bool start_recording) { return false; }
// set camera zoom step. returns true on success
// zoom out = -1, hold = 0, zoom in = 1
virtual bool set_zoom_step(int8_t zoom_step) { return false; }
// set focus in, out or hold. returns true on success
// focus in = -1, focus hold = 0, focus out = 1
virtual bool set_manual_focus_step(int8_t focus_step) { return false; }
// auto focus. returns true on success
virtual bool set_auto_focus() { return false; }
// handle incoming mavlink message
virtual void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) {}
// configure camera
virtual void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) {}
// handle camera control
virtual void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id);
// set camera trigger distance in meters
void set_trigger_distance(float distance_m) { _params.trigg_dist.set(distance_m); }
// send camera feedback message to GCS
void send_camera_feedback(mavlink_channel_t chan) const;
protected:
// references
AP_Camera &_frontend; // reference to the front end which holds parameters
AP_Camera_Params &_params; // parameters for this backend
// feedback pin related methods
void setup_feedback_callback();
void feedback_pin_isr(uint8_t pin, bool high, uint32_t timestamp_us);
void feedback_pin_timer();
void check_feedback();
// store vehicle location and attitude for use in camera_feedback message to GCS
void prep_mavlink_msg_camera_feedback(uint64_t timestamp_us);
struct {
uint64_t timestamp_us; // system time of most recent image
Location location; // location where most recent image was taken
int32_t roll_sensor; // vehicle roll in centi-degrees
int32_t pitch_sensor; // vehicle pitch in centi-degrees
int32_t yaw_sensor; // vehicle yaw in centi-degrees
uint32_t feedback_trigger_logged_count; // ID sequence number
} camera_feedback;
// Logging Function
void log_picture();
void Write_Camera(uint64_t timestamp_us=0);
void Write_Trigger();
void Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us=0);
// internal members
uint8_t _instance; // this instance's number
bool timer_installed; // true if feedback pin change detected using timer
bool isr_installed; // true if feedback pin change is detected with an interrupt
uint8_t last_pin_state; // last pin state. used by timer based detection
uint32_t feedback_trigger_count; // number of times the interrupt detected the feedback pin changed
uint32_t feedback_trigger_timestamp_us; // system time (in microseconds) that timer detected the feedback pin changed
uint32_t feedback_trigger_logged_count; // number of times the feedback has been logged
bool trigger_pending; // true if a call to take_pic() was delayed due to the minimum time interval time
uint32_t last_photo_time_ms; // system time that photo was last taken
Location last_location; // Location that last picture was taken at (used for trigg_dist calculation)
uint16_t image_index; // number of pictures taken since boot
};
#endif // AP_CAMERA_ENABLED

22
libraries/AP_Camera/AP_Camera_Logging.cpp
View File

@ -1,4 +1,4 @@
#include "AP_Camera.h" #include "AP_Camera_Backend.h"
#if AP_CAMERA_ENABLED #if AP_CAMERA_ENABLED
@ -6,8 +6,19 @@
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
// Write a Camera packet // Write a Camera packet
void AP_Camera::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us) void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us)
{ {
// exit immediately if no logger
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) {
return;
}
// exit immediately if should not log camera messages
if (!logger->should_log(_frontend.get_log_camera_bit())) {
return;
}
const AP_AHRS &ahrs = AP::ahrs(); const AP_AHRS &ahrs = AP::ahrs();
Location current_loc; Location current_loc;
@ -32,7 +43,8 @@ void AP_Camera::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us)
const struct log_Camera pkt{ const struct log_Camera pkt{
LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)), LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),
time_us : timestamp_us?timestamp_us:AP_HAL::micros64(), time_us : timestamp_us ? timestamp_us : AP_HAL::micros64(),
instance : _instance,
gps_time : gps.time_week_ms(), gps_time : gps.time_week_ms(),
gps_week : gps.time_week(), gps_week : gps.time_week(),
latitude : current_loc.lat, latitude : current_loc.lat,
@ -48,13 +60,13 @@ void AP_Camera::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us)
} }
// Write a Camera packet // Write a Camera packet
void AP_Camera::Write_Camera(uint64_t timestamp_us) void AP_Camera_Backend::Write_Camera(uint64_t timestamp_us)
{ {
Write_CameraInfo(LOG_CAMERA_MSG, timestamp_us); Write_CameraInfo(LOG_CAMERA_MSG, timestamp_us);
} }
// Write a Trigger packet // Write a Trigger packet
void AP_Camera::Write_Trigger(void) void AP_Camera_Backend::Write_Trigger()
{ {
Write_CameraInfo(LOG_TRIGGER_MSG, 0); Write_CameraInfo(LOG_TRIGGER_MSG, 0);
} }

64
libraries/AP_Camera/AP_Camera_MAVLink.cpp Normal file
View File

@ -0,0 +1,64 @@
#include "AP_Camera_MAVLink.h"
#if AP_CAMERA_ENABLED
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
// entry point to actually take a picture. returns true on success
bool AP_Camera_MAVLink::trigger_pic()
{
// tell all of our components to take a picture:
mavlink_command_long_t cmd_msg {};
cmd_msg.command = MAV_CMD_DO_DIGICAM_CONTROL;
cmd_msg.param5 = 1;
// forward to all components
GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&cmd_msg, sizeof(cmd_msg));
return true;
}
// configure camera
void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
{
// convert to mavlink message and send to all components
mavlink_command_long_t mav_cmd_long = {};
// convert mission command to mavlink command_long
mav_cmd_long.command = MAV_CMD_DO_DIGICAM_CONFIGURE;
mav_cmd_long.param1 = shooting_mode;
mav_cmd_long.param2 = shutter_speed;
mav_cmd_long.param3 = aperture;
mav_cmd_long.param4 = ISO;
mav_cmd_long.param5 = exposure_type;
mav_cmd_long.param6 = cmd_id;
mav_cmd_long.param7 = engine_cutoff_time;
// send to all components
GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long));
}
// handle camera control message
void AP_Camera_MAVLink::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
{
// take picture and ignore other arguments
if (is_equal(shooting_cmd, 1.0f)) {
take_picture();
return;
}
// convert command to mavlink command long
mavlink_command_long_t mav_cmd_long = {};
mav_cmd_long.command = MAV_CMD_DO_DIGICAM_CONTROL;
mav_cmd_long.param1 = session;
mav_cmd_long.param2 = zoom_pos;
mav_cmd_long.param3 = zoom_step;
mav_cmd_long.param4 = focus_lock;
mav_cmd_long.param5 = shooting_cmd;
mav_cmd_long.param6 = cmd_id;
// send to all components
GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long));
}
#endif // AP_CAMERA_ENABLED

45
libraries/AP_Camera/AP_Camera_MAVLink.h Normal file
View File

@ -0,0 +1,45 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Camera driver for cameras included in Mount
*/
#pragma once
#include "AP_Camera_Backend.h"
#if AP_CAMERA_ENABLED
class AP_Camera_MAVLink : public AP_Camera_Backend
{
public:
// Constructor
using AP_Camera_Backend::AP_Camera_Backend;
/* Do not allow copies */
CLASS_NO_COPY(AP_Camera_MAVLink);
// entry point to actually take a picture
bool trigger_pic() override;
// configure camera
void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) override;
// handle camera control message
void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) override;
};
#endif // AP_CAMERA_ENABLED

72
libraries/AP_Camera/AP_Camera_Mount.cpp Normal file
View File

@ -0,0 +1,72 @@
#include "AP_Camera_Mount.h"
#if AP_CAMERA_ENABLED
#include <AP_Mount/AP_Mount.h>
extern const AP_HAL::HAL& hal;
// entry point to actually take a picture. returns true on success
bool AP_Camera_Mount::trigger_pic()
{
#if HAL_MOUNT_ENABLED
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
mount->take_picture(0);
return true;
}
#endif
return false;
}
// start/stop recording video. returns true on success
// start_recording should be true to start recording, false to stop recording
bool AP_Camera_Mount::record_video(bool start_recording)
{
#if HAL_MOUNT_ENABLED
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->record_video(0, start_recording);
}
#endif
return false;
}
// zoom in, out or hold. returns true on success
// zoom out = -1, hold = 0, zoom in = 1
bool AP_Camera_Mount::set_zoom_step(int8_t zoom_step)
{
#if HAL_MOUNT_ENABLED
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->set_zoom_step(0, zoom_step);
}
#endif
return false;
}
// focus in, out or hold. returns true on success
// focus in = -1, focus hold = 0, focus out = 1
bool AP_Camera_Mount::set_manual_focus_step(int8_t focus_step)
{
#if HAL_MOUNT_ENABLED
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->set_manual_focus_step(0, focus_step);
}
#endif
return false;
}
// auto focus. returns true on success
bool AP_Camera_Mount::set_auto_focus()
{
#if HAL_MOUNT_ENABLED
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->set_auto_focus(0);
}
#endif
return false;
}
#endif // AP_CAMERA_ENABLED

54
libraries/AP_Camera/AP_Camera_Mount.h Normal file
View File

@ -0,0 +1,54 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Camera driver for cameras included in Mount
*/
#pragma once
#include "AP_Camera_Backend.h"
#if AP_CAMERA_ENABLED
class AP_Camera_Mount : public AP_Camera_Backend
{
public:
// Constructor
using AP_Camera_Backend::AP_Camera_Backend;
/* Do not allow copies */
CLASS_NO_COPY(AP_Camera_Mount);
// entry point to actually take a picture. returns true on success
bool trigger_pic() override;
// start or stop video recording. returns true on success
// set start_recording = true to start record, false to stop recording
bool record_video(bool start_recording) override;
// set camera zoom step. returns true on success
// zoom out = -1, hold = 0, zoom in = 1
bool set_zoom_step(int8_t zoom_step) override;
// set focus in, out or hold. returns true on success
// focus in = -1, focus hold = 0, focus out = 1
bool set_manual_focus_step(int8_t focus_step) override;
// auto focus. returns true on success
bool set_auto_focus() override;
};
#endif // AP_CAMERA_ENABLED

83
libraries/AP_Camera/AP_Camera_Params.cpp Normal file
View File

@ -0,0 +1,83 @@
#include "AP_Camera_Params.h"
// table of user settable parameters
const AP_Param::GroupInfo AP_Camera_Params::var_info[] = {
// 0 should not be used
// @Param: _TYPE
// @DisplayName: Camera shutter (trigger) type
// @Description: how to trigger the camera to take a picture
// @Values: 1:Servo,2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink
// @User: Standard
AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE),
// @Param: _DURATION
// @DisplayName: Camera shutter duration held open
// @Description: Duration in seconds that the camera shutter is held open
// @Units: s
// @Range: 0 5
// @User: Standard
AP_GROUPINFO("_DURATION", 2, AP_Camera_Params, trigger_duration, 0.1),
// @Param: _SERVO_ON
// @DisplayName: Camera servo ON PWM value
// @Description: PWM value in microseconds to move servo to when shutter is activated
// @Units: PWM
// @Range: 1000 2000
// @User: Standard
AP_GROUPINFO("_SERVO_ON", 3, AP_Camera_Params, servo_on_pwm, 1300),
// @Param: _SERVO_OFF
// @DisplayName: Camera servo OFF PWM value
// @Description: PWM value in microseconds to move servo to when shutter is deactivated
// @Units: PWM
// @Range: 1000 2000
// @User: Standard
AP_GROUPINFO("_SERVO_OFF", 4, AP_Camera_Params, servo_off_pwm, 1100),
// @Param: _TRIGG_DIST
// @DisplayName: Camera trigger distance
// @Description: Distance in meters between camera triggers. If this value is non-zero then the camera will trigger whenever the position changes by this number of meters regardless of what mode the APM is in. Note that this parameter can also be set in an auto mission using the DO_SET_CAM_TRIGG_DIST command, allowing you to enable/disable the triggering of the camera during the flight.
// @User: Standard
// @Units: m
// @Range: 0 1000
AP_GROUPINFO("_TRIGG_DIST", 5, AP_Camera_Params, trigg_dist, 0),
// @Param: _RELAY_ON
// @DisplayName: Camera relay ON value
// @Description: This sets whether the relay goes high or low when it triggers. Note that you should also set RELAY_DEFAULT appropriately for your camera
// @Values: 0:Low,1:High
// @User: Standard
AP_GROUPINFO("_RELAY_ON", 6, AP_Camera_Params, relay_on, 1),
// @Param: _INTRVAL_MIN
// @DisplayName: Camera minimum time interval between photos
// @Description: Postpone shooting if previous picture was taken less than this many seconds ago
// @Units: s
// @Range: 0 10
// @User: Standard
AP_GROUPINFO("_INTRVAL_MIN", 7, AP_Camera_Params, interval_min, 0),
// @Param: _FEEDBAK_PIN
// @DisplayName: Camera feedback pin
// @Description: pin number to use for save accurate camera feedback messages. If set to -1 then don't use a pin flag for this, otherwise this is a pin number which if held high after a picture trigger order, will save camera messages when camera really takes a picture. A universal camera hot shoe is needed. The pin should be held high for at least 2 milliseconds for reliable trigger detection. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. See also the CAMx_FEEDBCK_POL option.
// @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("_FEEDBAK_PIN", 8, AP_Camera_Params, feedback_pin, -1),
// @Param: _FEEDBAK_POL
// @DisplayName: Camera feedback pin polarity
// @Description: Polarity for feedback pin. If this is 1 then the feedback pin should go high on trigger. If set to 0 then it should go low
// @Values: 0:TriggerLow,1:TriggerHigh
// @User: Standard
AP_GROUPINFO("_FEEDBAK_POL", 9, AP_Camera_Params, feedback_polarity, 1),
AP_GROUPEND
};
AP_Camera_Params::AP_Camera_Params(void) {
AP_Param::setup_object_defaults(this, var_info);
}

28
libraries/AP_Camera/AP_Camera_Params.h Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
class AP_Camera_Params {
public:
static const struct AP_Param::GroupInfo var_info[];
AP_Camera_Params(void);
/* Do not allow copies */
CLASS_NO_COPY(AP_Camera_Params);
AP_Int8 type; // camera type (see CameraType enum)
AP_Float trigger_duration; // duration in seconds that the camera shutter is held open
AP_Int16 servo_on_pwm; // PWM value to move servo to when shutter is activated
AP_Int16 servo_off_pwm; // PWM value to move servo to when shutter is deactivated
AP_Float trigg_dist; // distance between trigger points (meters)
AP_Int8 relay_on; // relay value to trigger camera
AP_Float interval_min; // minimum time (in seconds) between shots required by camera
// pin number for accurate camera feedback messages
AP_Int8 feedback_pin;
AP_Int8 feedback_polarity;
};

57
libraries/AP_Camera/AP_Camera_Relay.cpp Normal file
View File

@ -0,0 +1,57 @@
#include "AP_Camera_Relay.h"
#if AP_CAMERA_ENABLED
#include <AP_Relay/AP_Relay.h>
extern const AP_HAL::HAL& hal;
// update - should be called at 50hz
void AP_Camera_Relay::update()
{
if (trigger_counter > 0) {
trigger_counter--;
} else {
AP_Relay *ap_relay = AP::relay();
if (ap_relay == nullptr) {
return;
}
if (_params.relay_on) {
ap_relay->off(0);
} else {
ap_relay->on(0);
}
}
// call parent update
AP_Camera_Backend::update();
}
// entry point to actually take a picture. returns true on success
bool AP_Camera_Relay::trigger_pic()
{
// fail if have not completed previous picture
if (trigger_counter > 0) {
return false;
}
// exit immediately if no relay is setup
AP_Relay *ap_relay = AP::relay();
if (ap_relay == nullptr) {
return false;
}
if (_params.relay_on) {
ap_relay->on(0);
} else {
ap_relay->off(0);
}
// set counter to move servo to off position after this many iterations of update (assumes 50hz update rate)
trigger_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX);
return true;
}
#endif // AP_CAMERA_ENABLED

46
libraries/AP_Camera/AP_Camera_Relay.h Normal file
View File

@ -0,0 +1,46 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Camera servo driver backend class
*/
#pragma once
#include "AP_Camera_Backend.h"
#if AP_CAMERA_ENABLED
class AP_Camera_Relay : public AP_Camera_Backend
{
public:
// Constructor
using AP_Camera_Backend::AP_Camera_Backend;
/* Do not allow copies */
CLASS_NO_COPY(AP_Camera_Relay);
// update - should be called at 50hz
void update() override;
// entry point to actually take a picture. returns true on success
bool trigger_pic() override;
private:
uint16_t trigger_counter; // count of number of cycles shutter should be held open
};
#endif // AP_CAMERA_ENABLED

71
libraries/AP_Camera/AP_Camera_Servo.cpp Normal file
View File

@ -0,0 +1,71 @@
#include "AP_Camera_Servo.h"
#if AP_CAMERA_ENABLED
#include <SRV_Channel/SRV_Channel.h>
extern const AP_HAL::HAL& hal;
// update - should be called at 50hz
void AP_Camera_Servo::update()
{
// shutter counter
if (trigger_counter > 0) {
trigger_counter--;
} else {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _params.servo_off_pwm);
}
// iso counter
if (iso_counter > 0) {
iso_counter--;
} else {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_iso, _params.servo_off_pwm);
}
// call parent update
AP_Camera_Backend::update();
}
// entry point to actually take a picture. returns true on success
bool AP_Camera_Servo::trigger_pic()
{
// fail if have not completed previous picture
if (trigger_counter > 0) {
return false;
}
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _params.servo_on_pwm);
// set counter to move servo to off position after this many iterations of update (assumes 50hz update rate)
trigger_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX);
return true;
}
// configure camera
void AP_Camera_Servo::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
{
// designed to control Blackmagic Micro Cinema Camera (BMMCC) cameras
// if the message contains non zero values then use them for the below functions
if (ISO > 0) {
// set a trigger for the iso function that is flip controlled
iso_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX);
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_iso, _params.servo_on_pwm);
}
if (aperture > 0) {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_aperture, (uint16_t)aperture);
}
if (shutter_speed > 0) {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_shutter_speed, (uint16_t)shutter_speed);
}
// Use the shooting mode PWM value for the BMMCC as the focus control - no need to modify or create a new MAVlink message type.
if (shooting_mode > 0) {
SRV_Channels::set_output_pwm(SRV_Channel::k_cam_focus, (uint16_t)shooting_mode);
}
}
#endif // AP_CAMERA_ENABLED

50
libraries/AP_Camera/AP_Camera_Servo.h Normal file
View File

@ -0,0 +1,50 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Camera servo driver backend class
*/
#pragma once
#include "AP_Camera_Backend.h"
#if AP_CAMERA_ENABLED
class AP_Camera_Servo : public AP_Camera_Backend
{
public:
// Constructor
using AP_Camera_Backend::AP_Camera_Backend;
/* Do not allow copies */
CLASS_NO_COPY(AP_Camera_Servo);
// update - should be called at 50hz
void update() override;
// entry point to actually take a picture. returns true on success
bool trigger_pic() override;
// configure camera
void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) override;
private:
uint16_t trigger_counter; // count of number of cycles shutter should be held open
uint16_t iso_counter; // count of number of cycles iso output should be held open
};
#endif // AP_CAMERA_ENABLED

33
libraries/AP_Camera/AP_Camera_SoloGimbal.cpp
View File

@ -1,22 +1,19 @@
#include "AP_Camera_SoloGimbal.h" #include "AP_Camera_SoloGimbal.h"
#if AP_CAMERA_ENABLED && HAL_SOLO_GIMBAL_ENABLED
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#if HAL_SOLO_GIMBAL_ENABLED
GOPRO_CAPTURE_MODE AP_Camera_SoloGimbal::gopro_capture_mode;
GOPRO_HEARTBEAT_STATUS AP_Camera_SoloGimbal::gopro_status;
bool AP_Camera_SoloGimbal::gopro_is_recording;
mavlink_channel_t AP_Camera_SoloGimbal::heartbeat_channel;
// Toggle the shutter on the GoPro // Toggle the shutter on the GoPro
// This is so ArduPilot can toggle the shutter directly, either for mission/GCS commands, or when the // This is so ArduPilot can toggle the shutter directly, either for mission/GCS commands, or when the
// Solo's gimbal is installed on a vehicle other than a Solo. The usual GoPro controls thorugh the // Solo's gimbal is installed on a vehicle other than a Solo. The usual GoPro controls thorugh the
// Solo app and Solo controller do not use this, as it is done offboard on the companion computer. // Solo app and Solo controller do not use this, as it is done offboard on the companion computer.
void AP_Camera_SoloGimbal::gopro_shutter_toggle() // entry point to actually take a picture. returns true on success
bool AP_Camera_SoloGimbal::trigger_pic()
{ {
if (gopro_status != GOPRO_HEARTBEAT_STATUS_CONNECTED) { if (gopro_status != GOPRO_HEARTBEAT_STATUS_CONNECTED) {
gcs().send_text(MAV_SEVERITY_ERROR, "GoPro Not Available"); gcs().send_text(MAV_SEVERITY_ERROR, "GoPro Not Available");
return; return false;
} }
const uint8_t gopro_shutter_start[4] = { 1, 0, 0, 0}; const uint8_t gopro_shutter_start[4] = { 1, 0, 0, 0};
@ -38,18 +35,22 @@ void AP_Camera_SoloGimbal::gopro_shutter_toggle()
mavlink_msg_gopro_set_request_send(heartbeat_channel, mavlink_system.sysid, MAV_COMP_ID_GIMBAL,GOPRO_COMMAND_SHUTTER,gopro_shutter_start); mavlink_msg_gopro_set_request_send(heartbeat_channel, mavlink_system.sysid, MAV_COMP_ID_GIMBAL,GOPRO_COMMAND_SHUTTER,gopro_shutter_start);
} }
} else { } else {
gcs().send_text(MAV_SEVERITY_ERROR, "GoPro Unsupported Capture Mode"); gcs().send_text(MAV_SEVERITY_ERROR, "GoPro Unsupported Capture Mode");
return false;
} }
return true;
} }
// Cycle the GoPro capture mode // Cycle the GoPro capture mode
// This is so ArduPilot can cycle through the capture modes of the GoPro directly, probably with an RC Aux function. // This is so ArduPilot can cycle through the capture modes of the GoPro directly, probably with an RC Aux function.
// This is primarily for Solo's gimbal being installed on a vehicle other than a Solo. The usual GoPro controls // This is primarily for Solo's gimbal being installed on a vehicle other than a Solo. The usual GoPro controls
// through the Solo app and Solo controller do not use this, as it is done offboard on the companion computer. // through the Solo app and Solo controller do not use this, as it is done offboard on the companion computer.
void AP_Camera_SoloGimbal::gopro_capture_mode_toggle() // momentary switch to change camera between picture and video modes
void AP_Camera_SoloGimbal::cam_mode_toggle()
{ {
uint8_t gopro_capture_mode_values[4] = { }; uint8_t gopro_capture_mode_values[4] = { };
if (gopro_status != GOPRO_HEARTBEAT_STATUS_CONNECTED) { if (gopro_status != GOPRO_HEARTBEAT_STATUS_CONNECTED) {
gcs().send_text(MAV_SEVERITY_ERROR, "GoPro Not Available"); gcs().send_text(MAV_SEVERITY_ERROR, "GoPro Not Available");
return; return;
@ -67,7 +68,7 @@ void AP_Camera_SoloGimbal::gopro_capture_mode_toggle()
gcs().send_text(MAV_SEVERITY_INFO, "GoPro changing to mode photo"); gcs().send_text(MAV_SEVERITY_INFO, "GoPro changing to mode photo");
} }
break; break;
case GOPRO_CAPTURE_MODE_PHOTO: case GOPRO_CAPTURE_MODE_PHOTO:
default: default:
// Change to video mode // Change to video mode
@ -78,8 +79,8 @@ void AP_Camera_SoloGimbal::gopro_capture_mode_toggle()
} }
} }
// heartbeat from the Solo gimbal GoPro // handle incoming heartbeat from the Solo gimbal GoPro
void AP_Camera_SoloGimbal::handle_gopro_heartbeat(mavlink_channel_t chan, const mavlink_message_t &msg) void AP_Camera_SoloGimbal::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
{ {
mavlink_gopro_heartbeat_t report_msg; mavlink_gopro_heartbeat_t report_msg;
mavlink_msg_gopro_heartbeat_decode(&msg, &report_msg); mavlink_msg_gopro_heartbeat_decode(&msg, &report_msg);
@ -113,4 +114,4 @@ void AP_Camera_SoloGimbal::handle_gopro_heartbeat(mavlink_channel_t chan, const
} }
} }
#endif // HAL_SOLO_GIMBAL_ENABLED #endif // AP_CAMERA_ENABLED && HAL_SOLO_GIMBAL_ENABLED

36
libraries/AP_Camera/AP_Camera_SoloGimbal.h
View File

@ -1,23 +1,37 @@
#pragma once #pragma once
#include <GCS_MAVLink/GCS_MAVLink.h> #include "AP_Camera_Backend.h"
#include <AP_Mount/AP_Mount.h> #include <AP_Mount/AP_Mount.h>
#if HAL_SOLO_GIMBAL_ENABLED #if AP_CAMERA_ENABLED && HAL_SOLO_GIMBAL_ENABLED
class AP_Camera_SoloGimbal { #include <GCS_MAVLink/GCS_MAVLink.h>
class AP_Camera_SoloGimbal : public AP_Camera_Backend
{
public: public:
static void gopro_shutter_toggle(); // Constructor
static void gopro_capture_mode_toggle(); using AP_Camera_Backend::AP_Camera_Backend;
static void handle_gopro_heartbeat(mavlink_channel_t chan, const mavlink_message_t &msg);
/* Do not allow copies */
CLASS_NO_COPY(AP_Camera_SoloGimbal);
// entry point to actually take a picture. returns true on success
bool trigger_pic() override;
// momentary switch to change camera between picture and video modes
void cam_mode_toggle() override;
// handle incoming mavlink message
void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) override;
private: private:
static GOPRO_CAPTURE_MODE gopro_capture_mode; GOPRO_CAPTURE_MODE gopro_capture_mode;
static GOPRO_HEARTBEAT_STATUS gopro_status; GOPRO_HEARTBEAT_STATUS gopro_status;
static bool gopro_is_recording; bool gopro_is_recording;
static mavlink_channel_t heartbeat_channel; mavlink_channel_t heartbeat_channel;
}; };
#endif // HAL_SOLO_GIMBAL_ENABLED #endif // AP_CAMERA_ENABLED && HAL_SOLO_GIMBAL_ENABLED

8
libraries/AP_Camera/LogStructure.h
View File

@ -9,6 +9,7 @@
// @LoggerMessage: CAM,TRIG // @LoggerMessage: CAM,TRIG
// @Description: Camera shutter information // @Description: Camera shutter information
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
// @Field: I: Instance number
// @Field: GPSTime: milliseconds since start of GPS week // @Field: GPSTime: milliseconds since start of GPS week
// @Field: GPSWeek: weeks since 5 Jan 1980 // @Field: GPSWeek: weeks since 5 Jan 1980
// @Field: Lat: current latitude // @Field: Lat: current latitude
@ -17,11 +18,12 @@
// @Field: RelAlt: current altitude relative to home // @Field: RelAlt: current altitude relative to home
// @Field: GPSAlt: altitude as reported by GPS // @Field: GPSAlt: altitude as reported by GPS
// @Field: Roll: current vehicle roll // @Field: Roll: current vehicle roll
// @Field: Pitch: current vehicle pitch // @Field: Pit: current vehicle pitch
// @Field: Yaw: current vehicle yaw // @Field: Yaw: current vehicle yaw
struct PACKED log_Camera { struct PACKED log_Camera {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
uint8_t instance;
uint32_t gps_time; uint32_t gps_time;
uint16_t gps_week; uint16_t gps_week;
int32_t latitude; int32_t latitude;
@ -36,6 +38,6 @@ struct PACKED log_Camera {
#define LOG_STRUCTURE_FROM_CAMERA \ #define LOG_STRUCTURE_FROM_CAMERA \
{ LOG_CAMERA_MSG, sizeof(log_Camera), \ { LOG_CAMERA_MSG, sizeof(log_Camera), \
"CAM", "QIHLLeeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pitch,Yaw", "s--DUmmmddd", "F--GGBBBBBB" }, \ "CAM", "QBIHLLeeeccC","TimeUS,I,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pit,Yaw", "s#--DUmmmddd", "F---GGBBBBBB" }, \
{ LOG_TRIGGER_MSG, sizeof(log_Camera), \ { LOG_TRIGGER_MSG, sizeof(log_Camera), \
"TRIG", "QIHLLeeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pitch,Yaw", "s--DUmmmddd", "F--GGBBBBBB" }, "TRIG", "QBIHLLeeeccC","TimeUS,I,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pit,Yaw", "s#--DUmmmddd", "F---GGBBBBBB" },