From 345e5172725987bc94c24e9150abbb5f6a19e138 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 | 10 +++--- libraries/AP_Camera/AP_Camera.h | 6 ++-- libraries/AP_Relay/AP_Relay.cpp | 49 ---------------------------- libraries/AP_Relay/AP_Relay.h | 38 ++++----------------- libraries/AP_Relay/AP_Relay_APM1.cpp | 36 ++++++++++++++++++++ libraries/AP_Relay/AP_Relay_APM1.h | 29 ++++++++++++++++ libraries/AP_Relay/AP_Relay_APM2.cpp | 36 ++++++++++++++++++++ libraries/AP_Relay/AP_Relay_APM2.h | 29 ++++++++++++++++ libraries/AP_Relay/Relay.h | 27 +++++++++++++++ 9 files changed, 173 insertions(+), 87 deletions(-) delete mode 100644 libraries/AP_Relay/AP_Relay.cpp create mode 100644 libraries/AP_Relay/AP_Relay_APM1.cpp create mode 100644 libraries/AP_Relay/AP_Relay_APM1.h create mode 100644 libraries/AP_Relay/AP_Relay_APM2.cpp create mode 100644 libraries/AP_Relay/AP_Relay_APM2.h create mode 100644 libraries/AP_Relay/Relay.h diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index aecc88ff53..3974579e4a 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -57,7 +57,7 @@ AP_Camera::servo_pic() void AP_Camera::relay_pic() { - relay.on(); + _apm_relay->on(); // leave a message that it should be active for this many loops (assumes 50hz loops) _trigger_counter = constrain(_trigger_duration*5,0,255); @@ -136,7 +136,7 @@ AP_Camera::trigger_pic_cleanup() RC_Channel_aux::set_radio(RC_Channel_aux::k_cam_trigger, _servo_off_pwm); break; case AP_CAMERA_TRIGGER_TYPE_RELAY: - relay.off(); + _apm_relay->off(); break; case AP_CAMERA_TRIGGER_TYPE_TRANSISTOR: digitalWrite(AP_CAMERA_TRANSISTOR_PIN, LOW); @@ -155,7 +155,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 @@ -183,7 +184,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 60e23c36f1..a4836db426 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -2,13 +2,13 @@ /// @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 #include #include +#include #define AP_CAMERA_TRIGGER_TYPE_SERVO 0 #define AP_CAMERA_TRIGGER_TYPE_RELAY 1 @@ -34,10 +34,11 @@ class AP_Camera { public: /// Constructor /// - AP_Camera() : + AP_Camera(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 { + _apm_relay = obj_relay; } // single entry point to take pictures @@ -60,6 +61,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 + 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 deleted file mode 100644 index b236bd886f..0000000000 --- a/libraries/AP_Relay/AP_Relay.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - * AP_Relay.cpp - * - * Created on: Oct 2, 2011 - * Author: Amilcar Lucas - */ - -#include -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "wiring.h" -#endif - -#include "AP_Relay.h" - -void AP_Relay::on() -{ - PORTL |= B00000100; -} - - -void AP_Relay::off() -{ - PORTL &= ~B00000100; -} - - -void AP_Relay::toggle() -{ - PORTL ^= B00000100; -} - - -void AP_Relay::set(bool status) -{ - if (status) - on(); - else - off(); -} - - -bool AP_Relay::get() -{ - return PORTL & B00000100; -} diff --git a/libraries/AP_Relay/AP_Relay.h b/libraries/AP_Relay/AP_Relay.h index 5b73410ed8..b41aa6d4a0 100644 --- a/libraries/AP_Relay/AP_Relay.h +++ b/libraries/AP_Relay/AP_Relay.h @@ -1,37 +1,11 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - * AP_Relay.h - * - * Created on: Oct 2, 2011 - * Author: Amilcar Lucas - */ - -/// @file AP_Relay.h -/// @brief APM relay control class - #ifndef AP_RELAY_H_ #define AP_RELAY_H_ -/// @class AP_Relay -/// @brief Class to manage the APM relay -class AP_Relay { -public: - // activate the relay - void on(); +/// @class AP_Relay +/// @brief Catch-all header that defines the Relay for all APM hardwares - // de-activate the relay - void off(); +#include "Relay.h" +#include "AP_Relay_APM1.h" +#include "AP_Relay_APM2.h" - // toggle the relay status - void toggle(); - - // set the relay status (on/off) - void set(bool status); - - // get the relay status (on/off) - bool get(); -}; - - -#endif /* AP_RELAY_H_ */ +#endif /* AP_RELAY_H_ */ \ No newline at end of file diff --git a/libraries/AP_Relay/AP_Relay_APM1.cpp b/libraries/AP_Relay/AP_Relay_APM1.cpp new file mode 100644 index 0000000000..9cd413de3b --- /dev/null +++ b/libraries/AP_Relay/AP_Relay_APM1.cpp @@ -0,0 +1,36 @@ +#include +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "wiring.h" +#endif + +#include "AP_Relay_APM1.h" + +void AP_Relay_APM1::on() +{ + PORTL |= B00000100; +} + +void AP_Relay_APM1::off() +{ + PORTL &= ~B00000100; +} + +void AP_Relay_APM1::toggle() +{ + PORTL ^= B00000100; +} + +void AP_Relay_APM1::set(bool status) +{ + if (status) + on(); + else + off(); +} + +bool AP_Relay_APM1::get() +{ + return PORTL & B00000100; +} diff --git a/libraries/AP_Relay/AP_Relay_APM1.h b/libraries/AP_Relay/AP_Relay_APM1.h new file mode 100644 index 0000000000..0975daec51 --- /dev/null +++ b/libraries/AP_Relay/AP_Relay_APM1.h @@ -0,0 +1,29 @@ +#ifndef AP_RELAY_APM1_H_ +#define AP_RELAY_APM1_H_ + +#include "Relay.h" + +/// @class AP_Relay_APM1 +/// @brief SubClass from Relay to manage the APM1 onboard relay + + +class AP_Relay_APM1: public Relay{ + public: + + // activate the relay + void on(); + + // de-activate the relay + void off(); + + // toggle the relay status + void toggle(); + + // set the relay status (on/off) + void set(bool status); + + // get the relay status (on/off) + bool get(); +}; + +#endif /* AP_RELAY_APM1_H_ */ diff --git a/libraries/AP_Relay/AP_Relay_APM2.cpp b/libraries/AP_Relay/AP_Relay_APM2.cpp new file mode 100644 index 0000000000..819599a1c0 --- /dev/null +++ b/libraries/AP_Relay/AP_Relay_APM2.cpp @@ -0,0 +1,36 @@ +#include +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "wiring.h" +#endif + +#include "AP_Relay_APM2.h" + +void AP_Relay_APM2::on() +{ + PORTB |= B10000000; +} + +void AP_Relay_APM2::off() +{ + PORTB &= ~B10000000; +} + +void AP_Relay_APM2::toggle() +{ + PORTB ^= B10000000; +} + +void AP_Relay_APM2::set(bool status) +{ + if (status) + on(); + else + off(); +} + +bool AP_Relay_APM2::get() +{ + return PORTB & B10000000; +} \ No newline at end of file diff --git a/libraries/AP_Relay/AP_Relay_APM2.h b/libraries/AP_Relay/AP_Relay_APM2.h new file mode 100644 index 0000000000..88988eecb8 --- /dev/null +++ b/libraries/AP_Relay/AP_Relay_APM2.h @@ -0,0 +1,29 @@ +#ifndef AP_RELAY_APM2_H_ +#define AP_RELAY_APM2_H_ + +#include "Relay.h" + +/// @class AP_Relay_APM2 +/// @brief SubClass from Relay to manage the APM2 A9 pin as external relay port + + +class AP_Relay_APM2: public Relay{ + public: + + // activate the relay + void on(); + + // de-activate the relay + void off(); + + // toggle the relay status + void toggle(); + + // set the relay status (on/off) + void set(bool status); + + // get the relay status (on/off) + bool get(); +}; + +#endif /* AP_RELAY_APM2_H_ */ diff --git a/libraries/AP_Relay/Relay.h b/libraries/AP_Relay/Relay.h new file mode 100644 index 0000000000..6c4e783ed6 --- /dev/null +++ b/libraries/AP_Relay/Relay.h @@ -0,0 +1,27 @@ +#ifndef RELAY_H_ +#define RELAY_H_ + +/// @class Relay +/// @brief Abstract base class for Relays on all APM hardwares + +class Relay +{ + public: + + // activate the relay + virtual void on() = 0; + + // de-activate the relay + virtual void off() = 0; + + // toggle the relay status + virtual void toggle() = 0; + + // set the relay status (on/off) + virtual void set(bool status) = 0; + + // get the relay status (on/off) + virtual bool get() = 0; +}; + +#endif /* RELAY_H_ */ \ No newline at end of file