void actuators_init(void) {
actuators_asctec.cmd = NONE;
actuators_asctec.cur_addr = FRONT;
actuators_asctec.new_addr = FRONT;
actuators_asctec.i2c_trans.status = I2CTransSuccess;
actuators_asctec.i2c_trans.type = I2CTransTx;
actuators_asctec.i2c_trans.slave_addr = 0x02;
#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
actuators_asctec.i2c_trans.len_w = 5;
#else
actuators_asctec.i2c_trans.len_w = 4;
#endif
actuators_asctec.nb_err = 0;
#if defined ACTUATORS_START_DELAY && ! defined SITL
actuators_delay_done = FALSE;
SysTimeTimerStart(actuators_delay_time);
#else
actuators_delay_done = TRUE;
actuators_delay_time = 0;
#endif
#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
supervision_init();
#endif
}
static inline void autopilot_main_init( void ) {
hw_init();
sys_time_init();
led_init();
supervision_init();
actuators_init(ACTUATOR_BANK_MOTORS);
#if BOMB_ENABLED
bomb_init_servo(RADIO_FMS, 0, 0);
#endif
rc_init();
#if HARDWARE_ENABLED_GPS
gps_init();
#else
comm_init(COMM_0);
comm_add_tx_callback(COMM_0, comm_autopilot_message_send);
comm_add_rx_callback(COMM_0, comm_autopilot_message_received);
#endif
comm_init(COMM_TELEMETRY);
comm_add_tx_callback(COMM_TELEMETRY, comm_autopilot_message_send);
comm_add_rx_callback(COMM_TELEMETRY, comm_autopilot_message_received);
analog_init();
analog_enable_channel(ANALOG_CHANNEL_BATTERY);
analog_enable_channel(ANALOG_CHANNEL_PRESSURE);
altimeter_init();
imu_init();
autopilot_init();
ahrs_init();
ins_init();
fms_init();
int_enable();
}
void actuators_init(void) {
supervision_init();
const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++) {
actuators_mkk.trans[i].type = I2CTransTx;
actuators_mkk.trans[i].len_w = 1;
actuators_mkk.trans[i].slave_addr = actuators_addr[i];
actuators_mkk.trans[i].status = I2CTransSuccess;
}
#if defined ACTUATORS_START_DELAY && ! defined SITL
actuators_delay_done = FALSE;
SysTimeTimerStart(actuators_delay_time);
#else
actuators_delay_done = TRUE;
actuators_delay_time = 0;
#endif
}
void actuators_init(void) {
supervision_init();
actuators_skiron.trans.type = I2CTransTx;
actuators_skiron.trans.len_w = ACTUATORS_SKIRON_NB;
actuators_skiron.trans.slave_addr = ACTUATORS_SKIRON_I2C_ADDR;
actuators_skiron.trans.status = I2CTransDone;
const uint8_t actuators_idx[ACTUATORS_SKIRON_NB] = ACTUATORS_SKIRON_IDX;
for (uint8_t i=0; i<ACTUATORS_SKIRON_NB; i++) {
actuators_skiron.actuators_idx[i] = actuators_idx[i];
}
#if defined ACTUATORS_START_DELAY && ! defined SITL
actuators_delay_done = FALSE;
SysTimeTimerStart(actuators_delay_time);
#else
actuators_delay_done = TRUE;
actuators_delay_time = 0;
#endif
}
void actuators_init(void)
{
supervision_init();
actuators_pwm_arch_init();
}
int main(int argc, char *argv[])
{
gp2x_init(1000, 16, 11025,16,1,60, 1);
gp2x_sound_volume(100,100);
screen16 = (unsigned short *)gp2x_video_RGB[0].screen;
int i,j;
char temp[255];
FILE *in = NULL;
if(argc <= 1) {
printf("\nnot enough arguments\n");
//return(TRUE);
} else {
// the hard-core UI, a command line:
for (i=0; (i < argc || argv[i] != NULL); i++) {
if(strcmp(argv[i], "--double") == 0) {
//screen_size = 1;
}
if(strcmp(argv[i], "--color white") == 0) {
supervision_set_colour_scheme(COLOUR_SCHEME_DEFAULT);
}
if(strcmp(argv[i], "--color amber") == 0) {
supervision_set_colour_scheme(COLOUR_SCHEME_AMBER);
}
if(strcmp(argv[i], "--color green") == 0) {
supervision_set_colour_scheme(COLOUR_SCHEME_GREEN);
}
if(strcmp(argv[i], "--color blue") == 0) {
supervision_set_colour_scheme(COLOUR_SCHEME_BLUE);
}
}
romname = strdup(argv[1]);
in = fopen(romname, "r");
if(in == NULL) {
printf("The file %s doesn't exist.\n",romname);
exit(0);
}
fflush(in);
fclose(in);
}
supervision_init(); //Init the emulator
getRunDir();
if(romname!=NULL){
loadROM(romname);
supervision_load((u8*)buffer, (uint32)buffer_size);
} else {
handleFileMenu(); // File menu
}
emu_ReadConfig();
gp2x_sound_volume(255,255);
gp2x_sound_pause(0);
while(1)
{
CheckKeys();
while(!paused)
{
CheckKeys(); //key control
controls_update();
switch(currentConfig.videoMode){
case 0:
supervision_exec((int16*)screenbuffer,1);
for(j=0; j < 160; j++)
gp2x_memcpy(screen16+(80+(j+40)*320),screenbuffer+(j * 160),160*2);
break;
case 1:
supervision_exec2((int16*)screen16,1);
break;
case 2:
supervision_exec3((int16*)screen16,1);
break;
default: break;
}
/*gp2x_video_waitvsync();
sprintf(temp,"FPS: %3d", FPS);
gp2x_printf(NULL,0,0,temp);
++svFrm;*/
gp2x_video_RGB_flip(0);
controls_reset();
}
}
supervision_done(); //shutsdown the system
gp2x_deinit();
}