From cc7f26a99b690af253ae634e64dfc87ea5a5da96 Mon Sep 17 00:00:00 2001 From: Sandro Benigno Date: Mon, 17 Dec 2012 13:03:51 -0200 Subject: [PATCH] New Relay class and the subclasses for APM1 and APM2. Updated AP_Camera class. --- libraries/AP_Camera/AP_Camera.cpp | 12 ++++++------ libraries/AP_Camera/AP_Camera.h | 6 ++++-- libraries/AP_Relay/AP_Relay.cpp | 6 ++++++ libraries/AP_Relay/AP_Relay.h | 1 - 4 files changed, 16 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index e4e7d68d30..65bd106611 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -60,9 +60,7 @@ AP_Camera::servo_pic() void AP_Camera::relay_pic() { -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 - relay.on(); -#endif + _apm_relay->on(); // leave a message that it should be active for this many loops (assumes 50hz loops) _trigger_counter = constrain_int16(_trigger_duration*5,0,255); @@ -142,7 +140,7 @@ AP_Camera::trigger_pic_cleanup() break; #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 case AP_CAMERA_TRIGGER_TYPE_RELAY: - relay.off(); + _apm_relay->off(); break; #endif case AP_CAMERA_TRIGGER_TYPE_TRANSISTOR: @@ -162,7 +160,8 @@ AP_Camera::configure_msg(mavlink_message_t* msg) // not for us return; } - // TODO do something with these values + // This values may or not be used by APM + // They are bypassed as "echo" to a external specialized board /* * packet.aperture * packet.command_id @@ -190,7 +189,8 @@ AP_Camera::control_msg(mavlink_message_t* msg) // not for us return; } - // TODO do something with these values + // This values may or not be used by APM (the shot is) + // They are bypassed as "echo" to a external specialized board /* * packet.command_id * packet.extra_param diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 99224f16e1..d6fa2cfe0f 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -2,7 +2,6 @@ /// @file AP_Camera.h /// @brief Photo or video camera manager, with EEPROM-backed storage of constants. -/// @author Amilcar Lucas #ifndef AP_CAMERA_H #define AP_CAMERA_H @@ -10,6 +9,7 @@ #include #include #include +#include #define AP_CAMERA_TRIGGER_TYPE_SERVO 0 #define AP_CAMERA_TRIGGER_TYPE_RELAY 1 @@ -35,11 +35,12 @@ class AP_Camera { public: /// Constructor /// - AP_Camera() : + AP_Camera(AP_Relay *obj_relay) : _trigger_counter(0), // count of number of cycles shutter has been held open _thr_pic_counter(0) // timer variable for throttle_pic { AP_Param::setup_object_defaults(this, var_info); + _apm_relay = obj_relay; } // single entry point to take pictures @@ -62,6 +63,7 @@ private: 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 _thr_pic_counter; // timer variable for throttle_pic + AP_Relay *_apm_relay; // pointer to relay object from the base class Relay. The subclasses could be AP_Relay_APM1 or AP_Relay_APM2 void servo_pic(); // Servo operated camera void relay_pic(); // basic relay activation diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index f7ee805c80..5f14a6e97f 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -12,7 +12,13 @@ extern const AP_HAL::HAL& hal; +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #define RELAY_PIN 47 +#else CONFIG_HAL_BOARD == HAL_BOARD_APM2 +#define RELAY_PIN 26 +#else +#error "no RELAY_PIN defined for this board" +#endif void AP_Relay::init() { hal.gpio->pinMode(RELAY_PIN, GPIO_OUTPUT); diff --git a/libraries/AP_Relay/AP_Relay.h b/libraries/AP_Relay/AP_Relay.h index 377fe78388..660d13fcdf 100644 --- a/libraries/AP_Relay/AP_Relay.h +++ b/libraries/AP_Relay/AP_Relay.h @@ -36,5 +36,4 @@ public: bool get(); }; - #endif /* AP_RELAY_H_ */