pelican_ptu.c
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Ascending Technologies GmbH
00004 All rights reserved.
00005 
00006 Redistribution and use in source and binary forms, with or without
00007 modification, are permitted provided that the following conditions are met:
00008 
00009  * Redistributions of source code must retain the above copyright notice,
00010    this list of conditions and the following disclaimer.
00011  * Redistributions in binary form must reproduce the above copyright
00012    notice, this list of conditions and the following disclaimer in the
00013    documentation and/or other materials provided with the distribution.
00014 
00015 THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND ANY
00016 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00017 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00018 DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY
00019 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00020 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00021 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00022 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00023 LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
00024 OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
00025 DAMAGE.
00026 
00027  */
00028 
00029 /*
00030 *       Camera Pan Tilt Unit (PTU) control
00031 */
00032 
00033 #include "main.h"
00034 #include "system.h"
00035 #include "pelican_ptu.h"
00036 #include "system.h"
00037 #include "LPC214x.h"
00038 
00039 struct CAMERA_PTU CAMERA_ptu;
00040 struct CAMERA_COMMANDS CAMERA_Commands;
00041 
00042 void PTU_init(void)
00043 {
00044 #ifdef HUMMINGBIRD_ROLL_SERVO
00045 #ifndef HUMMINGBIRD_ROLL_SERVO_ON_SSEL0
00046         PINSEL0&=~0x01;
00047         PINSEL0|=0x02;
00048 #else
00049         PINSEL0|=0x8000;
00050         PINSEL0&=~0x4000;
00051 #endif
00052 #endif
00053 
00054 #ifdef PELICAN_PTU
00055 
00056     CAMERA_ptu.servo_pitch_offset=61500;
00057         CAMERA_ptu.servo_pitch_scale=54853;
00058         CAMERA_ptu.servo_pitch_min=46000;
00059         CAMERA_ptu.servo_pitch_max=128000;
00060 
00061         CAMERA_ptu.servo_roll_offset=82000;
00062         CAMERA_ptu.servo_roll_scale=220000;
00063         CAMERA_ptu.servo_roll_min=46000;
00064         CAMERA_ptu.servo_roll_max=128000;
00065 
00066 #endif
00067 }
00068 
00069 
00070 void PTU_update(void)
00071 {
00072         static int ptu_cnt=0;
00073         if(++ptu_cnt>9) //generate 100Hz
00074         {
00075                 ptu_cnt=0;
00076         int angle_pitch, angle_roll;
00077 #ifdef CAMMOUNT_XCONFIG //rotate pitch/roll tiltcompensation for 45°
00078 #ifndef CAM_FACING_FRONT_RIGHT
00079     angle_pitch=IMU_CalcData.angle_nick*707/1000+IMU_CalcData.angle_roll*707/1000;
00080     angle_roll=IMU_CalcData.angle_roll*707/1000-IMU_CalcData.angle_nick*707/1000;
00081 #else
00082     angle_roll=IMU_CalcData.angle_nick*707/1000+IMU_CalcData.angle_roll*707/1000;
00083     angle_pitch=-IMU_CalcData.angle_roll*707/1000+IMU_CalcData.angle_nick*707/1000;
00084 #endif
00085 #else
00086     angle_pitch=IMU_CalcData.angle_nick;
00087     angle_roll=IMU_CalcData.angle_roll;
00088 #endif
00089 #ifndef HUMMINGBIRD_ROLL_SERVO
00090                 if(CAMERA_Commands.status&0x02) //no tilt compensation
00091                 {
00092                         SERVO_move_analog((CAMERA_OFFSET_HUMMINGBIRD+CAMERA_Commands.desired_angle_pitch)*HUMMINGBIRD_SERVO_DIRECTION_PITCH);
00093                 }
00094                 else
00095                 {
00096                         int t=0;        //to overcome compiler optimization problem
00097                         SERVO_move_analog(t+(CAMERA_OFFSET_HUMMINGBIRD+CAMERA_Commands.desired_angle_pitch+IMU_CalcData.angle_nick)*HUMMINGBIRD_SERVO_DIRECTION_PITCH);
00098                 }
00099 #else
00100                 static int cam_angle_pitch=0;
00101 #ifdef SET_CAMERA_ANGLE_INCREMENTAL
00102                 if(LL_1khz_attitude_data.RC_data[4]>192) cam_angle_pitch+=200;
00103                 else if(LL_1khz_attitude_data.RC_data[4]<64) cam_angle_pitch-=200;
00104                 if(cam_angle_pitch>55000) cam_angle_pitch=55000;
00105                 if(cam_angle_pitch<-55000) cam_angle_pitch=-55000;
00106 #else
00107                 cam_angle_pitch=CAMERA_Commands.desired_angle_pitch;
00108 #ifdef APTINA
00109                 if(cam_angle_pitch<-90000) cam_angle_pitch=-90000;
00110                 else if(cam_angle_pitch>0) cam_angle_pitch=0;
00111 #endif
00112 #endif
00113 
00114                 if(CAMERA_Commands.status&0x02) //no tilt compensation
00115                 {
00116                         SERVO_pitch_move((CAMERA_OFFSET_HUMMINGBIRD_PITCH+cam_angle_pitch)*HUMMINGBIRD_SERVO_DIRECTION_PITCH);
00117                         SERVO_roll_move((CAMERA_OFFSET_HUMMINGBIRD_ROLL+CAMERA_Commands.desired_angle_roll)*HUMMINGBIRD_SERVO_DIRECTION_ROLL);
00118                 }
00119                 else
00120                 {
00121                         SERVO_pitch_move((CAMERA_OFFSET_HUMMINGBIRD_PITCH+cam_angle_pitch+angle_pitch)*HUMMINGBIRD_SERVO_DIRECTION_PITCH);
00122                         SERVO_roll_move((CAMERA_OFFSET_HUMMINGBIRD_ROLL+CAMERA_Commands.desired_angle_roll+angle_roll)*HUMMINGBIRD_SERVO_DIRECTION_ROLL);
00123                 }
00124 #endif
00125         }
00126 }
00127 
00128 
00129 void SERVO_pitch_move (int angle)
00130 {
00131     unsigned int value;
00132     value=CAMERA_ptu.servo_pitch_offset+(angle/10)*CAMERA_ptu.servo_pitch_scale/9000;   //9000
00133 
00134     if(value>CAMERA_ptu.servo_pitch_max) value=CAMERA_ptu.servo_pitch_max;
00135     else if(value<CAMERA_ptu.servo_pitch_min) value=CAMERA_ptu.servo_pitch_min;
00136 
00137     PWMMR5 = value;
00138     PWMLER = LER5_EN|LER1_EN|LER2_EN;
00139 }
00140 
00141 void SERVO_roll_move (int angle)
00142 {
00143     int value;
00144     value=CAMERA_ptu.servo_roll_offset+(angle/10)*CAMERA_ptu.servo_roll_scale/9000;     //9000
00145 
00146     if(value>CAMERA_ptu.servo_roll_max) value=CAMERA_ptu.servo_roll_max;
00147     else if(value<CAMERA_ptu.servo_roll_min) value=CAMERA_ptu.servo_roll_min;
00148 
00149 #ifdef HUMMINGBIRD_ROLL_SERVO_ON_SSEL0
00150     PWMMR2 = value;
00151 #else
00152     PWMMR1 = value;
00153 #endif
00154     PWMLER = LER5_EN|LER1_EN|LER2_EN;
00155 }
00156 
00157 


asctec_hl_firmware
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Tue Dec 17 2013 11:39:27