Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 12:57:27 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

cam and pwm command for booz2

Browse Source
This commit is contained in:
Gautier Hattenberger
2009-05-10 19:45:48 +00:00
parent 8ce03d888a
commit 1037c7141d
6 changed files with 286 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/arm7/booz2_pwm_hw.c
+48
View File
@@ -0,0 +1,48 @@
/*
* $Id: $
*
* Copyright (C) 2007 ENAC
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include "booz2_pwm_hw.h"
#define PWM_PERIOD SYS_TICS_OF_USEC(20000)
#define PWM_DUTY SYS_TICS_OF_USEC(1500)
void booz2_pwm_init_hw( void ) {
/* start PWM5 */
/* select P0.21 as PWM5 */
PWM_PINSEL |= PWM_PINSEL_VAL << PWM_PINSEL_BIT;
/* select pwm period */
PWMMR0 = PWM_PERIOD;
/* select pwm value to 50% at init (1500 us) */
PWMMR5 = PWM_DUTY;
/* commit values */
PWMLER = PWMLER_LATCH0 | PWMLER_LATCH5;
/* enable counter and pwm mode */
PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
/* enable PWM5 */
PWMPCR = PWMPCR_ENA5;
}
sw/airborne/booz/arm7/booz2_pwm_hw.h
+38
View File
@@ -0,0 +1,38 @@
/*
* $Id: $
*
* Copyright (C) 2007 ENAC
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef BOOZ2_PWM_HW_H
#define BOOZ2_PWM_HW_H
#include "std.h"
#include "sys_time.h"
extern void booz2_pwm_init_hw(void);
#define Booz2SetPwmValue(_v) { \
PWMMR5 = SYS_TICS_OF_USEC(_v); \
PWMLER = PWMLER_LATCH5; \
}
#endif /* BOOZ2_PWM_HW_H */
sw/airborne/booz/booz2_cam.c
+91
View File
@@ -0,0 +1,91 @@
/* $Id: booz2_commands.c 3002 2009-02-10 11:36:07Z poine $
*
* (c) 2009 Gautier Hattenberger
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include "booz2_cam.h"
#include "booz2_pwm_hw.h"
//#include "booz2_ahrs.h"
#include "booz2_navigation.h"
#include "booz2_ins.h"
#include "flight_plan.h"
uint8_t booz2_cam_mode;
#ifdef BOOZ2_CAM_TILT_NEUTRAL
int16_t booz2_cam_tilt;
#endif
#ifdef BOOZ2_CAM_PAN_NEUTRAL
int16_t booz2_cam_pan;
#endif
void booz2_cam_init(void) {
booz2_pwm_init_hw();
booz2_cam_mode = BOOZ2_CAM_MODE_NONE;
#ifdef BOOZ2_CAM_TILT_NEUTRAL
booz2_cam_tilt = BOOZ2_CAM_TILT_NEUTRAL;
Booz2SetPwmValue(booz2_cam_tilt);
#endif
#ifdef BOOZ2_CAM_PAN_NEUTRAL
booz2_cam_pan = BOOZ2_CAM_PAN_NEUTRAL;
#endif
}
void booz2_cam_periodic(void) {
switch (booz2_cam_mode) {
case BOOZ2_CAM_MODE_NONE:
#ifdef BOOZ2_CAM_TILT_NEUTRAL
booz2_cam_tilt = BOOZ2_CAM_TILT_NEUTRAL;
#endif
#ifdef BOOZ2_CAM_PAN_NEUTRAL
booz2_cam_pan = BOOZ2_CAM_PAN_NEUTRAL;
#endif
break;
case BOOZ2_CAM_MODE_MANUAL:
case BOOZ2_CAM_MODE_HEADING:
#if defined BOOZ2_CAM_TILT_MIN && defined BOOZ2_CAM_TILT_MAX
Bound(booz2_cam_tilt,BOOZ2_CAM_TILT_MIN,BOOZ2_CAM_TILT_MAX);
Booz2SetPwmValue(booz2_cam_tilt);
#endif
#if defined BOOZ2_CAM_PAN_MIN && defined BOOZ2_CAM_PAN_MAX
//Bound(booz2_cam_pan,BOOZ2_CAM_PAN_MIN,BOOZ2_CAM_PAN_MAX);
nav_heading = booz2_cam_pan;
#endif
break;
case BOOZ2_CAM_MODE_WP:
#ifdef WP_CAM
{
struct Int32Vect2 diff;
VECT2_DIFF(diff, waypoints[WP_CAM], booz_ins_enu_pos);
INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
INT32_ATAN2(booz2_cam_pan,diff.x,diff.y);
//if (diff.y < 0) {
// booz2_cam_pan += INT32_ANGLE_PI;
//}
nav_heading = booz2_cam_pan;
}
#endif
break;
}
Booz2SetPwmValue(booz2_cam_tilt);
}
sw/airborne/booz/booz2_cam.h
+48
View File
@@ -0,0 +1,48 @@
/* $Id: booz2_commands.h 3002 2009-02-10 11:36:07Z poine $
*
* (c) 2009 Gautier Hattenberger
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef BOOZ2_CAM_H
#define BOOZ2_CAM_H
#include "airframe.h"
#include "std.h"
#define BOOZ2_CAM_MODE_NONE 0
#define BOOZ2_CAM_MODE_MANUAL 1
#define BOOZ2_CAM_MODE_HEADING 2
#define BOOZ2_CAM_MODE_WP 3
extern uint8_t booz2_cam_mode;
#ifdef BOOZ2_CAM_TILT_NEUTRAL
extern int16_t booz2_cam_tilt;
#endif
#ifdef BOOZ2_CAM_PAN_NEUTRAL
extern int16_t booz2_cam_pan;
#endif
extern void booz2_cam_init(void);
extern void booz2_cam_periodic(void);
#endif /* BOOZ2_CAM_H */
sw/airborne/booz/sim/booz2_pwm_hw.c
+29
View File
@@ -0,0 +1,29 @@
/*
* $Id: $
*
* Copyright (C) 2007 ENAC
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include "booz2_pwm_hw.h"
void booz2_pwm_init_hw( void ) {}
sw/airborne/booz/sim/booz2_pwm_hw.h
+32
View File
@@ -0,0 +1,32 @@
/*
* $Id: $
*
* Copyright (C) 2007 ENAC
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef BOOZ2_PWM_HW_H
#define BOOZ2_PWM_HW_H
extern void booz2_pwm_init_hw(void);
#define Booz2SetPwmValue(_v) {}
#endif /* BOOZ2_PWM_HW_H */