razzor/sam7fc
1
0
Fork 0
Code Issues Pull Requests Releases Activity

working rc->mixer->blctrls path

Browse Source
This commit is contained in:
Olaf Rempel 2008-03-11 22:02:06 +01:00
parent 87edb0819d
commit a8256718ab
5 changed files with 130 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/at91_adc.h
View File

@ -11,6 +11,7 @@
#define ADC_ACC_YAW 5
#define ADC_VOLTAGE 6
#define ADC_CAL_NONE 0
#define ADC_CAL_GYRO 1
#define ADC_CAL_ACC 2

2
include/at91_tc1.h
View File

@ -11,6 +11,8 @@ struct rc_values {
};
uint32_t rcontrol_getvalues(struct rc_values *rc);
uint32_t rcontrol_getswitches(struct rc_values *rc);
void rcontrol_calibrate(uint32_t mode);
void rcontrol_print_cal(void);

99
main.c
View File

@ -33,54 +33,90 @@
#include <stdio.h>
#include <string.h>
static uint32_t pitc_test(struct pitc_timer *timer)
static uint32_t global_state;
#define MOTORS_RUNNING 0x0001
#define STICK_CALIBRATION 0x0002
static uint32_t stick_timer_cb(struct pitc_timer *timer)
{
static uint32_t i;
*AT91C_PIOA_SODR = i;
i = i ^ LED_GREEN;
*AT91C_PIOA_CODR = i;
/*
struct rc_values rc;
uint32_t count = rcontrol_getvalues(&rc);
uint32_t count = rcontrol_getswitches(&rc);
printf("%ld channels: ", count);
uint32_t j;
for (j = 0; j < count; j++)
printf("%+5d ", rc.chan[j]);
static uint32_t stick_cal_count;
printf("\r");
if (global_state & MOTORS_RUNNING) {
if ((rc.chan[2] < 0 && rc.chan[3] > 0) || count < 4)
global_state &= ~MOTORS_RUNNING;
static uint32_t cnt;
} else if (global_state & STICK_CALIBRATION) {
stick_cal_count--;
if (stick_cal_count == 0) {
rcontrol_calibrate(RC_CAL_END);
rcontrol_print_cal();
global_state &= ~STICK_CALIBRATION;
}
if (cnt++ == 300) {
rcontrol_calibrate(RC_CAL_END);
printf("\n\r");
rcontrol_print_cal();
} else {
if (rc.chan[2] < 0 && rc.chan[3] < 0)
global_state |= MOTORS_RUNNING;
if (rc.chan[2] > 0 && rc.chan[3] > 0)
adc_calibrate(ADC_CAL_GYRO);
if (rc.chan[2] > 0 && rc.chan[3] < 0) {
adc_calibrate(ADC_CAL_ACC);
stick_cal_count = 3000;
global_state |= STICK_CALIBRATION;
rcontrol_calibrate(RC_CAL_START);
}
}
*/
adc_trigger();
rcontrol_getvalues(&rc);
static uint8_t pwm[4];
if (global_state & MOTORS_RUNNING) {
int32_t pitch = rc.chan[2] * 256;
int32_t gier = rc.chan[3] * 64;
int32_t nick = rc.chan[0] * 64;
int32_t roll = rc.chan[1] * 64;
int32_t data[4];
data[0] = (pitch - nick + gier) / 1024;
data[1] = (pitch + nick + gier) / 1024;
data[2] = (pitch + roll - gier) / 1024;
data[3] = (pitch - roll - gier) / 1024;
pwm[0] = (data[0] > 0x0f) ? data[0] : 0x0f;
pwm[1] = (data[1] > 0x0f) ? data[1] : 0x0f;
pwm[2] = (data[2] > 0x0f) ? data[2] : 0x0f;
pwm[3] = (data[3] > 0x0f) ? data[3] : 0x0f;
} else {
pwm[0] = 0x00;
pwm[1] = 0x00;
pwm[2] = 0x00;
pwm[3] = 0x00;
}
// printf("%3d %3d %3d %3d\n\r", pwm[0], pwm[1], pwm[2], pwm[3]);
twi_setpwm(pwm);
// adc_trigger();
return PITC_RESTART_TIMER;
}
static struct pitc_timer pitc_test_timer = {
.interval = 100,
.func = &pitc_test,
static struct pitc_timer stick_timer = {
.interval = 10,
.func = &stick_timer_cb,
};
static void motor_mixer(int16_t pitch, int16_t nick, int16_t roll, int16_t yaw)
{
static uint8_t pwm[4];
pwm[0] = pitch + nick + yaw;
pwm[1] = pitch - nick + yaw;
pwm[2] = pitch + roll - yaw;
pwm[3] = pitch - roll - yaw;
twi_setpwm(pwm);
}
int main(void)
{
/* LED outputs */
@ -113,8 +149,7 @@ int main(void)
/* adc, need timer */
at91_adc_init();
pitc_schedule_timer(&pitc_test_timer);
pitc_schedule_timer(&stick_timer);
// rcontrol_calibrate(RC_CAL_START);
while (1);
}

80
src/at91_adc.c
View File

@ -77,45 +77,7 @@ static uint32_t adc_calibrate_cb(struct pitc_timer *timer)
adc_cal_data[1] /= ADC_CAL_COUNT;
adc_cal_data[2] /= ADC_CAL_COUNT;
adc_cal_mode = 0;
return PITC_REMOVE_TIMER;
}
return PITC_RESTART_TIMER;
}
static struct pitc_timer adc_cal_timer = {
.interval = 1,
.func = &adc_calibrate_cb,
};
void adc_trigger(void)
{
printf("R:%4d:%4d N:%4d:%4d Y:%4d:%4d U:%4d\n\r",
adc_result[ADC_GYRO_ROLL], adc_result[ADC_ACC_ROLL],
adc_result[ADC_GYRO_NICK], adc_result[ADC_ACC_NICK],
adc_result[ADC_GYRO_YAW], adc_result[ADC_ACC_YAW],
adc_result[ADC_VOLTAGE]);
*AT91C_ADC_CR = AT91C_ADC_START;
}
void adc_calibrate(uint32_t mode)
{
if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) {
adc_cal_count = ADC_CAL_COUNT;
adc_cal_mode = mode;
adc_cal_data[0] = 0;
adc_cal_data[1] = 0;
adc_cal_data[2] = 0;
pitc_schedule_timer(&adc_cal_timer);
// wait (1024 * 1ms ~> 1s)
while (adc_cal_mode);
if (mode == ADC_CAL_GYRO) {
if (adc_cal_mode == ADC_CAL_GYRO) {
adc_offset[ADC_GYRO_ROLL] = adc_cal_data[0];
adc_offset[ADC_GYRO_NICK] = adc_cal_data[1];
adc_offset[ADC_GYRO_YAW] = adc_cal_data[2];
@ -140,6 +102,42 @@ void adc_calibrate(uint32_t mode)
adc_offset[ADC_ACC_NICK],
adc_offset[ADC_ACC_YAW]);
}
adc_cal_mode = ADC_CAL_NONE;
return PITC_REMOVE_TIMER;
}
return PITC_RESTART_TIMER;
}
static struct pitc_timer adc_cal_timer = {
.interval = 1,
.func = &adc_calibrate_cb,
};
void adc_trigger(void)
{
printf("R:%4d:%4d N:%4d:%4d Y:%4d:%4d U:%4d\n\r",
adc_result[ADC_GYRO_ROLL], adc_result[ADC_ACC_ROLL],
adc_result[ADC_GYRO_NICK], adc_result[ADC_ACC_NICK],
adc_result[ADC_GYRO_YAW], adc_result[ADC_ACC_YAW],
adc_result[ADC_VOLTAGE]);
*AT91C_ADC_CR = AT91C_ADC_START;
}
void adc_calibrate(uint32_t mode)
{
if (adc_cal_mode != ADC_CAL_NONE)
return;
if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) {
adc_cal_count = ADC_CAL_COUNT;
adc_cal_mode = mode;
adc_cal_data[0] = 0;
adc_cal_data[1] = 0;
adc_cal_data[2] = 0;
pitc_schedule_timer(&adc_cal_timer);
}
}
@ -183,8 +181,4 @@ void at91_adc_init(void)
aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
aic->AIC_IECR = (1<<AT91C_ID_ADC);
// TODO: trigger via rc
adc_calibrate(ADC_CAL_GYRO);
adc_calibrate(ADC_CAL_ACC);
}

23
src/at91_tc1.c
View File

@ -27,7 +27,9 @@
#define PULSE_MIN 0x0500
#define PULSE_MAX 0x0D00
#define PULSE_TIMEOUT 0x0F00
#define PULSE_CENTER 0x08C0
#define PULSE_SWITCH 0x01F0
/* moving average filters */
#define PULSE_FILTER_FAST (1<<2)
@ -132,11 +134,32 @@ uint32_t rcontrol_getvalues(struct rc_values *rc)
if (tmp < -VALUE_RANGE)
tmp = -VALUE_RANGE;
// TODO: stick mapping from eeprom
rc->chan[i] = tmp;
}
return cnt;
}
uint32_t rcontrol_getswitches(struct rc_values *rc)
{
if (valid < 5)
return 0;
uint32_t i;
uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
for (i = 0; i < cnt; i++) {
if (ch_data[i].width > (PULSE_CENTER + PULSE_SWITCH))
rc->chan[i] = VALUE_RANGE;
else if (ch_data[i].width < (PULSE_CENTER - PULSE_SWITCH))
rc->chan[i] = -VALUE_RANGE;
else
rc->chan[i] = 0;
}
return cnt;
}
void rcontrol_calibrate(uint32_t mode)
{
uint32_t i;