int main() {
rnd_init();
rgb_init();
usitwi_init();
sei();
while (1) {
}
}
void clear_table() {
int i, j;
for (i = 0; i < ROWS_E; i++) {
for (j = 0; j < COLS; j++) {
rgb_init(&table[i][j], 0, 0, 0);
}
}
}
// FILTERS
void strobe(struct visual_params *arg) {
int i, j;
if (arg->iterations % 2 == 1) {
for (i = 0; i < ROWS_E; i++) {
for (j = 0; j < COLS; j++) {
rgb_init(&(table[i][j]), 0, 0, 0);
}
}
}
}
void peaker(struct visual_params *arg) {
int i, j;
for (i = 0; i < ROWS_E - 1; i++) {
for (j = 0; j < COLS; j++) {
if (rgb_nz(&table[i + 1][j])) {
rgb_init(&(table[i][j]), 0, 0, 0);
}
}
}
}
void some_off(struct visual_params *arg) {
int i, j, color_index;
for (i = 0; i < ROWS_E; i++) {
for (j = 0; j < COLS; j++) {
int color_index = rand() % 7;
if (prob(arg->energy)) {
arg->color(&table[i][j], color_index);
} else {
rgb_init(&table[i][j], 0, 0, 0);
}
}
}
}
void blue_to_red_cgen(struct rgb *out, int index) {
if (index == 0) {
rgb_init(out, 0x00, 0x00, 0xc0);
} else if (index == 1) {
rgb_init(out, 0x40, 0x00, 0xc0);
} else if (index == 2) {
rgb_init(out, 0x80, 0x00, 0xc0);
} else if (index == 3) {
rgb_init(out, 0xc0, 0x00, 0xc0);
} else if (index == 4) {
rgb_init(out, 0xc0, 0x00, 0x80);
} else if (index == 5) {
rgb_init(out, 0xc0, 0x00, 0x40);
} else {
rgb_init(out, 0xc0, 0x00, 0x00);
}
}
void translate(int x, int y) {
struct rgb buffer[ROWS_E][COLS];
int i, j;
for (i = 0; i < ROWS_E; i++) {
for (j = 0; j < COLS; j++) {
if (i - y >= 0 && i - y < ROWS_E && j - x >= 0 && j - x < COLS) {
buffer[i][j] = table[i - y][j - x];
} else {
rgb_init(&buffer[i][j], 0, 0, 0);
}
}
}
for (i = 0; i < ROWS_E; i++) {
for (j = 0; j < COLS; j++) {
table[i][j] = buffer[i][j];
}
}
}
void draw_frame(struct visual_params *arg, char frame[4][4], int p) {
int s, i, j, col;
struct rgb *out;
clear_table();
for (s = 0; s < SQUARES; s++) {
if (prob(p)) {
for (i = 0; i < ROWS_P; i++) {
for (j = 0; j < COLS_P; j++) {
col = j + COLS_P*s;
out = &table[i + get_base_height(col)][col];
if (frame[i][j]) {
arg->color(out, frame[i][j]);
} else {
rgb_init(out, 0, 0, 0);
}
}
}
}
}
}
static void
init_member_suboptions(struct sol_flow_node_named_options_member *m)
{
switch (m->type) {
case SOL_FLOW_NODE_OPTIONS_MEMBER_DRANGE_SPEC:
drange_spec_init(&m->drange_spec);
break;
case SOL_FLOW_NODE_OPTIONS_MEMBER_DIRECTION_VECTOR:
direction_vector_init(&m->direction_vector);
break;
case SOL_FLOW_NODE_OPTIONS_MEMBER_IRANGE_SPEC:
irange_spec_init(&m->irange_spec);
break;
case SOL_FLOW_NODE_OPTIONS_MEMBER_RGB:
rgb_init(&m->rgb);
break;
default:
break;
}
}
void clear_col_x(struct visual_params *arg, int x) {
int i;
for (i = 0; i < ROWS_E; i++) {
rgb_init(&table[i][x], 0, 0, 0);
}
}
void purple_cgen(struct rgb *out, int index) {
rgb_init(out, 0xc0, 0x00, 0xc0);
}
void orange_cgen(struct rgb *out, int index) {
rgb_init(out, 0xc0, 0x40, 0x00);
}
void yellow_cgen(struct rgb *out, int index) {
rgb_init(out, 0xc0, 0xc0, 0x00);
}
// solids
void red_cgen(struct rgb *out, int index) {
rgb_init(out, 0xc0, 0x00, 0x00);
}
int export_img(struct play_s *play)
{
/*
Export img uses following pipeline:
file -(uncompressed_buffer)-> reads data from stream file
unpack -(uncompressed_buffer)-> decompresses lzo/quicklz packets
rgb -(rgb)-> does conversion to BGR
scale -(scale)-> does rescaling
color -(color)-> applies color correction
img writes separate image files for each frame
*/
ps_bufferattr_t attr;
ps_buffer_t uncompressed_buffer, compressed_buffer,
rgb_buffer, color_buffer, scale_buffer;
img_t img;
color_t color;
scale_t scale;
unpack_t unpack;
rgb_t rgb;
int ret = 0;
if ((ret = ps_bufferattr_init(&attr)))
goto err;
/* buffers */
if ((ret = ps_bufferattr_setsize(&attr, play->compressed_size)))
goto err;
if ((ret = ps_buffer_init(&compressed_buffer, &attr)))
goto err;
if ((ret = ps_bufferattr_setsize(&attr, play->uncompressed_size)))
goto err;
if ((ret = ps_buffer_init(&uncompressed_buffer, &attr)))
goto err;
if ((ret = ps_buffer_init(&color_buffer, &attr)))
goto err;
if ((ret = ps_buffer_init(&rgb_buffer, &attr)))
goto err;
if ((ret = ps_buffer_init(&scale_buffer, &attr)))
goto err;
if ((ret = ps_bufferattr_destroy(&attr)))
goto err;
/* filters */
if ((ret = unpack_init(&unpack, &play->glc)))
goto err;
if ((ret = rgb_init(&rgb, &play->glc)))
goto err;
if ((ret = scale_init(&scale, &play->glc)))
goto err;
if (play->scale_width && play->scale_height)
scale_set_size(scale, play->scale_width, play->scale_height);
else
scale_set_scale(scale, play->scale_factor);
if ((ret = color_init(&color, &play->glc)))
goto err;
if (play->override_color_correction)
color_override(color, play->brightness, play->contrast,
play->red_gamma, play->green_gamma, play->blue_gamma);
if ((ret = img_init(&img, &play->glc)))
goto err;
img_set_filename(img, play->export_filename_format);
img_set_stream_id(img, play->export_video_id);
img_set_format(img, play->img_format);
img_set_fps(img, play->fps);
/* pipeline... */
if ((ret = unpack_process_start(unpack, &compressed_buffer, &uncompressed_buffer)))
goto err;
if ((ret = rgb_process_start(rgb, &uncompressed_buffer, &rgb_buffer)))
goto err;
if ((ret = scale_process_start(scale, &rgb_buffer, &scale_buffer)))
goto err;
if ((ret = color_process_start(color, &scale_buffer, &color_buffer)))
goto err;
if ((ret = img_process_start(img, &color_buffer)))
goto err;
/* ok, read the file */
if ((ret = file_read(play->file, &compressed_buffer)))
goto err;
/* wait 'till its done and clean up the mess... */
if ((ret = img_process_wait(img)))
goto err;
if ((ret = color_process_wait(color)))
goto err;
if ((ret = scale_process_wait(scale)))
goto err;
if ((ret = rgb_process_wait(rgb)))
goto err;
if ((ret = unpack_process_wait(unpack)))
goto err;
unpack_destroy(unpack);
rgb_destroy(rgb);
scale_destroy(scale);
color_destroy(color);
img_destroy(img);
ps_buffer_destroy(&compressed_buffer);
ps_buffer_destroy(&uncompressed_buffer);
ps_buffer_destroy(&color_buffer);
ps_buffer_destroy(&scale_buffer);
ps_buffer_destroy(&rgb_buffer);
return 0;
err:
fprintf(stderr, "exporting images failed: %s (%d)\n", strerror(ret), ret);
return ret;
}
int play_stream(struct play_s *play)
{
/*
Playback uses following pipeline:
file -(uncompressed)-> reads data from stream file
unpack -(uncompressed)-> decompresses lzo/quicklz packets
rgb -(rgb)-> does conversion to BGR
scale -(scale)-> does rescaling
color -(color)-> applies color correction
demux -(...)-> gl_play, alsa_play
Each filter, except demux and file, has glc_threads_hint(glc) worker
threads. Packet order in stream is preserved. Demux creates
separate buffer and _play handler for each video/audio stream.
*/
ps_bufferattr_t attr;
ps_buffer_t uncompressed_buffer, compressed_buffer,
rgb_buffer, color_buffer, scale_buffer;
demux_t demux;
color_t color;
scale_t scale;
unpack_t unpack;
rgb_t rgb;
int ret = 0;
if ((ret = ps_bufferattr_init(&attr)))
goto err;
/*
'compressed_buffer' buffer holds raw data from file and
has its own size.
*/
if ((ret = ps_bufferattr_setsize(&attr, play->compressed_size)))
goto err;
if ((ret = ps_buffer_init(&compressed_buffer, &attr)))
goto err;
/* rest use 'uncompressed_buffer' size */
if ((ret = ps_bufferattr_setsize(&attr, play->uncompressed_size)))
goto err;
if ((ret = ps_buffer_init(&uncompressed_buffer, &attr)))
goto err;
if ((ret = ps_buffer_init(&color_buffer, &attr)))
goto err;
if ((ret = ps_buffer_init(&rgb_buffer, &attr)))
goto err;
if ((ret = ps_buffer_init(&scale_buffer, &attr)))
goto err;
/* no longer necessary */
if ((ret = ps_bufferattr_destroy(&attr)))
goto err;
/* init filters */
if ((ret = unpack_init(&unpack, &play->glc)))
goto err;
if ((ret = rgb_init(&rgb, &play->glc)))
goto err;
if ((ret = scale_init(&scale, &play->glc)))
goto err;
if (play->scale_width && play->scale_height)
scale_set_size(scale, play->scale_width, play->scale_height);
else
scale_set_scale(scale, play->scale_factor);
if ((ret = color_init(&color, &play->glc)))
goto err;
if (play->override_color_correction)
color_override(color, play->brightness, play->contrast,
play->red_gamma, play->green_gamma, play->blue_gamma);
if ((ret = demux_init(&demux, &play->glc)))
goto err;
demux_set_video_buffer_size(demux, play->uncompressed_size);
demux_set_audio_buffer_size(demux, play->uncompressed_size / 10);
demux_set_alsa_playback_device(demux, play->alsa_playback_device);
/* construct a pipeline for playback */
if ((ret = unpack_process_start(unpack, &compressed_buffer, &uncompressed_buffer)))
goto err;
if ((ret = rgb_process_start(rgb, &uncompressed_buffer, &rgb_buffer)))
goto err;
if ((ret = scale_process_start(scale, &rgb_buffer, &scale_buffer)))
goto err;
if ((ret = color_process_start(color, &scale_buffer, &color_buffer)))
goto err;
if ((ret = demux_process_start(demux, &color_buffer)))
goto err;
/* the pipeline is ready - lets give it some data */
if ((ret = file_read(play->file, &compressed_buffer)))
goto err;
/* we've done our part - just wait for the threads */
if ((ret = demux_process_wait(demux)))
goto err; /* wait for demux, since when it quits, others should also */
if ((ret = color_process_wait(color)))
goto err;
if ((ret = scale_process_wait(scale)))
goto err;
if ((ret = rgb_process_wait(rgb)))
goto err;
if ((ret = unpack_process_wait(unpack)))
goto err;
/* stream processed - clean up time */
unpack_destroy(unpack);
rgb_destroy(rgb);
scale_destroy(scale);
color_destroy(color);
demux_destroy(demux);
ps_buffer_destroy(&compressed_buffer);
ps_buffer_destroy(&uncompressed_buffer);
ps_buffer_destroy(&color_buffer);
ps_buffer_destroy(&scale_buffer);
ps_buffer_destroy(&rgb_buffer);
return 0;
err:
if (!ret) {
fprintf(stderr, "playing stream failed: initializing filters failed\n");
return EAGAIN;
} else {
fprintf(stderr, "playing stream failed: %s (%d)\n", strerror(ret), ret);
return ret;
}
}
void pink_cgen(struct rgb *out, int index) {
rgb_init(out, 0xc0, 0x00, 0x40);
}
void cyan_cgen(struct rgb *out, int index) {
rgb_init(out, 0x00, 0xc0, 0xc0);
}
void gr_mode(void) {
int errorcode;
if (!in_graphics_mode) {
msm_hidecursor();
x_coord = x_margin;
y_coord = y_margin;
errorcode = fg_init();
if (have_been_in_graphics_mode) {
in_graphics_mode = TRUE;
if (turtle_shown && !refresh_p) draw_turtle();
redraw_graphics();
}
else {
if (errorcode == 0)
err_logo(BAD_GRAPH_INIT, NIL);
else {
in_graphics_mode = have_been_in_graphics_mode = TRUE;
if (can_do_color = (fg.nsimulcolor > 16 /* != fg.ncolormap */ )) {
rgb_init();
dull = fg.nsimulcolor-1;
bright = 7;
} else {
turtle_color = FG_HIGHLIGHT;
dull = FG_WHITE;
bright = FG_HIGHLIGHT;
}
bg_color = FG_BLACK;
ztc_set_penc(7);
if (ztc_textcolor == FG_WHITE) ztc_textcolor = dull;
back_ground = 0;
ztc_box[FG_X1] = ztc_textbox[FG_X1]
= text_scroll_box[FG_X1]
= text_last_line_box[FG_X1]
= clear_box[FG_X1]
= fg.displaybox[FG_X1];
ztc_textbox[FG_Y1] = fg.displaybox[FG_Y1];
ztc_box[FG_X2] = ztc_textbox[FG_X2]
= text_scroll_box[FG_X2]
= text_last_line_box[FG_X2]
= clear_box[FG_X2]
= MaxX = fg.displaybox[FG_X2];
ztc_box[FG_Y2] = MaxY
= clear_box[FG_Y2]
= fg.displaybox[FG_Y2];
y_scale = (double)fg.pixelx/(double)fg.pixely;
{
FILE *fp = fopen("scrunch.dat","r");
if (fp != NULL) {
scrunching = TRUE;
if (filelength(fileno(fp)) > 0) {
fread(&x_scale, sizeof(FLONUM), 1, fp);
fread(&y_scale, sizeof(FLONUM), 1, fp);
}
fclose(fp);
}
}
if (MaxY == 479)
texth = 16;
else
texth = (MaxY+1)/25;
ztc_box[FG_Y1] = 4*texth+1;
clear_box[FG_Y1] = 4*texth;
ibm_screen_bottom = MaxY - (ztc_box[FG_Y1]);
ztc_textbox[FG_Y2] = 4*texth-1;
text_scroll_box[FG_Y2] = 3*texth-1;
text_scroll_box[FG_Y1] = 0;
text_last_line_box[FG_Y2] = texth-1;
lclearscreen(NIL);
lcleartext(NIL);
in_splitscreen = TRUE;
}
}
msm_showcursor();
}
}
int main(void)
{
clk_init();
gpio_init();
#ifdef SERIAL
serial_init();
#endif
i2c_init();
spi_init();
pwm_init();
pwm_set(MOTOR_FL, 0); // FL
pwm_set(MOTOR_FR, 0);
pwm_set(MOTOR_BL, 0); // BL
pwm_set(MOTOR_BR, 0); // FR
time_init();
if (RCC_GetCK_SYSSource() == 8)
{
}
else
{
failloop(5);
}
sixaxis_init();
if (sixaxis_check())
{
#ifdef SERIAL
printf(" MPU found \n");
#endif
}
else
{
#ifdef SERIAL
printf("ERROR: MPU NOT FOUND \n");
#endif
failloop(4);
}
adc_init();
rx_init();
int count = 0;
vbattfilt = 0.0;
while (count < 64)
{
vbattfilt += adc_read(1);
count++;
}
// for randomising MAC adddress of ble app - this will make the int = raw float value
random_seed = *(int *)&vbattfilt ;
random_seed = random_seed&0xff;
vbattfilt = vbattfilt / 64;
#ifdef SERIAL
printf("Vbatt %2.2f \n", vbattfilt);
#ifdef NOMOTORS
printf("NO MOTORS\n");
#warning "NO MOTORS"
#endif
#endif
#ifdef STOP_LOWBATTERY
// infinite loop
if (vbattfilt < STOP_LOWBATTERY_TRESH)
failloop(2);
#endif
// loads acc calibration and gyro dafaults
loadcal();
gyro_cal();
rgb_init();
imu_init();
extern unsigned int liberror;
if (liberror)
{
#ifdef SERIAL
printf("ERROR: I2C \n");
#endif
failloop(7);
}
lastlooptime = gettime();
extern int rxmode;
extern int failsafe;
float thrfilt;
//
//
// MAIN LOOP
//
//
checkrx();
while (1)
{
// gettime() needs to be called at least once per second
maintime = gettime();
looptime = ((uint32_t) (maintime - lastlooptime));
if (looptime <= 0)
looptime = 1;
looptime = looptime * 1e-6f;
if (looptime > 0.02f) // max loop 20ms
{
failloop(3);
//endless loop
}
lastlooptime = maintime;
if (liberror > 20)
{
failloop(8);
// endless loop
}
sixaxis_read();
control();
// battery low logic
float hyst;
float battadc = adc_read(1);
vbatt = battadc;
// average of all 4 motor thrusts
// should be proportional with battery current
extern float thrsum; // from control.c
// filter motorpwm so it has the same delay as the filtered voltage
// ( or they can use a single filter)
lpf ( &thrfilt , thrsum , 0.9968f); // 0.5 sec at 1.6ms loop time
lpf ( &vbattfilt , battadc , 0.9968f);
#ifdef AUTO_VDROP_FACTOR
static float lastout[12];
static float lastin[12];
static float vcomp[12];
static float score[12];
static int current_index = 0;
int minindex = 0;
float min = score[0];
{
int i = current_index;
vcomp[i] = vbattfilt + (float) i *0.1f * thrfilt;
if ( lastin[i] < 0.1f ) lastin[i] = vcomp[i];
float temp;
// y(n) = x(n) - x(n-1) + R * y(n-1)
// out = in - lastin + coeff*lastout
// hpf
temp = vcomp[i] - lastin[i] + FILTERCALC( 1000*12 , 1000e3) *lastout[i];
lastin[i] = vcomp[i];
lastout[i] = temp;
lpf ( &score[i] , fabsf(temp) , FILTERCALC( 1000*12 , 10e6 ) );
}
current_index++;
if ( current_index >= 12 ) current_index = 0;
for ( int i = 0 ; i < 12; i++ )
{
if ( score[i] < min )
{
min = score[i];
minindex = i;
}
}
#undef VDROP_FACTOR
#define VDROP_FACTOR minindex * 0.1f
#endif
if ( lowbatt ) hyst = HYST;
else hyst = 0.0f;
vbatt_comp = vbattfilt + (float) VDROP_FACTOR * thrfilt;
if ( vbatt_comp <(float) VBATTLOW + hyst ) lowbatt = 1;
else lowbatt = 0;
// led flash logic
if (rxmode != RX_MODE_BIND)
{
// non bind
if (failsafe)
{
if (lowbatt)
ledflash(500000, 8);
else
ledflash(500000, 15);
}
else
{
if (lowbatt)
ledflash(500000, 8);
else
{
if (ledcommand)
{
if (!ledcommandtime)
ledcommandtime = gettime();
if (gettime() - ledcommandtime > 500000)
{
ledcommand = 0;
ledcommandtime = 0;
}
ledflash(100000, 8);
}
else
{
if ( aux[LEDS_ON] )
ledon( 255);
else
ledoff( 255);
}
}
}
}
else
{ // bind mode
ledflash(100000 + 500000 * (lowbatt), 12);
}
// rgb strip logic
#if (RGB_LED_NUMBER > 0)
extern void rgb_led_lvc( void);
rgb_led_lvc( );
#endif
#ifdef BUZZER_ENABLE
buzzer();
#endif
#ifdef FPV_ON
static int fpv_init = 0;
if ( rxmode == RX_MODE_NORMAL && ! fpv_init ) {
fpv_init = gpio_init_fpv();
}
if ( fpv_init ) {
if ( failsafe ) {
GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, Bit_RESET );
} else {
GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, aux[ FPV_ON ] ? Bit_SET : Bit_RESET );
}
}
#endif
checkrx();
// loop time 1ms
while ((gettime() - maintime) < (1000 - 22) )
delay(10);
} // end loop
}
