diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 2c8178ae51f83..3892638182d92 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -237,7 +237,7 @@ class Rover : public AP_HAL::HAL::Callbacks { // Camera #if CAMERA == ENABLED - AP_Camera camera{&relay, MASK_LOG_CAMERA, current_loc, ahrs}; + AP_Camera camera{MASK_LOG_CAMERA, current_loc}; #endif // Camera/Antenna mount tracking and stabilisation stuff diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 24a29a6446df1..e0ba8b34799ad 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -486,7 +486,7 @@ class Copter : public AP_HAL::HAL::Callbacks { // Camera #if CAMERA == ENABLED - AP_Camera camera{&relay, MASK_LOG_CAMERA, current_loc, ahrs}; + AP_Camera camera{MASK_LOG_CAMERA, current_loc}; #endif // Camera/Antenna mount tracking and stabilisation stuff diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 3ca8b72ecf107..c3f1c0c2d6dd8 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -291,7 +291,7 @@ class Plane : public AP_HAL::HAL::Callbacks { // Camera #if CAMERA == ENABLED - AP_Camera camera{&relay, MASK_LOG_CAMERA, current_loc, ahrs}; + AP_Camera camera{MASK_LOG_CAMERA, current_loc}; #endif #if OPTFLOW == ENABLED diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 66cbba1dc9b51..bbe02d5ec1ab5 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -406,7 +406,7 @@ class Sub : public AP_HAL::HAL::Callbacks { // Camera #if CAMERA == ENABLED - AP_Camera camera{&relay, MASK_LOG_CAMERA, current_loc, ahrs}; + AP_Camera camera{MASK_LOG_CAMERA, current_loc}; #endif // Camera/Antenna mount tracking and stabilisation stuff diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index bde46339f2a4e..4d25e91eea600 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -1,4 +1,6 @@ #include "AP_Camera.h" + +#include #include #include #include @@ -123,6 +125,10 @@ AP_Camera::servo_pic() void AP_Camera::relay_pic() { + AP_Relay *_apm_relay = AP::relay(); + if (_apm_relay == nullptr) { + return; + } if (_relay_on) { _apm_relay->on(0); } else { @@ -164,7 +170,11 @@ AP_Camera::trigger_pic_cleanup() case AP_CAMERA_TRIGGER_TYPE_SERVO: SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _servo_off_pwm); break; - case AP_CAMERA_TRIGGER_TYPE_RELAY: + case AP_CAMERA_TRIGGER_TYPE_RELAY: { + AP_Relay *_apm_relay = AP::relay(); + if (_apm_relay == nullptr) { + break; + } if (_relay_on) { _apm_relay->off(0); } else { @@ -172,6 +182,7 @@ AP_Camera::trigger_pic_cleanup() } break; } + } } if (_trigger_counter_cam_function) { @@ -274,6 +285,8 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo */ void AP_Camera::send_feedback(mavlink_channel_t chan) { + const AP_AHRS &ahrs = AP::ahrs(); + float altitude, altitude_rel; if (current_loc.relative_alt) { altitude = current_loc.alt+ahrs.get_home().alt; @@ -319,7 +332,7 @@ void AP_Camera::update() return; } - if (_max_roll > 0 && fabsf(ahrs.roll_sensor*1e-2f) > _max_roll) { + if (_max_roll > 0 && fabsf(AP::ahrs().roll_sensor*1e-2f) > _max_roll) { return; } @@ -402,11 +415,11 @@ void AP_Camera::log_picture() if (!using_feedback_pin()) { gcs().send_message(MSG_CAMERA_FEEDBACK); if (logger->should_log(log_camera_bit)) { - logger->Write_Camera(ahrs, current_loc); + logger->Write_Camera(current_loc); } } else { if (logger->should_log(log_camera_bit)) { - logger->Write_Trigger(ahrs, current_loc); + logger->Write_Trigger(current_loc); } } } @@ -447,7 +460,7 @@ void AP_Camera::update_trigger() if (logger->should_log(log_camera_bit)) { uint32_t tdiff = AP_HAL::micros() - timestamp32; uint64_t timestamp = AP_HAL::micros64(); - logger->Write_Camera(ahrs, current_loc, timestamp - tdiff); + logger->Write_Camera(current_loc, timestamp - tdiff); } } } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index a52ff1e65e726..c25c2e0561095 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -5,7 +5,7 @@ #include #include #include -#include +#include #define AP_CAMERA_TRIGGER_TYPE_SERVO 0 #define AP_CAMERA_TRIGGER_TYPE_RELAY 1 @@ -24,13 +24,11 @@ class AP_Camera { public: - AP_Camera(AP_Relay *obj_relay, uint32_t _log_camera_bit, const struct Location &_loc, const AP_AHRS &_ahrs) + AP_Camera(uint32_t _log_camera_bit, const struct Location &_loc) : log_camera_bit(_log_camera_bit) , current_loc(_loc) - , ahrs(_ahrs) { AP_Param::setup_object_defaults(this, var_info); - _apm_relay = obj_relay; _singleton = this; } @@ -91,7 +89,6 @@ class AP_Camera { 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_Relay *_apm_relay; // pointer to relay object from the base class Relay. 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 @@ -124,7 +121,6 @@ class AP_Camera { uint32_t log_camera_bit; const struct Location ¤t_loc; - const AP_AHRS &ahrs; // entry point to trip local shutter (e.g. by relay or servo) void trigger_pic(); diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 750c5dd9e094c..2504a9fb38cb2 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -246,9 +246,9 @@ class AP_Logger void Write_Radio(const mavlink_radio_t &packet); void Write_Message(const char *message); void Write_MessageF(const char *fmt, ...); - void Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const Location ¤t_loc, uint64_t timestamp_us=0); - void Write_Camera(const AP_AHRS &ahrs, const Location ¤t_loc, uint64_t timestamp_us=0); - void Write_Trigger(const AP_AHRS &ahrs, const Location ¤t_loc); + void Write_CameraInfo(enum LogMessages msg, const Location ¤t_loc, uint64_t timestamp_us=0); + void Write_Camera(const Location ¤t_loc, uint64_t timestamp_us=0); + void Write_Trigger(const Location ¤t_loc); void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t temperature, uint16_t current_tot); void Write_Attitude(AP_AHRS &ahrs, const Vector3f &targets); void Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 43039e7a20f40..0e86d78a409fb 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -543,8 +543,10 @@ void AP_Logger::Write_Radio(const mavlink_radio_t &packet) } // Write a Camera packet -void AP_Logger::Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const Location ¤t_loc, uint64_t timestamp_us) +void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location ¤t_loc, uint64_t timestamp_us) { + const AP_AHRS &ahrs = AP::ahrs(); + int32_t altitude, altitude_rel, altitude_gps; if (current_loc.relative_alt) { altitude = current_loc.alt+ahrs.get_home().alt; @@ -578,15 +580,15 @@ void AP_Logger::Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, cons } // Write a Camera packet -void AP_Logger::Write_Camera(const AP_AHRS &ahrs, const Location ¤t_loc, uint64_t timestamp_us) +void AP_Logger::Write_Camera(const Location ¤t_loc, uint64_t timestamp_us) { - Write_CameraInfo(LOG_CAMERA_MSG, ahrs, current_loc, timestamp_us); + Write_CameraInfo(LOG_CAMERA_MSG, current_loc, timestamp_us); } // Write a Trigger packet -void AP_Logger::Write_Trigger(const AP_AHRS &ahrs, const Location ¤t_loc) +void AP_Logger::Write_Trigger(const Location ¤t_loc) { - Write_CameraInfo(LOG_TRIGGER_MSG, ahrs, current_loc, 0); + Write_CameraInfo(LOG_TRIGGER_MSG, current_loc, 0); } // Write an attitude packet