From 54069918310c8e4b1832fef57c911e9cf0f012b3 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 13 Sep 2011 01:24:06 +0200 Subject: [PATCH] Added support for routing any function to any of the aux. servos. This is a manual merge from the APM_Camera branch. It reverts the stuff that Oliver did not menat to do with his commit 6dcbc7f44bc0 --- ArduPlane/APM_Config.h | 16 +- ArduPlane/APM_Config.h.reference | 13 ++ ArduPlane/ArduPlane.pde | 31 +--- ArduPlane/Parameters.h | 39 +++-- ArduPlane/WP_activity.pde | 56 ------- ArduPlane/config.h | 13 ++ ArduPlane/defines.h | 27 ++++ ArduPlane/radio.pde | 18 +-- libraries/APM_RC/APM_RC.h | 11 -- libraries/AP_Camera/AP_Camera.cpp | 113 -------------- libraries/AP_Camera/AP_Camera.h | 57 ------- libraries/AP_DCM/AP_DCM.h | 2 +- libraries/AP_Mount/AP_Mount.cpp | 192 ------------------------ libraries/AP_Mount/AP_Mount.h | 90 ----------- libraries/RC_Channel/RC_Channel.cpp | 2 +- libraries/RC_Channel/RC_Channel.h | 10 +- libraries/RC_Channel/RC_Channel_aux.cpp | 36 ----- libraries/RC_Channel/RC_Channel_aux.h | 8 - 18 files changed, 84 insertions(+), 650 deletions(-) delete mode 100644 ArduPlane/WP_activity.pde delete mode 100644 libraries/AP_Camera/AP_Camera.cpp delete mode 100644 libraries/AP_Camera/AP_Camera.h delete mode 100644 libraries/AP_Mount/AP_Mount.cpp delete mode 100644 libraries/AP_Mount/AP_Mount.h diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 4ebb342fd1..3a8bbb504f 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -1,4 +1,5 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + + // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from // their default values, place the appropriate #define statements here. @@ -21,15 +22,4 @@ #define HIL_PORT 0 #define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK #define GCS_PORT 3 -*/ - - -// ----- Camera definitions ------ -// ------------------------------ -#define CAMERA ENABLED -#define CAM_DEBUG DISABLED - -// - Options to reduce code size - -// ------------------------------- -// Disable text based terminal configuration -#define CLI_ENABLED DISABLED +*/ diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 4c560ef062..52b59655a0 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -304,6 +304,19 @@ //#define FLIGHT_MODE_6 MANUAL // +////////////////////////////////////////////////////////////////////////////// +// RC_5_FUNCT OPTIONAL +// RC_6_FUNCT OPTIONAL +// RC_7_FUNCT OPTIONAL +// RC_8_FUNCT OPTIONAL +// +// The channel 5 through 8 function assignments allow configuration of those +// channels for use with differential ailerons, flaps, flaperons, or camera +// or intrument mounts +// +//#define RC_5_FUNCT RC_5_FUNCT_NONE +//etc. + ////////////////////////////////////////////////////////////////////////////// // For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed // then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index a01fd6501f..9a0da7d2e8 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega V2.24" +#define THISFIRMWARE "ArduPlane V2.24" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi @@ -41,9 +41,6 @@ version 2.1 of the License, or (at your option) any later version. #include // RC Channel Library #include // Range finder library #include -#include // Photo or video camera -#include // Camera mount - #include // MAVLink GCS definitions #include @@ -418,15 +415,6 @@ static unsigned long nav_loopTimer; // used to track the elapsed time for GP static unsigned long dTnav; // Delta Time in milliseconds for navigation computations static float load; // % MCU cycles used -RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function - -//Camera tracking and stabilisation stuff -// -------------------------------------- -#if CAMERA == ENABLED -AP_Mount camera_mount(g_gps, &dcm); - -//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 //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -575,18 +563,6 @@ static void fast_loop() static void medium_loop() { -#if CAMERA == ENABLED - // TODO replace home with a POI coming from a MavLink message or command - //camera_mount.set_GPS_target(home); - - // For now point the camera manually via the RC inputs (later remove these two lines) - // for simple dcm tests, replace k_manual with k_stabilise - camera_mount.set_mode(AP_Mount::k_stabilise); - camera_mount.update_mount(); - - g.camera.trigger_pic_cleanup(); -#endif - // This is the start of the medium (10 Hz) loop pieces // ----------------------------------------- switch(medium_loopCounter) { @@ -734,11 +710,6 @@ static void slow_loop() // ---------------------------------- update_servo_switches(); - update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); - -#if CAMERA == ENABLED - camera_mount.update_mount_type(); -#endif break; case 2: diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7b62d9a8fa..570c38800a 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 12; + static const uint16_t k_format_version = 11; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -112,13 +112,6 @@ public: k_param_RTL_altitude, k_param_inverted_flight_ch, - // - // Camera parameters - // -#if CAMERA == ENABLED - k_param_camera, -#endif - // // 170: Radio settings // @@ -141,6 +134,11 @@ public: k_param_long_fs_action, k_param_gcs_heartbeat_fs_enabled, k_param_throttle_slewrate, + + k_param_rc_5_funct, + k_param_rc_6_funct, + k_param_rc_7_funct, + k_param_rc_8_funct, // // 200: Feed-forward gains @@ -324,20 +322,19 @@ public: AP_Int8 flap_2_percent; AP_Int8 flap_2_speed; - // Camera -#if CAMERA == ENABLED - AP_Camera camera; -#endif - // RC channels RC_Channel channel_roll; RC_Channel channel_pitch; RC_Channel channel_throttle; RC_Channel channel_rudder; - RC_Channel_aux rc_5; - RC_Channel_aux rc_6; - RC_Channel_aux rc_7; - RC_Channel_aux rc_8; + RC_Channel rc_5; + RC_Channel rc_6; + RC_Channel rc_7; + RC_Channel rc_8; + AP_Int8 rc_5_funct; + AP_Int8 rc_6_funct; + AP_Int8 rc_7_funct; + AP_Int8 rc_8_funct; // PID controllers // @@ -431,13 +428,13 @@ public: inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")), sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")), airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")), + rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")), + rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")), + rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")), + rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")), // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! -#if CAMERA == ENABLED - camera (k_param_camera, PSTR("CAM_")), -#endif - // RC channel group key name //---------------------------------------------------------------------- channel_roll (k_param_channel_roll, PSTR("RC1_")), diff --git a/ArduPlane/WP_activity.pde b/ArduPlane/WP_activity.pde deleted file mode 100644 index 474644cb90..0000000000 --- a/ArduPlane/WP_activity.pde +++ /dev/null @@ -1,56 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CAMERA == ENABLED -void waypoint_check() -{ - if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want - if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you - g.camera.trigger_pic(); - } // DO SOMETHIMNG - } - if(g.waypoint_index == 20){ // When here do whats underneath - g.camera.trigger_pic(); - } -} - -void picture_time_check() -{ - if (g.camera.picture_time == 1){ - if (wp_distance < g.camera.wp_distance_min){ - g.camera.trigger_pic(); // or any camera activation command - } - } -} -#endif - -void egg_waypoint() -{ - if (g_rc_function[RC_Channel_aux::k_egg_drop]) - { - float temp = (float)(current_loc.alt - home.alt) * .01; - float egg_dist = sqrt(temp / 4.903) * (float)g_gps->ground_speed *.01; - - if(g.waypoint_index == 3){ - if(wp_distance < egg_dist){ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 100; - } - }else{ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 0; - } - } -} - -#if CAMERA == ENABLED -void johann_check() // if you aren't Johann it doesn't really matter :D -{ - APM_RC.OutputCh(CH_7,1500 + (350)); - if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want - if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you - g.camera.trigger_pic(); - } - } - if(g.waypoint_index == 20){ // When here do whats underneath - g.camera.trigger_pic(); - } -} -#endif diff --git a/ArduPlane/config.h b/ArduPlane/config.h index d48b4cb73d..e6261d3560 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -226,6 +226,19 @@ # define CH8_MAX 2000 #endif +////////////////////////////////////////////////////////////////////////////// +#ifndef RC_5_FUNCT +# define RC_5_FUNCT RC_5_FUNCT_NONE +#endif +#ifndef RC_6_FUNCT +# define RC_6_FUNCT RC_6_FUNCT_NONE +#endif +#ifndef RC_7_FUNCT +# define RC_7_FUNCT RC_7_FUNCT_NONE +#endif +#ifndef RC_8_FUNCT +# define RC_8_FUNCT RC_8_FUNCT_NONE +#endif #ifndef FLAP_1_PERCENT # define FLAP_1_PERCENT 0 diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 5c5b01b10f..b48deed2da 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -41,12 +41,39 @@ #define GPS_PROTOCOL_MTK16 6 #define GPS_PROTOCOL_AUTO 7 +// Radio channels +// Note channels are from 0! +// +// XXX these should be CH_n defines from RC.h at some point. +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 + #define CH_ROLL CH_1 #define CH_PITCH CH_2 #define CH_THROTTLE CH_3 #define CH_RUDDER CH_4 #define CH_YAW CH_4 +#define RC_5_FUNCT_NONE 0 +#define RC_5_FUNCT_AILERON 1 +#define RC_5_FUNCT_FLAP_AUTO 2 +#define RC_5_FUNCT_FLAPERON 3 + +#define RC_6_FUNCT_NONE 0 +#define RC_6_FUNCT_AILERON 1 +#define RC_6_FUNCT_FLAP_AUTO 2 +#define RC_6_FUNCT_FLAPERON 3 + +#define RC_7_FUNCT_NONE 0 + +#define RC_8_FUNCT_NONE 0 + // HIL enumerations #define HIL_PROTOCOL_XPLANE 1 #define HIL_PROTOCOL_MAVLINK 2 diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 5b26479ce6..6cacb73e86 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -28,22 +28,6 @@ static void init_rc_in() G_RC_AUX(k_flap_auto)->set_range(0,100); G_RC_AUX(k_aileron)->set_angle(SERVO_MAX); G_RC_AUX(k_flaperon)->set_range(0,100); -#if CAMERA == ENABLED - G_RC_AUX(k_mount_yaw)->set_range( - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10); - G_RC_AUX(k_mount_pitch)->set_range( - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10); - G_RC_AUX(k_mount_roll)->set_range( - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10); - G_RC_AUX(k_cam_trigger)->set_range( - g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); - G_RC_AUX(k_cam_open)->set_range(0,100); -#endif - G_RC_AUX(k_egg_drop)->set_range(0,100); } static void init_rc_out() @@ -56,7 +40,7 @@ static void init_rc_out() APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); + APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); APM_RC.Init(); // APM Radio initialization diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index bcac692abe..236f85593d 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -5,17 +5,6 @@ #define MIN_PULSEWIDTH 900 #define MAX_PULSEWIDTH 2100 -// Radio channels -// Note channels are from 0! -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 - #include class APM_RC_Class diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp deleted file mode 100644 index ddfbc2d8a3..0000000000 --- a/libraries/AP_Camera/AP_Camera.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include -#include <../RC_Channel/RC_Channel_aux.h> - -extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function -extern long wp_distance; -extern "C" { -void relay_on(); -void relay_off(); -} - -void -AP_Camera::servo_pic() // Servo operated camera -{ - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) - { - g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_max; - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles - } -} - -void -AP_Camera::relay_pic() // basic relay activation -{ - relay_on(); - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles -} - -void -AP_Camera::throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. -{ -// TODO find a way to do this without using the global parameter g -// g.channel_throttle.radio_out = g.throttle_min; - if (thr_pic == 10){ - servo_pic(); // triggering method - thr_pic = 0; -// g.channel_throttle.radio_out = g.throttle_cruise; - } - thr_pic++; -} - -void -AP_Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. -{ -// TODO find a way to do this without using the global parameter g -// g.channel_throttle.radio_out = g.throttle_min; - if (wp_distance < 3){ - servo_pic(); // triggering method -// g.channel_throttle.radio_out = g.throttle_cruise; - } -} - -void -AP_Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output. -{ - // TODO: Assign pin spare pin for output - digitalWrite(camtrig, HIGH); - keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles -} - -// single entry point to take pictures -void -AP_Camera::trigger_pic() -{ - switch (trigger_type) - { - case 0: - servo_pic(); // Servo operated camera - break; - case 1: - relay_pic(); // basic relay activation - break; - case 2: - throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - break; - case 3: - distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - break; - case 4: - NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - break; - } -} - -// de-activate the trigger after some delay, but without using a delay() function -void -AP_Camera::trigger_pic_cleanup() -{ - if (keep_cam_trigg_active_cycles) - { - keep_cam_trigg_active_cycles --; - } - else - { - switch (trigger_type) - { - case 0: - case 2: - case 3: - G_RC_AUX(k_cam_trigger)->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; - break; - case 1: - // TODO for some strange reason the function call bellow gives a linker error - //relay_off(); - PORTL &= ~B00000100; // hardcoded version of relay_off(). Replace with a proper function call later. - break; - case 4: - digitalWrite(camtrig, LOW); - break; - } - } -} diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h deleted file mode 100644 index e1c520131a..0000000000 --- a/libraries/AP_Camera/AP_Camera.h +++ /dev/null @@ -1,57 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file AP_Camera.h -/// @brief Photo or video camera manager, with EEPROM-backed storage of constants. - -#ifndef AP_CAMERA_H -#define AP_CAMERA_H - -#include - -/// @class Camera -/// @brief Object managing a Photo or video camera -class AP_Camera{ -protected: - AP_Var_group _group; // must be before all vars to keep ctor init order correct - -public: - /// Constructor - /// - /// @param key EEPROM storage key for the camera configuration parameters. - /// @param name Optional name for the group. - /// - AP_Camera(AP_Var::Key key, const prog_char_t *name) : - _group(key, name), - trigger_type(&_group, 0, 0, name ? PSTR("TRIGG_TYPE") : 0), - picture_time (0), // waypoint trigger variable - thr_pic (0), // timer variable for throttle_pic - camtrig (83), // PK6 chosen as it not near anything so safer for soldering - keep_cam_trigg_active_cycles (0), - wp_distance_min (10) - {} - - // single entry point to take pictures - void trigger_pic(); - - // de-activate the trigger after some delay, but without using a delay() function - void trigger_pic_cleanup(); - - int picture_time; // waypoint trigger variable - long wp_distance_min; // take picture if distance to WP is smaller than this - -private: - uint8_t keep_cam_trigg_active_cycles; // event loop cycles to keep trigger active - int thr_pic; // timer variable for throttle_pic - int camtrig; // PK6 chosen as it not near anything so safer for soldering - - AP_Int8 trigger_type; // 0=Servo, 1=relay, 2=throttle_off time, 3=throttle_off waypoint 4=transistor - - void servo_pic(); // Servo operated camera - void relay_pic(); // basic relay activation - void throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - void distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - void NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - -}; - -#endif /* AP_CAMERA_H */ diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 8e8af7cb36..9a10eb03bc 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -1,7 +1,7 @@ #ifndef AP_DCM_h #define AP_DCM_h -// temporarily include all other classes here +// teporarily include all other classes here // since this naming is a bit off from the // convention and the AP_DCM should be the top // header file diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp deleted file mode 100644 index ef931eb58c..0000000000 --- a/libraries/AP_Mount/AP_Mount.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include -#include <../RC_Channel/RC_Channel_aux.h> - -extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function - -AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) -{ - _dcm=dcm; - _gps=gps; -} - -void AP_Mount::set_pitch_yaw(int pitchCh, int yawCh) -{ -} - -void AP_Mount::set_GPS_target(Location targetGPSLocation) -{ - _target_GPS_location=targetGPSLocation; - - //set mode - _mount_mode=k_gps_target; - - //update mount position - update_mount(); -} - -void AP_Mount::set_assisted(int roll, int pitch, int yaw) -{ - _assist_angles.x = roll; - _assist_angles.y = pitch; - _assist_angles.z = yaw; - - //set mode - _mount_mode=k_assisted; - - //update mount position - update_mount(); -} - -//sets the servo angles for FPV, note angles are * 100 -void AP_Mount::set_mount_free_roam(int roll, int pitch, int yaw) -{ - _roam_angles.x=roll; - _roam_angles.y=pitch; - _roam_angles.z=yaw; - - //set mode - _mount_mode=k_roam; - - //now update mount position - update_mount(); -} - -//sets the servo angles for landing, note angles are * 100 -void AP_Mount::set_mount_landing(int roll, int pitch, int yaw) -{ - _landing_angles.x=roll; - _landing_angles.y=pitch; - _landing_angles.z=yaw; - - //set mode - _mount_mode=k_landing; - - //now update mount position - update_mount(); -} - -void AP_Mount::set_none() -{ - //set mode - _mount_mode=k_none; - - //now update mount position - update_mount(); -} - -void AP_Mount::update_mount() -{ - Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs - Vector3f targ; //holds target vector, var is used as temp in calcs - - switch(_mount_mode) - { - case k_gps_target: - { - if(_gps->fix) - { - calc_GPS_target_vector(&_target_GPS_location); - } - m = _dcm->get_dcm_transposed(); - targ = m*_GPS_vector; - roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll - pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch - break; - } - case k_stabilise: - { - // TODO replace this simplified implementation with a proper one - roll_angle = -_dcm->roll_sensor; - pitch_angle = -_dcm->pitch_sensor; - yaw_angle = -_dcm->yaw_sensor; - break; - } - case k_roam: - { - roll_angle=100*_roam_angles.x; - pitch_angle=100*_roam_angles.y; - yaw_angle=100*_roam_angles.z; - break; - } - case k_assisted: - { - m = _dcm->get_dcm_transposed(); - //rotate vector - targ = m*_assist_vector; - roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll - pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch - break; - } - case k_landing: - { - roll_angle=100*_roam_angles.x; - pitch_angle=100*_roam_angles.y; - yaw_angle=100*_roam_angles.z; - break; - } - case k_manual: // radio manual control - if (g_rc_function[RC_Channel_aux::k_mount_roll]) - roll_angle = map(g_rc_function[RC_Channel_aux::k_mount_roll]->radio_in, - g_rc_function[RC_Channel_aux::k_mount_roll]->radio_min, - g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max, - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max); - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) - pitch_angle = map(g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_in, - g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_min, - g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max); - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) - yaw_angle = map(g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_in, - g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_min, - g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max); - break; - case k_none: - default: - { - //do nothing - break; - } - } - - // write the results to the servos - // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - G_RC_AUX(k_mount_roll)->closest_limit(roll_angle/10); - G_RC_AUX(k_mount_pitch)->closest_limit(pitch_angle/10); - G_RC_AUX(k_mount_yaw)->closest_limit(yaw_angle/10); -} - -void AP_Mount::set_mode(MountMode mode) -{ - _mount_mode=mode; -} - -void AP_Mount::calc_GPS_target_vector(struct Location *target) -{ - _GPS_vector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195; - _GPS_vector.y = (target->lat-_gps->latitude)*.01113195; - _GPS_vector.z = (_gps->altitude-target->alt); -} - -void -AP_Mount::update_mount_type() -{ - // Auto-detect the mount gimbal type depending on the functions assigned to the servos - if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) - { - _mount_type = k_pan_tilt; - } - if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL)) - { - _mount_type = k_tilt_roll; - } - if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) - { - _mount_type = k_pan_tilt_roll; - } -} diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h deleted file mode 100644 index e80c3e294a..0000000000 --- a/libraries/AP_Mount/AP_Mount.h +++ /dev/null @@ -1,90 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/************************************************************ -* AP_mount -- library to control a 2 or 3 axis mount. * -* * -* Author: Joe Holdsworth; * -* Ritchie Wilson; * -* Amilcar Lucas; * -* * -* Purpose: Move a 2 or 3 axis mount attached to vehicle, * -* Used for mount to track targets or stabilise * -* camera plus other modes. * -* * -* Usage: Use in main code to control mounts attached to * -* vehicle. * -* * -*Comments: All angles in degrees * 100, distances in meters* -* unless otherwise stated. * - ************************************************************/ -#ifndef AP_Mount_H -#define AP_Mount_H - -//#include -#include -#include -#include - -class AP_Mount -{ -public: - //Constructors - AP_Mount(GPS *gps, AP_DCM *dcm); - - //enums - enum MountMode{ - k_gps_target = 0, - k_stabilise = 1, //note the correct English spelling :) - k_roam = 2, - k_assisted = 3, - k_landing = 4, - k_none = 5, - k_manual = 6 - }; - - enum MountType{ - k_pan_tilt = 0, //yaw-pitch - k_tilt_roll = 1, //pitch-roll - k_pan_tilt_roll = 2, //yaw-pitch-roll - }; - - //Accessors - void set_pitch_yaw(int pitchCh, int yawCh); - void set_pitch_roll(int pitchCh, int rollCh); - void set_pitch_roll_yaw(int pitchCh, int rollCh, int yawCh); - - void set_GPS_target(Location targetGPSLocation); //used to tell the mount to track GPS location - void set_assisted(int roll, int pitch, int yaw); - void set_mount_free_roam(int roll, int pitch, int yaw);//used in the FPV for example, - void set_mount_landing(int roll, int pitch, int yaw); //set mount landing position - void set_none(); - - //methods - void update_mount(); - void update_mount_type(); //Auto-detect the mount gimbal type depending on the functions assigned to the servos - void set_mode(MountMode mode); - - int pitch_angle; //degrees*100 - int roll_angle; //degrees*100 - int yaw_angle; //degrees*100 -protected: - //methods - void calc_GPS_target_vector(struct Location *target); - //void CalculateDCM(int roll, int pitch, int yaw); - //members - AP_DCM *_dcm; - GPS *_gps; - - MountMode _mount_mode; - MountType _mount_type; - - struct Location _target_GPS_location; - Vector3f _GPS_vector; //target vector calculated stored in meters - - Vector3i _roam_angles; //used for roam mode vector.x = roll vector.y = pitch, vector.z=yaw - Vector3i _landing_angles; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw - - Vector3i _assist_angles; //used to keep angles that user has supplied from assisted targeting - Vector3f _assist_vector; //used to keep vector calculated from _AssistAngles -}; -#endif \ No newline at end of file diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index aa189dff15..24fae0f5ea 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -120,7 +120,7 @@ RC_Channel::calc_pwm(void) { if(_type == RC_CHANNEL_RANGE){ pwm_out = range_to_pwm(); - radio_out = (_reverse >=0 ? pwm_out + radio_min : radio_max - pwm_out); + radio_out = pwm_out + radio_min; }else if(_type == RC_CHANNEL_ANGLE_RAW){ pwm_out = (float)servo_out * .1; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 132e9146f3..aec2ebc998 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -7,11 +7,12 @@ #define RC_Channel_h #include +#include /// @class RC_Channel /// @brief Object managing one RC channel class RC_Channel{ - protected: + private: AP_Var_group _group; // must be before all vars to keep ctor init order correct public: @@ -102,7 +103,8 @@ class RC_Channel{ int16_t _low; }; -// This is ugly, but it fixes compilation on arduino -#include "RC_Channel_aux.h" - #endif + + + + diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index 55ed6819af..18b35ea05e 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -5,42 +5,6 @@ extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function -int16_t -RC_Channel_aux::closest_limit(int16_t angle) -{ - // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - int16_t min = angle_min / 10; - int16_t max = angle_max / 10; - - // Make sure the angle lies in the interval [-180 .. 180[ degrees - while (angle < -1800) angle += 3600; - while (angle >= 1800) angle -= 3600; - - // Make sure the angle limits lie in the interval [-180 .. 180[ degrees - while (min < -1800) min += 3600; - while (min >= 1800) min -= 3600; - while (max < -1800) max += 3600; - while (max >= 1800) max -= 3600; - // This is done every time because the user might change the min, max values on the fly - set_range(min, max); - - // If the angle is outside servo limits, saturate the angle to the closest limit - // On a circle the closest angular position must be carefully calculated to account for wrap-around - if ((angle < min) && (angle > max)){ - // angle error if min limit is used - int16_t err_min = min - angle + (anglemax?0:3600); // add 360 degrees if on the "wrong side" - angle = err_min