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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-07-15 12:08:26 +00:00
parent 45e5f1f183
commit 7aff72db66
11 changed files with 120 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/motor_bench/main_motor_bench.c
+15 -8
View File
@@ -7,7 +7,8 @@
#include "mb_tacho.h"
#include "mb_servo.h"
#include "i2c.h"
#include "mb_twi_controller_asctech.h"
//#include "mb_twi_controller_asctech.h"
#include "mb_twi_controller_mkk.h"
#include "mb_current.h"
#include "mb_scale.h"
@@ -21,6 +22,7 @@
#include "adc.h"
#include "mb_modes.h"
#include "mb_static.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
@@ -51,7 +53,7 @@ static inline void main_init( void ) {
#endif
mb_servo_init();
mb_servo_set_range( 1275000, 1825000 );
mb_servo_set_range( 1090000, 1910000 );
adc_init();
mb_current_init();
@@ -64,8 +66,14 @@ static inline void main_init( void ) {
}
static inline void main_periodic_task( void ) {
mb_mode_periodic();
float rpm = mb_tacho_get_averaged();
mb_current_periodic();
float amps = mb_current_amp;
float thrust = mb_scale_thrust;
float torque = 0.;
mb_mode_periodic(rpm, thrust, amps);
float throttle = mb_modes_throttle;
#if defined USE_TWI_CONTROLLER
@@ -73,17 +81,16 @@ static inline void main_periodic_task( void ) {
#endif
mb_servo_set(throttle);
float rpm = mb_tacho_get_averaged();
mb_current_periodic();
float amps = mb_current_amp;
float thrust = mb_scale_thrust;
float torque = 0.;
RunOnceEvery(125, {
DOWNLINK_SEND_ALIVE(16, MD5SUM);
PeriodicSendDlValue();
});
DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &throttle, &rpm, &amps , &thrust, &torque, &cpu_time_sec, &mb_modes_mode);
}
static inline void main_event_task( void ) {
sw/airborne/motor_bench/mb_buss_twi_controller.h
-12
View File
@@ -1,12 +0,0 @@
#ifndef MB_BUSS_TWI_CONTROLLER_H
#define MB_BUSS_TWI_CONTROLLER_H
#include "std.h"
extern void mb_buss_twi_controller_init(void);
extern void mb_buss_twi_controller_set( float throttle );
#endif /* MB_BUSS_TWI_CONTROLLER_H */
sw/airborne/motor_bench/mb_modes.c
+32 -7
View File
@@ -1,5 +1,7 @@
#include "mb_modes.h"
#include "mb_static.h"
#include "adc.h"
#include "sys_time.h"
@@ -15,11 +17,15 @@ float mb_modes_step_low_throttle;
float mb_modes_step_high_throttle;
float mb_modes_step_duration;
float mb_modes_sine_freq;
float mb_modes_sine_mean;
float mb_modes_sine_ampl;
static void mb_modes_manual( void );
static void mb_modes_ramp( void );
static void mb_modes_step( void );
static void mb_modes_prbs( void );
static void mb_modes_sine( void );
static struct adc_buf mb_modes_adc_buf; /* manual mode */
@@ -30,15 +36,21 @@ void mb_mode_init(void) {
mb_modes_ramp_duration = 200;
mb_modes_step_low_throttle = 0.6;
mb_modes_step_high_throttle = 0.7;
mb_modes_step_duration = 1.;
mb_modes_step_low_throttle = 0.48;
mb_modes_step_high_throttle = 0.65;
mb_modes_step_duration = 0.5;
mb_modes_sine_freq = 80.;
mb_modes_sine_mean = 0.5;
mb_modes_sine_ampl = 0.1;
mb_static_init();
}
void mb_mode_event(void) {}
void mb_mode_periodic(void) {
void mb_mode_periodic(float rpm, float thrust, float current) {
switch (mb_modes_mode) {
case MB_MODES_IDLE :
mb_modes_throttle = 0.;
@@ -47,11 +59,20 @@ void mb_mode_periodic(void) {
mb_modes_manual();
break;
case MB_MODES_RAMP :
mb_modes_ramp();
mb_static_periodic(rpm, thrust, current);
mb_modes_throttle = (float)mb_static_throttle / (float)MB_STATIC_MAX_THROTTLE;
// mb_modes_ramp();
break;
case MB_MODES_STEP :
mb_modes_step();
break;
case MB_MODES_SINE :
mb_modes_sine();
break;
case MB_MODES_FIXED_RPM :
mb_mode_fixed_rpm_periodic(rpm, thrust, current);
mb_modes_throttle = mb_mode_fixed_rpm_throttle;
break;
default:
mb_modes_throttle = 0.;
}
@@ -90,4 +111,8 @@ static void mb_modes_step( void ) {
}
}
static void mb_modes_sine( void ) {
float now = GET_CUR_TIME_FLOAT();
float alpha = 2. * M_PI * mb_modes_sine_freq * now;
mb_modes_throttle = mb_modes_sine_mean + mb_modes_sine_ampl * sin(alpha);
}
sw/airborne/motor_bench/mb_modes.h
+13 -6
View File
@@ -3,12 +3,15 @@
#include "std.h"
#include "mb_mode_fixed_rpm.h"
#define MB_MODES_IDLE 0
#define MB_MODES_MANUAL 1
#define MB_MODES_RAMP 2
#define MB_MODES_STEP 3
#define MB_MODES_PRBS 4
#define MB_MODES_IDLE 0
#define MB_MODES_MANUAL 1
#define MB_MODES_RAMP 2
#define MB_MODES_STEP 3
#define MB_MODES_PRBS 4
#define MB_MODES_SINE 5
#define MB_MODES_FIXED_RPM 6
extern uint8_t mb_modes_mode;
extern float mb_modes_throttle;
@@ -24,11 +27,15 @@ extern float mb_modes_step_duration;
extern void mb_mode_init(void);
extern void mb_mode_event(void);
extern void mb_mode_periodic(void);
extern void mb_mode_periodic(float rpm, float thrust, float current);
#define mb_modes_SetMode(_val) { \
mb_modes_mode = _val; \
mb_modes_last_change_time = GET_CUR_TIME_FLOAT(); \
if (mb_modes_mode == MB_MODES_RAMP) \
mb_static_init(); \
if (mb_modes_mode == MB_MODES_FIXED_RPM) \
mb_mode_fixed_rpm_init(); \
}
#endif /* MB_MODES_H */
sw/airborne/motor_bench/mb_scale.c
+8 -4
View File
@@ -1,15 +1,19 @@
#include "mb_scale.h"
volatile uint32_t mb_scale_pulse_len;
float mb_scale_thrust;
float mb_scale_torque;
volatile float mb_scale_thrust;
volatile float mb_scale_torque;
uint32_t mb_scale_neutral = 2892000; //2944640;
float mb_scale_gain = 1.69e-3; //2.72e-3;
volatile uint32_t mb_scale_neutral = 2892000; //2944640;
float mb_scale_gain = 0.0018584; //1;
volatile uint8_t mb_scale_calib;
void mb_scale_init ( void ) {
/* select pin for capture */
ICP_PINSEL |= ICP_PINSEL_VAL << ICP_PINSEL_BIT;
/* enable capture 0.3 on falling edge + trigger interrupt */
T0CCR |= TCCR_CR3_F | TCCR_CR3_I;
mb_scale_thrust = 0.;
mb_scale_torque = 0.;
mb_scale_calib = 0;
}
sw/airborne/motor_bench/mb_scale.h
+28 -10
View File
@@ -10,24 +10,42 @@
#define ICP_PINSEL_BIT 26
extern volatile uint32_t mb_scale_pulse_len;
extern float mb_scale_thrust;
extern float mb_scale_torque;
extern volatile float mb_scale_thrust;
extern volatile float mb_scale_torque;
extern uint32_t mb_scale_neutral;
extern volatile uint32_t mb_scale_neutral;
extern float mb_scale_gain;
extern volatile uint8_t mb_scale_calib;
#define MB_SCALE_NB_CALIB 50
void mb_scale_init ( void );
#define MB_SCALE_ICP_ISR() { \
static uint32_t last; \
uint32_t now = T0CR3; \
#define MB_SCALE_ICP_ISR() { \
static uint32_t last; \
uint32_t now = T0CR3; \
mb_scale_pulse_len = now - last; \
int32_t diff = (int32_t)mb_scale_pulse_len - (int32_t)mb_scale_neutral; \
mb_scale_thrust = mb_scale_gain * diff; \
last = now; \
last = now; \
if (mb_scale_calib > 0) { \
mb_scale_thrust += mb_scale_pulse_len; \
if (mb_scale_calib >= MB_SCALE_NB_CALIB) { \
mb_scale_neutral = mb_scale_thrust / MB_SCALE_NB_CALIB; \
mb_scale_calib = 0; \
} \
else \
mb_scale_calib++; \
} \
else { \
int32_t diff = (int32_t)mb_scale_pulse_len - (int32_t)mb_scale_neutral; \
mb_scale_thrust = mb_scale_gain * diff; \
} \
}
#define mb_scale_Calib(_val) { \
mb_scale_calib = 1; \
mb_scale_thrust = 0.; \
}
#endif /* MB_SCALE_H */
sw/airborne/motor_bench/mb_servo.c
+1 -1
View File
@@ -13,7 +13,7 @@ void mb_servo_init( void ) {
/* enable and select the type of PWM channel */
PWMPCR |= PWMPCR_ENA5;
/* set Match0 value (refresh rate) */
PWMMR0 = MY_NB_CLOCK_TIMER_PWM(25000);
PWMMR0 = MY_NB_CLOCK_TIMER_PWM(5000);
/* commit PWMMRx changes */
PWMLER = PWMLER_LATCH0;
/* enable PWM timer in PWM mode */
sw/airborne/motor_bench/mb_twi_controller_asctech.c
+5
View File
@@ -79,3 +79,8 @@ void mb_twi_controller_set( float throttle ) {
else
mb_twi_nb_overun++;
}
void mb_twi_controller_set_raw( uint8_t throttle ) {
}
sw/airborne/motor_bench/mb_twi_controller_asctech.h
+1
View File
@@ -5,6 +5,7 @@
extern void mb_twi_controller_init(void);
extern void mb_twi_controller_set( float throttle );
extern void mb_twi_controller_set_raw( uint8_t throttle );
#define MB_TWI_CONTROLLER_COMMAND_NONE 0
#define MB_TWI_CONTROLLER_COMMAND_TEST 1
sw/airborne/motor_bench/mb_buss_twi_controller.c → sw/airborne/motor_bench/mb_twi_controller_mkk.c
+5 -5
View File
@@ -1,4 +1,4 @@
#include "mb_buss_twi_controller.h"
#include "mb_twi_controller_mkk.h"
#include <string.h>
@@ -10,7 +10,7 @@ uint8_t mb_buss_twi_nb_overun;
uint8_t mb_buss_twi_i2c_done;
#define MB_BUSS_TWI_CONTROLLER_MAX_CMD 255
#define MB_BUSS_TWI_CONTROLLER_MAX_CMD 200
/*
Slave address
front = 0x52
@@ -18,14 +18,14 @@ uint8_t mb_buss_twi_i2c_done;
right = 0x56
left = 0x58
*/
#define MB_BUSS_TWI_CONTROLLER_ADDR 0x52
#define MB_BUSS_TWI_CONTROLLER_ADDR 0x56
void mb_buss_twi_controller_init(void) {
void mb_twi_controller_init(void) {
mb_buss_twi_nb_overun = 0;
mb_buss_twi_i2c_done = TRUE;
}
void mb_buss_twi_controller_set( float throttle ) {
void mb_twi_controller_set( float throttle ) {
if (mb_buss_twi_i2c_done) {
mb_buss_twi_command = throttle * MB_BUSS_TWI_CONTROLLER_MAX_CMD;
i2c_buf[0] = mb_buss_twi_command;
sw/airborne/motor_bench/mb_twi_controller_mkk.h
+12
View File
@@ -0,0 +1,12 @@
#ifndef MB_TWI_CONTROLLER_MKK_H
#define MB_TWI_CONTROLLER_MKK_H
#include "std.h"
extern void mb_twi_controller_init(void);
extern void mb_twi_controller_set( float throttle );
#endif /* MB_TWI_CONTROLLER_MKK_H */