2012-06-13 16:00:20 -03:00
|
|
|
/// @file AP_Camera.h
|
|
|
|
/// @brief Photo or video camera manager, with EEPROM-backed storage of constants.
|
2016-02-17 21:25:17 -04:00
|
|
|
#pragma once
|
2012-06-13 16:00:20 -03:00
|
|
|
|
2022-06-02 05:28:26 -03:00
|
|
|
#include "AP_Camera_config.h"
|
|
|
|
|
|
|
|
#if AP_CAMERA_ENABLED
|
|
|
|
|
2022-03-03 23:29:46 -04:00
|
|
|
#include <AP_Common/Location.h>
|
|
|
|
#include <AP_Logger/LogStructure.h>
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_Param/AP_Param.h>
|
2022-02-25 01:06:27 -04:00
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
2023-02-10 20:27:39 -04:00
|
|
|
#include "AP_Camera_Params.h"
|
2023-03-06 17:41:41 -04:00
|
|
|
#include "AP_Mount/AP_Mount_config.h"
|
2012-06-13 16:00:20 -03:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
#define AP_CAMERA_MAX_INSTANCES 2 // maximum number of camera backends
|
2012-12-06 04:46:09 -04:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
// declare backend classes
|
|
|
|
class AP_Camera_Backend;
|
|
|
|
class AP_Camera_Servo;
|
|
|
|
class AP_Camera_Relay;
|
|
|
|
class AP_Camera_SoloGimbal;
|
|
|
|
class AP_Camera_Mount;
|
2015-06-07 14:11:30 -03:00
|
|
|
|
2012-06-13 16:00:20 -03:00
|
|
|
/// @class Camera
|
|
|
|
/// @brief Object managing a Photo or video camera
|
2012-08-17 03:09:29 -03:00
|
|
|
class AP_Camera {
|
2012-06-13 16:00:20 -03:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
// 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;
|
|
|
|
|
2012-06-13 16:00:20 -03:00
|
|
|
public:
|
2023-02-10 20:27:39 -04:00
|
|
|
|
|
|
|
// constructor
|
|
|
|
AP_Camera(uint32_t _log_camera_bit);
|
2012-06-13 16:00:20 -03:00
|
|
|
|
2017-08-29 15:48:37 -03:00
|
|
|
/* Do not allow copies */
|
2022-09-30 06:50:43 -03:00
|
|
|
CLASS_NO_COPY(AP_Camera);
|
2017-08-29 15:48:37 -03:00
|
|
|
|
2018-04-14 01:03:29 -03:00
|
|
|
// get singleton instance
|
2023-02-10 20:27:39 -04:00
|
|
|
static AP_Camera *get_singleton() { return _singleton; }
|
|
|
|
|
|
|
|
// enums
|
|
|
|
enum class CameraType {
|
|
|
|
NONE = 0, // None
|
2023-03-06 19:04:45 -04:00
|
|
|
#if AP_CAMERA_SERVO_ENABLED
|
2023-02-10 20:27:39 -04:00
|
|
|
SERVO = 1, // Servo/PWM controlled camera
|
2023-03-06 19:04:45 -04:00
|
|
|
#endif
|
2023-03-06 19:04:43 -04:00
|
|
|
#if AP_CAMERA_RELAY_ENABLED
|
2023-02-10 20:27:39 -04:00
|
|
|
RELAY = 2, // Relay controlled camera
|
2023-03-06 19:04:43 -04:00
|
|
|
#endif
|
2023-03-06 19:04:39 -04:00
|
|
|
#if AP_CAMERA_SOLOGIMBAL_ENABLED
|
2023-02-10 20:27:39 -04:00
|
|
|
SOLOGIMBAL = 3, // GoPro in Solo gimbal
|
2023-03-06 19:04:39 -04:00
|
|
|
#endif
|
2023-03-06 18:15:58 -04:00
|
|
|
#if AP_CAMERA_MOUNT_ENABLED
|
2023-02-10 20:27:39 -04:00
|
|
|
MOUNT = 4, // Mount library implements camera
|
2023-03-06 18:15:58 -04:00
|
|
|
#endif
|
2023-03-06 19:04:30 -04:00
|
|
|
#if AP_CAMERA_MAVLINK_ENABLED
|
2023-02-10 20:27:39 -04:00
|
|
|
MAVLINK = 5, // MAVLink enabled camera
|
2023-03-06 19:04:30 -04:00
|
|
|
#endif
|
2023-02-10 20:27:39 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
// detect and initialise backends
|
|
|
|
void init();
|
|
|
|
|
|
|
|
// update - to be called periodically at 50Hz
|
|
|
|
void update();
|
2017-08-29 15:48:37 -03:00
|
|
|
|
2012-08-17 03:09:29 -03:00
|
|
|
// MAVLink methods
|
2023-02-10 20:27:39 -04:00
|
|
|
void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
|
2023-03-07 02:39:52 -04:00
|
|
|
MAV_RESULT handle_command_long(const mavlink_command_long_t &packet);
|
2023-02-10 20:27:39 -04:00
|
|
|
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);
|
2012-06-13 16:00:20 -03:00
|
|
|
|
2017-07-25 23:45:20 -03:00
|
|
|
// handle camera control
|
2023-02-10 20:27:39 -04:00
|
|
|
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);
|
2015-04-18 10:30:03 -03:00
|
|
|
|
2013-10-11 07:29:01 -03:00
|
|
|
// set camera trigger distance in a mission
|
2023-03-06 17:41:41 -04:00
|
|
|
void set_trigger_distance(float distance_m);
|
2023-02-10 20:27:39 -04:00
|
|
|
void set_trigger_distance(uint8_t instance, float distance_m);
|
2013-10-11 07:29:01 -03:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
// momentary switch to change camera between picture and video modes
|
2023-03-06 17:41:41 -04:00
|
|
|
void cam_mode_toggle();
|
2023-02-10 20:27:39 -04:00
|
|
|
void cam_mode_toggle(uint8_t instance);
|
2020-02-09 01:47:52 -04:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
// take a picture
|
2023-03-06 17:41:41 -04:00
|
|
|
void take_picture();
|
2023-02-10 20:27:39 -04:00
|
|
|
void take_picture(uint8_t instance);
|
2013-06-24 09:39:50 -03:00
|
|
|
|
2022-09-26 09:06:45 -03:00
|
|
|
// start/stop recording video
|
|
|
|
// start_recording should be true to start recording, false to stop recording
|
2023-03-06 17:41:41 -04:00
|
|
|
bool record_video(bool start_recording);
|
2023-02-10 20:27:39 -04:00
|
|
|
bool record_video(uint8_t instance, bool start_recording);
|
2022-09-26 09:06:45 -03:00
|
|
|
|
|
|
|
// zoom in, out or hold
|
|
|
|
// zoom out = -1, hold = 0, zoom in = 1
|
2023-03-06 17:41:41 -04:00
|
|
|
bool set_zoom_step(int8_t zoom_step);
|
2023-02-10 20:27:39 -04:00
|
|
|
bool set_zoom_step(uint8_t instance, int8_t zoom_step);
|
2022-09-26 09:06:45 -03:00
|
|
|
|
|
|
|
// focus in, out or hold
|
|
|
|
// focus in = -1, focus hold = 0, focus out = 1
|
2023-03-06 17:41:41 -04:00
|
|
|
bool set_manual_focus_step(int8_t focus_step);
|
2023-02-10 20:27:39 -04:00
|
|
|
bool set_manual_focus_step(uint8_t instance, int8_t focus_step);
|
2022-09-26 09:06:45 -03:00
|
|
|
|
|
|
|
// auto focus
|
2023-03-06 17:41:41 -04:00
|
|
|
bool set_auto_focus();
|
2023-02-10 20:27:39 -04:00
|
|
|
bool set_auto_focus(uint8_t instance);
|
2022-09-26 09:06:45 -03:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
// set if vehicle is in AUTO mode
|
|
|
|
void set_is_auto_mode(bool enable) { _is_in_auto_mode = enable; }
|
2017-07-25 23:45:20 -03:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
// parameter var table
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
2012-06-13 16:00:20 -03:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
protected:
|
2017-10-24 07:42:17 -03:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
// 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; }
|
2020-02-15 20:27:08 -04:00
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
// 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];
|
2020-02-15 20:27:08 -04:00
|
|
|
|
2012-06-13 16:00:20 -03:00
|
|
|
private:
|
2018-04-14 01:03:29 -03:00
|
|
|
|
|
|
|
static AP_Camera *_singleton;
|
|
|
|
|
2023-02-10 20:27:39 -04:00
|
|
|
// 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
|
|
|
|
|
|
|
|
// check instance number is valid
|
2023-03-06 17:41:41 -04:00
|
|
|
AP_Camera_Backend *get_instance(uint8_t instance);
|
2023-02-10 20:27:39 -04:00
|
|
|
|
|
|
|
// perform any required parameter conversion
|
|
|
|
void convert_params();
|
2017-07-25 23:45:20 -03:00
|
|
|
|
2023-03-06 17:41:41 -04:00
|
|
|
AP_Camera_Backend *primary; // primary camera backed
|
2023-02-10 20:27:39 -04:00
|
|
|
bool _is_in_auto_mode; // true if in AUTO mode
|
|
|
|
uint32_t log_camera_bit; // logging bit (from LOG_BITMASK) to enable camera logging
|
|
|
|
AP_Camera_Backend *_backends[AP_CAMERA_MAX_INSTANCES]; // pointers to instantiated backends
|
2012-06-13 16:00:20 -03:00
|
|
|
};
|
2018-04-14 01:03:29 -03:00
|
|
|
|
|
|
|
namespace AP {
|
2018-10-03 21:26:34 -03:00
|
|
|
AP_Camera *camera();
|
2018-04-14 01:03:29 -03:00
|
|
|
};
|
2022-06-02 05:28:26 -03:00
|
|
|
|
|
|
|
#endif
|