From 68b62abd3836d64126c45660ceee2534b319bc04 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sat, 22 Dec 2012 17:26:27 +0900 Subject: [PATCH] ArduCopter with the new Camera and Relay classes for APM1 and APM2. All changes by Sandro Benigno --- ArduCopter/ArduCopter.pde | 12 +++++++----- ArduCopter/GCS_Mavlink.pde | 4 ++-- ArduCopter/Parameters.h | 5 ----- ArduCopter/Parameters.pde | 13 +++++++------ ArduCopter/commands_logic.pde | 3 +-- ArduCopter/defines.h | 3 ++- ArduCopter/events.pde | 7 +++---- ArduCopter/system.pde | 2 -- 8 files changed, 22 insertions(+), 27 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c64a505142..86aa0d043c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -822,9 +822,14 @@ static uint32_t last_gps_time; // Used to exit the roll and pitch auto trim function static uint8_t auto_trim_counter; -// Reference to the AP relay object - APM1 only +// Reference to the relay object (APM1 -> PORTL 2) (APM2 -> PORTB 7) AP_Relay relay; +//Reference to the camera object (it uses the relay object inside it) +#if CAMERA == ENABLED + AP_Camera camera(&relay); +#endif + // a pin for reading the receiver RSSI voltage. The scaling by 0.25 // is to take the 0 to 1024 range down to an 8 bit range for MAVLink AP_HAL::AnalogSource* rssi_analog_source; @@ -854,9 +859,6 @@ AP_Mount camera_mount(¤t_loc, g_gps, &ahrs, 0); AP_Mount camera_mount2(¤t_loc, g_gps, &ahrs, 1); #endif -#if CAMERA == ENABLED -//pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82) -#endif //////////////////////////////////////////////////////////////////////////////// // Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters @@ -1197,7 +1199,7 @@ static void fifty_hz_loop() #endif #if CAMERA == ENABLED - g.camera.trigger_pic_cleanup(); + camera.trigger_pic_cleanup(); #endif # if HIL_MODE == HIL_MODE_DISABLED diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index a780535b41..31ae3d2871 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1896,13 +1896,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #if CAMERA == ENABLED case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: { - g.camera.configure_msg(msg); + camera.configure_msg(msg); break; } case MAVLINK_MSG_ID_DIGICAM_CONTROL: { - g.camera.control_msg(msg); + camera.control_msg(msg); break; } #endif // CAMERA == ENABLED diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 39d5ab00dc..4582c96b4d 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -338,11 +338,6 @@ public: AP_Float heli_yaw_ff; // yaw rate feed-forward #endif - // Camera -#if CAMERA == ENABLED - AP_Camera camera; -#endif - // RC channels RC_Channel rc_1; RC_Channel rc_2; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 55e9c65f69..ffcf9a97ac 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -443,12 +443,6 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(heli_yaw_ff, "RATE_YAW_FF", HELI_YAW_FF), #endif -#if CAMERA == ENABLED - // @Group: CAM_ - // @Path: ../libraries/AP_Camera/AP_Camera.cpp - GGROUP(camera, "CAM_", AP_Camera), -#endif - // RC channel //----------- // @Group: RC1_ @@ -569,6 +563,13 @@ const AP_Param::Info var_info[] PROGMEM = { // variables not in the g class which contain EEPROM saved variables + // variables not in the g class which contain EEPROM saved variables +#if CAMERA == ENABLED + // @Group: CAM_ + // @Path: ../libraries/AP_Camera/AP_Camera.cpp + GOBJECT(camera, "CAM_", AP_Camera), +#endif + // @Group: COMPASS_ // @Path: ../libraries/AP_Compass/Compass.cpp GOBJECT(compass, "COMPASS_", Compass), diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 3cbe1f92ac..d7c95de982 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -387,7 +387,6 @@ static bool verify_nav_wp() // Did we pass the WP? // Distance checking if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()) { - // if we have a distance calc error, wp_distance may be less than 0 if(wp_distance > 0) { wp_verify_byte |= NAV_LOCATION; @@ -883,7 +882,7 @@ static void do_nav_roi() static void do_take_picture() { #if CAMERA == ENABLED - g.camera.trigger_pic(); + camera.trigger_pic(); if (g.log_bitmask & MASK_LOG_CAMERA) { Log_Write_Camera(); } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 1408fcda3c..a07a1e4d08 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -385,7 +385,8 @@ enum gcs_severity { #define AN14 68 // NC #define AN15 69 // NC -#define RELAY_PIN 47 +#define RELAY_APM1_PIN 47 +#define RELAY_APM2_PIN 13 #define PIEZO_PIN AN5 //Last pin on the back ADC connector diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index 99226a9513..debf14a7c4 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -100,10 +100,6 @@ static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_ if(event_repeat == 0 || (millis() - event_timer) < event_delay) return; - if (event_repeat > 0) { - event_repeat--; - } - if(event_repeat != 0) { // event_repeat = -1 means repeat forever event_timer = millis(); @@ -118,6 +114,9 @@ static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_ if (event_id == RELAY_TOGGLE) { relay.toggle(); } + if (event_repeat > 0) { + event_repeat--; + } } } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index e4953e4d9a..ec620370b4 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -125,9 +125,7 @@ static void init_ardupilot() pinMode(PUSHBUTTON_PIN, INPUT); // unused #endif -#if CONFIG_RELAY == ENABLED relay.init(); -#endif #if COPTER_LEDS == ENABLED pinMode(COPTER_LED_1, OUTPUT); //Motor LED