From b6295c12e1928de26576f87fa8134fbbb3756ba2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Jan 2013 10:18:45 +1100 Subject: [PATCH] Plane: fixes for new AP_Relay interface --- ArduPlane/ArduPlane.pde | 9 +++++---- ArduPlane/GCS_Mavlink.pde | 6 +++--- ArduPlane/Parameters.h | 5 ----- ArduPlane/Parameters.pde | 2 +- ArduPlane/config.h | 8 -------- ArduPlane/defines.h | 3 --- ArduPlane/events.pde | 2 -- ArduPlane/system.pde | 2 -- ArduPlane/test.pde | 6 ------ 9 files changed, 9 insertions(+), 34 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index b780a97a30..e53be76ac1 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -236,11 +236,12 @@ AP_HAL::AnalogSource * batt_curr_pin; //////////////////////////////////////////////////////////////////////////////// // Relay //////////////////////////////////////////////////////////////////////////////// - -#if CONFIG_RELAY == ENABLED AP_Relay relay; -#endif +// Camera +#if CAMERA == ENABLED +AP_Camera camera(&relay); +#endif //////////////////////////////////////////////////////////////////////////////// // Global variables @@ -784,7 +785,7 @@ static void medium_loop() #endif #if CAMERA == ENABLED - g.camera.trigger_pic_cleanup(); + camera.trigger_pic_cleanup(); #endif // This is the start of the medium (10 Hz) loop pieces diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 4ea42d3e43..c77af7cc90 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -505,7 +505,7 @@ static void NOINLINE send_statustext(mavlink_channel_t chan) static bool telemetry_delayed(mavlink_channel_t chan) { uint32_t tnow = millis() >> 10; - if (tnow > g.telem_delay) { + if (tnow > (uint32_t)g.telem_delay) { return false; } #if USB_MUX_PIN > 0 @@ -1847,13 +1847,13 @@ mission_failed: #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/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 891320f092..b6bffe7f3f 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -354,11 +354,6 @@ public: AP_Int8 stick_mixing; AP_Int8 rudder_steer; - // Camera -#if CAMERA == ENABLED - AP_Camera camera; -#endif - // RC channels RC_Channel channel_roll; RC_Channel channel_pitch; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 1afc6a02db..95dda92a4f 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -588,7 +588,7 @@ const AP_Param::Info var_info[] PROGMEM = { #if CAMERA == ENABLED // @Group: CAM_ // @Path: ../libraries/AP_Camera/AP_Camera.cpp - GGROUP(camera, "CAM_", AP_Camera), + GOBJECT(camera, "CAM_", AP_Camera), #endif // RC channel diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 9f587a4d4d..406310346d 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -62,9 +62,6 @@ // default choices for a 1280. We can't fit everything in, so we // make some popular choices by default #define LOGGING_ENABLED DISABLED - #ifndef CONFIG_RELAY - # define CONFIG_RELAY DISABLED - #endif #ifndef GEOFENCE_ENABLED # define GEOFENCE_ENABLED DISABLED #endif @@ -98,9 +95,6 @@ # define LED_ON HIGH # define LED_OFF LOW # define USB_MUX_PIN -1 -#ifndef CONFIG_RELAY - # define CONFIG_RELAY ENABLED -#endif # define BATTERY_VOLT_PIN 0 // Battery voltage on A0 # define BATTERY_CURR_PIN 1 // Battery current on A1 # define CONFIG_INS_TYPE CONFIG_INS_OILPAN @@ -117,7 +111,6 @@ #else # define USB_MUX_PIN 23 #endif - # define CONFIG_RELAY DISABLED # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 # define BATTERY_CURR_PIN 2 // Battery current on A2 # define CONFIG_INS_TYPE CONFIG_INS_MPU6000 @@ -135,7 +128,6 @@ # define C_LED_PIN 25 # define LED_ON LOW # define LED_OFF HIGH - # define CONFIG_RELAY DISABLED # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 # define BATTERY_CURR_PIN 2 // Battery current on A2 # define CONFIG_INS_TYPE CONFIG_INS_STUB diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a237a5aab7..1597ddb4ec 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -187,9 +187,6 @@ enum gcs_severity { #define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio #define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-g.curr_amp_offset)*g.curr_amp_per_volt -#define RELAY_PIN 47 - - #define AN4 4 #define AN5 5 diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index 0b9a39af55..b9240d3de4 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -116,9 +116,7 @@ static void update_events(void) break; case EVENT_TYPE_RELAY: -#if CONFIG_RELAY == ENABLED relay.toggle(); -#endif break; } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 23fd66983c..a3afe43b55 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -197,9 +197,7 @@ static void init_ardupilot() pinMode(C_LED_PIN, OUTPUT); // GPS status LED pinMode(A_LED_PIN, OUTPUT); // GPS status LED pinMode(B_LED_PIN, OUTPUT); // GPS status LED -#if CONFIG_RELAY == ENABLED relay.init(); -#endif #if FENCE_TRIGGERED_PIN > 0 pinMode(FENCE_TRIGGERED_PIN, OUTPUT); diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index a32bea118a..4e470bc391 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -14,9 +14,7 @@ static int8_t test_adc(uint8_t argc, const Menu::arg *argv); #endif static int8_t test_ins(uint8_t argc, const Menu::arg *argv); static int8_t test_battery(uint8_t argc, const Menu::arg *argv); -#if CONFIG_RELAY == ENABLED static int8_t test_relay(uint8_t argc, const Menu::arg *argv); -#endif static int8_t test_wp(uint8_t argc, const Menu::arg *argv); static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv); static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); @@ -37,9 +35,7 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { {"passthru", test_passthru}, {"failsafe", test_failsafe}, {"battery", test_battery}, -#if CONFIG_RELAY == ENABLED {"relay", test_relay}, -#endif {"waypoints", test_wp}, {"xbee", test_xbee}, {"eedump", test_eedump}, @@ -284,7 +280,6 @@ test_battery(uint8_t argc, const Menu::arg *argv) } -#if CONFIG_RELAY == ENABLED static int8_t test_relay(uint8_t argc, const Menu::arg *argv) { @@ -307,7 +302,6 @@ test_relay(uint8_t argc, const Menu::arg *argv) } } } -#endif static int8_t test_wp(uint8_t argc, const Menu::arg *argv)