int main(int argc, char *argv[])
{
struct stat stat;
void *fdm;
if (argc != 3)
{
printf("Usage: minimad + mp3 file name");
return 1;
}
outFile = fopen(argv[2], "w+");
int fd;
fd = open(argv[1], O_RDWR);
if (fd < 0)
{
perror("open file failed:");
return 1;
}
if (fstat(fd, &stat) == -1 || stat.st_size == 0)
{
printf("fstat failed:\n");
return 2;
}
//printf("stat.st_size=%d\n",stat.st_size);
fdm = mmap(0, stat.st_size, PROT_READ, MAP_SHARED, fd, 0);
if (fdm == MAP_FAILED)
return 3;
if (set_pcm() != 0) //??pcm ??
{
printf("set_pcm fialed:\n");
return 1;
}
decode(fdm, stat.st_size);
if (munmap(fdm, stat.st_size) == -1)
return 4;
snd_pcm_drain(handle);
snd_pcm_close(handle);
fclose(outFile);//关闭文件指针
return 0;
}
int MusicPlayer::playMusic( char * musicName, char * diviceName, int sampleRate, int channels)
{
this->diviceName = diviceName;
struct stat stat;
void *fdm;
int fd;
fd=open(musicName,O_RDWR);
if(fd<0)
{
perror("open file failed:");
return 1;
}
if (fstat(fd, &stat) == -1 ||stat.st_size == 0)
{
printf("fstat failed:\n");
return 2;
}
//printf("stat.st_size=%d\n",stat.st_size);
fdm = mmap(0, stat.st_size, PROT_READ, MAP_SHARED, fd, 0);
if (fdm == MAP_FAILED)
return 3;
if(set_pcm(sampleRate, channels)!=0) //设置pcm 参数
{
printf("set_pcm fialed:\n");
return 4;
}
decode((const unsigned char*)fdm, stat.st_size);
if (munmap(fdm, stat.st_size) == -1)
return 5;
snd_pcm_drain(handle);
snd_pcm_close(handle);
return 0;
}
//GPS connector towards the back
//front-left, rear-right ccw (decrease heading)
//nose up, pitch down
//right wing down, roll down
//FIXME: 16-bit PCMs
void adjust_attitude(float pitch_corr, float roll_corr, float heading_corr)
{
int16_t fl_speed;
int16_t fr_speed;
int16_t bl_speed;
int16_t br_speed;
fl_speed = throttle - pitch_corr - roll_corr;
fr_speed = throttle - pitch_corr + roll_corr;
bl_speed = throttle + pitch_corr - roll_corr;
br_speed = throttle + pitch_corr + roll_corr;
if (fl_speed < 0)
fl_speed = 0;
else if (fl_speed > MAX_SPEED)
fl_speed = MAX_SPEED;
if (fr_speed < 0)
fr_speed = 0;
else if (fr_speed > MAX_SPEED)
fr_speed = MAX_SPEED;
if (bl_speed < 0)
bl_speed = 0;
else if (bl_speed > MAX_SPEED)
bl_speed = MAX_SPEED;
if (br_speed < 0)
br_speed = 0;
else if (br_speed > MAX_SPEED)
br_speed = MAX_SPEED;
pcms[FRONT_LEFT] = (uint8_t)fl_speed;
pcms[FRONT_RIGHT] = (uint8_t)fr_speed;
pcms[REAR_LEFT] = (uint8_t)bl_speed;
pcms[REAR_RIGHT] = (uint8_t)br_speed;
set_pcm(pcms);
}
int main(void)
{
//Set data direction, set mux pin to low and turn off LEDs.
DDRD |= _BV(SERIAL_MUX_PIN) | _BV(RED_LED_PIN) | _BV(BLUE_LED_PIN);
PORTD &= ~(_BV(SERIAL_MUX_PIN) | _BV(BLUE_LED_PIN) | _BV(RED_LED_PIN));
//Enable USART and connect to scanf/printf.
USART_init();
USART_to_stdio();
//Initialize sensors and FPGA.
mpu_init();
fpga_init(&PORTB, &DDRB, 2); //Needs to be after mpu_init().
mag_init();
milli_timer_init();
//This is pretty important.
sei();
//Get some initial readings
mpu_scale();
mag_read();
attitude();
mag_calculate(roll, pitch);
//Use those to initialize PID state
pid_init(&pitch_pid, P_CONST, I_CONST, D_CONST, pitch + PITCH_CORRECT);
pid_init(&roll_pid, P_CONST, I_CONST, D_CONST, roll + ROLL_CORRECT);
pid_init(&heading_pid, P_CONST, I_CONST, D_CONST, heading);
//We want to be level and retain heading (for now)
target_pitch = 0;
target_roll = 0;
target_heading = heading;
//Calibrate and arm the motors.
printf("Enter y to calibrate and arm motors.\n");
scanf("%c", &cmd);
if (cmd == 'y')
{
calibrate_motors();
printf("Motors calibrated and armed.\n");
}
else
{
printf("Calibration skipped.\n");
do
{
printf("Enter 'a' to arm motors.\n");
scanf("%c", &cmd);
} while (cmd != 'a');
arm_motors();
PORTD |= _BV(BLUE_LED_PIN);
printf("Motors armed.\n");
}
uint16_t loop_duration;
unsigned long last_loop_time = 0;
uint8_t count = 0;
while(1)
{
//unsigned pt = PID_PERIOD;
//unsigned pf = PID_FREQ;
/*
printf("Period: %u; frequency: %u\n", pt, pf);
//Wait for pid flag to be true.
while (!pid_flag);
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) //Reset it atomically as soon as we start.
{
pid_flag = 0;
}
*/
loop_duration = current_time - last_loop_time;
last_loop_time = current_time;
mpu_scale();
mag_read();
attitude();
//mag_calculate(roll, pitch);
pitch_correct = CORRECTION_GAIN * pid_update(&pitch_pid, target_pitch, pitch + PITCH_CORRECT);
roll_correct = CORRECTION_GAIN * pid_update(&roll_pid, target_roll, roll + ROLL_CORRECT);
//heading_correct = CORRECTION_GAIN * pid_update(&heading_pid, target_heading, heading);
heading_correct = 0;
//Safety: if throttle=0, stop motors.
if (throttle)
{
PORTD &= ~_BV(RED_LED_PIN);
//adjust_attitude(pitch_correct, roll_correct, heading_correct);
pcms[0] = throttle; pcms[1] = throttle; pcms[2] = throttle; pcms[3] = throttle;
set_pcm(pcms);
}
else
{
PORTD |= _BV(RED_LED_PIN);
pcms[0] = 0; pcms[1] = 0; pcms[2] = 0; pcms[3] = 0;
set_pcm(pcms);
//prevent integration windup when stationary
roll_pid.integrated_error = 0;
pitch_pid.integrated_error = 0;
}
++count;
#ifdef PRINTING
if (count % 100 == 0)
{
#ifdef PRINT_CSV
printf("%i,%i,%i,", accel_x, accel_y, accel_z);
printf("%f,%f,%f,", gyro_x, gyro_y, gyro_z);
printf("%i,%i,%i,", mag_x, mag_y, mag_z);
printf("%f,%f,%f,", pitch, roll, heading);
printf("%f,%f,%f,", pitch_correct, roll_correct, heading_correct);
printf("%u,%u,%u,%u,%u", (0x00FF & (unsigned)pcms[0]), (0x00FF & (unsigned)pcms[1]), (0x00FF & (unsigned)pcms[2]), (0x00FF & (unsigned)pcms[3]), (0x00FF & (unsigned)throttle));
printf("%u\n", loop_duration);
#endif
#ifdef PRETTY_PRINT
printf("ax: %i; ay: %i; az: %i; ", accel_x, accel_y, accel_z);
printf("gx: %f; gy: %f; gz: %f; ", gyro_x, gyro_y, gyro_z);
printf("mx: %i; my: %i; mz: %i; ", mag_x, mag_y, mag_z);
printf("p: %f; r: %f; h: %f; ", pitch + PITCH_CORRECT, roll + ROLL_CORRECT, heading_deg);
printf("PC: %f; RC: %f; HC: %f; ", pitch_correct, roll_correct, heading_correct);
printf("fl: %u; fr: %u; rl: %u; rr: %u; ", (0x00FF & (unsigned)pcms[FRONT_LEFT]), (0x00FF & (unsigned)pcms[FRONT_RIGHT]), (0x00FF & (unsigned)pcms[REAR_LEFT]), (0x00FF & (unsigned)pcms[REAR_RIGHT]));
printf("thr: %u; ", (0x00FF & (unsigned)throttle));
printf("t: %u\n", loop_duration);
#endif
}
#endif
//don't send anything other than numbers, dumbass
if (!(count % 64) && (rx_bytes >= 2))
{
if (scanf("%u", &num) != 0x00)
{
if (num > 800)
throttle = 800;
else
throttle = num;
#ifndef PRINTING
printf("Throttle: %u\n", throttle);
#endif
}
}
}
}
