int main()
{
int start_t=0, end_t=0;
printf("****************************************\n");
printf("remote_control_run : start\n");
printf("period : 3600 seconds!\n");
printf("****************************************\n");
while(1)
{
sleep(1200);
printf("remote control : start\n");
start_t = time(NULL);
if(remote_control()==0)
printf("remote_control : faliure\n");
else
printf("remote_control : success\n");
end_t = time(NULL);
sleep(2400-(end_t-start_t));
}
return 0;
}
int main( int argc, char** argv )
{
/*
id = 7
x_orig = 1
y_orig = 2
x_next = 3
y_next = 4
*/
// CAUTION: id = 7
// char input[] = {16, 7, 32, 1, 32, 2, 32, 3, 32, 4, 17 };
/*
id = 77
x_orig = 11
y_orig = 22
x_next = 33
y_next = 44
*/
// char input[] = {16, 7, 7, 32, 1, 1, 32, 2, 2, 32, 3, 3, 32, 4, 4, 17 };
/*
id = 77
x_orig = 11
y_orig = -22
x_next = 33
y_next = -44
*/
char input[] = {16, 7, 7, 32, 1, 1, 32, 33, 2, 2, 32, 3, 3, 32, 33, 4, 4, 17 };
// FAIL - negative in between
// char input[] = {16, 7, 7, 32, 1, 1, 32, 2, 2, 32, 3, 3, 32, 4, 33, 4, 17, 1 };
// FAIL - number too short
// char input[] = {16, 7, 7, 32, 1, 1, 32, 2, 2, 32, 3, 17 };
// FAIL - double +
// char input[] = {16, 7, 7, 32, 1, 1, 32, 33, 2, 2, 32, 32, 33, 4, 4, 17 };
// LIMIT = 100
// 99
// char input[] = {16, 7, 7, 32, 9,9, 32, 2, 5, 32, 2, 5, 32, 2, 5, 17 };
// 100 - ???
// char input[] = {16, 7, 7, 32, 1,0,0, 32, 2, 5, 32, 2, 5, 32, 2, 5, 17 };
// 101 - FAIL
// char input[] = {16, 7, 7, 32, 1,0,1, 32, 2, 5, 32, 2, 5, 32, 2, 5, 17 };
// 110 - FAIL
// char input[] = {16, 7, 7, 32, 1,1,0, 32, 2, 5, 32, 2, 5, 32, 2, 5, 17 };
int idx=0;
char *rc5_command = input;
// reset
for( idx=0; idx<5; ++idx ){
command_input[idx] = EMPTY;
command_sequence[idx] = EMPTY;
}
//*/
// remote control
for( idx=0; idx<sizeof(input); idx++, rc5_command++ ){
remote_control( rc5_command );
}
printf("\n\n");
// evaluation
printf("result:\n");
for( idx=0; idx<sizeof(command_sequence); ++idx){
printf("%i\n", command_sequence[idx] );
}
printf("READY.\n");
exit( EXIT_SUCCESS );
}
/* Read a character (until one is available) and store it in buffer */
static void vtty_read_and_store(vtty_t *vtty,int *fd_slot)
{
int c;
/* wait until we get a character input */
c = vtty_read(vtty,fd_slot);
/* if read error, do nothing */
if (c < 0) return;
/* If something was read, make sure the handler is informed */
vtty->input_pending = TRUE;
if (!vtty->terminal_support) {
vtty_store(vtty,c);
return;
}
switch(vtty->input_state) {
case VTTY_INPUT_TEXT :
switch(c) {
case 0x1b:
vtty->input_state = VTTY_INPUT_VT1;
return;
/* Ctrl + ']' (0x1d, 29), or Alt-Gr + '*' (0xb3, 179) */
case 0x1d:
case 0xb3:
if (ctrl_code_ok == 1) {
vtty->input_state = VTTY_INPUT_REMOTE;
} else {
vtty_store(vtty,c);
}
return;
case IAC :
vtty->input_state = VTTY_INPUT_TELNET;
return;
case 0: /* NULL - Must be ignored - generated by Linux telnet */
case 10: /* LF (Line Feed) - Must be ignored on Windows platform */
return;
default:
/* Store a standard character */
vtty_store(vtty,c);
return;
}
case VTTY_INPUT_VT1 :
switch(c) {
case 0x5b:
vtty->input_state = VTTY_INPUT_VT2;
return;
default:
vtty_store(vtty,0x1b);
vtty_store(vtty,c);
}
vtty->input_state = VTTY_INPUT_TEXT;
return;
case VTTY_INPUT_VT2 :
switch(c) {
case 0x41: /* Up Arrow */
vtty_store(vtty,16);
break;
case 0x42: /* Down Arrow */
vtty_store(vtty,14);
break;
case 0x43: /* Right Arrow */
vtty_store(vtty,6);
break;
case 0x44: /* Left Arrow */
vtty_store(vtty,2);
break;
default:
vtty_store(vtty,0x5b);
vtty_store(vtty,0x1b);
vtty_store(vtty,c);
break;
}
vtty->input_state = VTTY_INPUT_TEXT;
return;
case VTTY_INPUT_REMOTE :
remote_control(vtty, c);
vtty->input_state = VTTY_INPUT_TEXT;
return;
case VTTY_INPUT_TELNET :
vtty->telnet_cmd = c;
switch(c) {
case WILL:
case WONT:
case DO:
case DONT:
vtty->input_state = VTTY_INPUT_TELNET_IYOU;
return;
case SB :
vtty->telnet_cmd = c;
vtty->input_state = VTTY_INPUT_TELNET_SB1;
return;
case SE:
break;
case IAC :
vtty_store(vtty, IAC);
break;
}
vtty->input_state = VTTY_INPUT_TEXT;
return;
case VTTY_INPUT_TELNET_IYOU :
vtty->telnet_opt = c;
/* if telnet client can support ttype, ask it to send ttype string */
if ((vtty->telnet_cmd == WILL) &&
(vtty->telnet_opt == TELOPT_TTYPE))
{
vtty_put_char(vtty, IAC);
vtty_put_char(vtty, SB);
vtty_put_char(vtty, TELOPT_TTYPE);
vtty_put_char(vtty, TELQUAL_SEND);
vtty_put_char(vtty, IAC);
vtty_put_char(vtty, SE);
}
vtty->input_state = VTTY_INPUT_TEXT;
return;
case VTTY_INPUT_TELNET_SB1 :
vtty->telnet_opt = c;
vtty->input_state = VTTY_INPUT_TELNET_SB2;
return;
case VTTY_INPUT_TELNET_SB2 :
vtty->telnet_qual = c;
if ((vtty->telnet_opt == TELOPT_TTYPE) &&
(vtty->telnet_qual == TELQUAL_IS))
vtty->input_state = VTTY_INPUT_TELNET_SB_TTYPE;
else
vtty->input_state = VTTY_INPUT_TELNET_NEXT;
return;
case VTTY_INPUT_TELNET_SB_TTYPE :
/* parse ttype string: first char is sufficient */
/* if client is xterm or vt, set the title bar */
if ((c == 'x') || (c == 'X') || (c == 'v') || (c == 'V')) {
fd_printf(*fd_slot,0,"\033]0;%s\07", vtty->vm->name);
}
vtty->input_state = VTTY_INPUT_TELNET_NEXT;
return;
case VTTY_INPUT_TELNET_NEXT :
/* ignore all chars until next IAC */
if (c == IAC)
vtty->input_state = VTTY_INPUT_TELNET;
return;
}
}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_ground_car(void)
{
last_mainloop_idle = sys_time_clock_get_time_usec();
debug_message_buffer("Starting main loop");
while (1)
{
// Time Measurement
uint64_t loop_start_time = sys_time_clock_get_time_usec();
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL 200 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(5000, COUNTER2, loop_start_time))
{
// Kalman Attitude filter, used on all systems
gyro_read();
sensors_read_acc();
// Read out magnetometer at its default 50 Hz rate
static uint8_t mag_count = 0;
if (mag_count == 3)
{
sensors_read_mag();
attitude_observer_correct_magnet(global_data.magnet_corrected);
mag_count = 0;
}
else
{
mag_count++;
}
// Correction step of observer filter
attitude_observer_correct_accel(global_data.accel_raw);
// Write in roll and pitch
static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
attitude_observer_get_angles(&att);
global_data.attitude.x = att.x;
global_data.attitude.y = att.y;
if (global_data.param[PARAM_ATT_KAL_IYAW])
{
global_data.attitude.z += 0.005 * global_data.gyros_si.z;
}
else
{
global_data.attitude.z = att.z;
}
// Prediction step of observer
attitude_observer_predict(global_data.gyros_si);
// TODO READ OUT MOUSE SENSOR
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 50 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(20000, COUNTER3, loop_start_time))
{
// Read Analog-to-Digital converter
adc_read();
// Read remote control
remote_control();
// Send the raw sensor/ADC values
communication_send_raw_data(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// UNCRITICAL SLOW 5 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(200000, COUNTER8, loop_start_time))
{
// Send buffered data such as debug text messages
communication_queued_send();
// Empty one message out of the buffer
debug_message_send_one();
// Toggle status led
//led_toggle(LED_YELLOW);
led_toggle(LED_RED); // just for green LED on alpha at the moment
// Toggle active mode led
if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode
== MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO)
{
led_on(LED_GREEN);
}
else
{
led_off(LED_GREEN);
}
handle_eeprom_write_request();
handle_reset_request();
communication_send_remote_control();
// Pressure sensor driver works, but not tested regarding stability
sensors_pressure_bmp085_read_out();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER7, loop_start_time))
{
led_toggle(LED_YELLOW);
communication_send_attitude_position(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 200 Hz functions //
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER5, loop_start_time))
{
if (global_data.state.status == MAV_STATE_STANDBY)
{
//Check if parameters should be written or read
param_handler();
}
}
///////////////////////////////////////////////////////////////////////////
else
{
// All Tasks are fine and we have no starvation
last_mainloop_idle = loop_start_time;
}
// Read out comm at max rate - takes only a few microseconds in worst case
communication_receive();
// MCU load measurement
uint64_t loop_stop_time = sys_time_clock_get_time_usec();
global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
if (loop_start_time - last_mainloop_idle >= 5000)
{
debug_message_buffer(
"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
}
if (global_data.cpu_usage > 800)
{
// CPU load higher than 80%
debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
}
} // End while(1)
}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_fixed_wing(void)
{
last_mainloop_idle = sys_time_clock_get_time_usec();
debug_message_buffer("Starting main loop");
while (1)
{
// Time Measurement
uint64_t loop_start_time = sys_time_clock_get_time_usec();
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// Camera Shutter
///////////////////////////////////////////////////////////////////////////
// Set camera shutter with 2.5ms resolution
if (us_run_every(2500, COUNTER1, loop_start_time))
{
camera_shutter_handling(loop_start_time);
}
if (global_data.state.mav_mode == MAV_MODE_RC_TRAINING)
{
///////////////////////////////////////////////////////////////////////////
/// RC INTERFACE HACK AT 50 Hz
///////////////////////////////////////////////////////////////////////////
if (us_run_every(20000, COUNTER8, loop_start_time))
{
// Write start byte
uart1_transmit(0xFF);
// Write channels 1-6
for (int i = 1; i < 7; i++)
{
uart1_transmit((radio_control_get_channel(1)+1)*127);
}
}
led_toggle(LED_GREEN);
led_toggle(LED_RED);
// Do not execute any of the functions below
continue;
}
///////////////////////////////////////////////////////////////////////////
/// CRITICAL 200 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(5000, COUNTER2, loop_start_time))
{
// Kalman Attitude filter, used on all systems
gyro_read();
sensors_read_acc();
// Read out magnetometer at its default 50 Hz rate
static uint8_t mag_count = 0;
if (mag_count == 3)
{
sensors_read_mag();
attitude_observer_correct_magnet(global_data.magnet_corrected);
mag_count = 0;
}
else
{
mag_count++;
}
// Correction step of observer filter
attitude_observer_correct_accel(global_data.accel_raw);
// Write in roll and pitch
static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
attitude_observer_get_angles(&att);
global_data.attitude.x = att.x;
global_data.attitude.y = att.y;
if (global_data.param[PARAM_ATT_KAL_IYAW])
{
global_data.attitude.z += 0.005 * global_data.gyros_si.z;
}
else
{
global_data.attitude.z = att.z;
}
// Prediction step of observer
attitude_observer_predict(global_data.gyros_si);
control_fixed_wing_attitude();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 50 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(20000, COUNTER3, loop_start_time))
{
// Read Analog-to-Digital converter
adc_read();
// Read remote control
remote_control();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 100 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(10000, COUNTER6, loop_start_time))
{
// Send the raw sensor/ADC values
communication_send_raw_data(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// UNCRITICAL SLOW 5 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(200000, COUNTER8, loop_start_time))
{
// The onboard controllers go into failsafe mode once
// position data is missing
handle_controller_timeouts(loop_start_time);
// Send buffered data such as debug text messages
communication_queued_send();
// Empty one message out of the buffer
debug_message_send_one();
// Toggle status led
//led_toggle(LED_YELLOW);
led_toggle(LED_RED); // just for green LED on alpha at the moment
// Toggle active mode led
if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode
== MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO)
{
led_on(LED_GREEN);
}
else
{
led_off(LED_GREEN);
}
handle_eeprom_write_request();
handle_reset_request();
communication_send_controller_feedback();
communication_send_remote_control();
// Pressure sensor driver works, but not tested regarding stability
sensors_pressure_bmp085_read_out();
if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
{
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
91, global_data.yaw_pos_setpoint);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 1 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(1000000, COUNTER9, loop_start_time))
{
// Send system state, mode, battery voltage, etc.
send_system_state();
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//Send GPS information
float_vect3 gps;
gps.x = gps_utm_north / 100.0f;//m
gps.y = gps_utm_east / 100.0f;//m
gps.z = gps_utm_zone;// gps_week;
debug_vect("GPS", gps);
}
beep_on_low_voltage();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER7, loop_start_time))
{
led_toggle(LED_YELLOW);
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//get thru all gps messages
debug_message_send_one();
}
communication_send_attitude_position(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 200 Hz functions //
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER5, loop_start_time))
{
if (global_data.state.status == MAV_STATE_STANDBY)
{
//Check if parameters should be written or read
param_handler();
}
}
///////////////////////////////////////////////////////////////////////////
else {
// All Tasks are fine and we have no starvation
last_mainloop_idle = loop_start_time;
}
// Read out comm at max rate - takes only a few microseconds in worst case
communication_receive();
// MCU load measurement
uint64_t loop_stop_time = sys_time_clock_get_time_usec();
global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
if (loop_start_time - last_mainloop_idle >= 5000)
{
debug_message_buffer(
"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
}
if (global_data.cpu_usage > 800)
{
// CPU load higher than 80%
debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
}
} // End while(1)
}
/**
* \brief Main program function
*
* This functions parse command line options to determine Isaac behaviour
* This progran can be used as a Isaac process or a CLI client
* if -r option is specified.
*
* \param argc Argument counter
* \param argv Array of string arguments passed to the program
*/
int
main(int argc, char *argv[])
{
pid_t pid;
int opt_remote = 0;
char opt;
char * xarg = NULL;
// Parse commandline arguments
while ((opt = getopt(argc, argv, "dhrvx:")) != EOF) {
switch (opt) {
case 'd':
debug++;
break;
case 'r':
opt_remote++;
break;
case 'x':
opt_execute++;
opt_remote++; // This is implicit in 'x'
xarg = strdup(optarg);
break;
case 'v':
version();
exit(EXIT_SUCCESS);
case 'h':
case '?':
usage();
exit(EXIT_SUCCESS);
}
}
// Check if there is an Isaac is already running
if (opt_remote) {
if (remote_tryconnect() == 0) {
if (!opt_execute) {
remote_control(NULL);
return 0;
} else {
remote_control(xarg);
return 0;
}
} else {
fprintf(stderr, "Unable to connect to remote Isaac (does %s exist?)\n", CLI_SOCKET);
exit(1);
}
} else {
// Check Isaac is not already running
if (access(CLI_SOCKET, F_OK) == 0) {
fprintf(stderr, "%s already running on %s. Use '%s -r' to connect\n", argv[0],
CLI_SOCKET, argv[0]);
exit(1);
}
}
// If we are not in debug mode, then fork to background
if (!debug) {
if ((pid = fork()) < 0)
exit(1);
else if (pid > 0)
exit(0);
}
cfg_init(&config);
// Setup signal handlers
(void) signal(SIGINT, quit);
(void) signal(SIGTERM, quit);
(void) signal(SIGPIPE, SIG_IGN);
// Read configuration files
if (cfg_read(&config, CFILE) != 0) {
fprintf(stderr, "Failed to read configuration file %s\n", CFILE);
quit(EXIT_FAILURE);
}
// Initialize logging
if (start_logging(config.logtype, config.logfile, config.logtag, config.loglevel) != 0) {
fprintf(stderr, "Failed to read configuration file %s\n", CFILE);
quit(EXIT_FAILURE);
}
// Load Modules. The contain the server Applications
if (load_modules() != 0) {
quit(EXIT_FAILURE);
}
// Start manager thread
if (start_manager(config.manaddr, config.manport, config.manuser, config.manpass) != 0) {
quit(EXIT_FAILURE);
}
// Start cli service
if (cli_server_start() != 0) {
quit(EXIT_FAILURE);
}
// Start server thread
if (start_server(config.listenaddr, config.listenport) == -1) {
quit(EXIT_FAILURE);
}
// All subsystems Up!
isaac_log(LOG_NONE, "\e[1;37m%s Ready.\e[0m\n", PACKAGE_NAME);
// Wait here until any signal is sent
pause();
// Unreachable code :D
return 0;
}
void main_loop_quadrotor(void)
{
/**
* @brief Initialize the whole system
*
* All functions that need to be called before the first mainloop iteration
* should be placed here.
*/
main_init_generic();
control_quadrotor_position_init();
control_quadrotor_attitude_init();
attitude_tobi_laurens_init();
// FIXME XXX Make proper mode switching
// outdoor_position_kalman_init();
//vision_position_kalman_init();
// Default filters, allow Vision, Vicon and optical flow inputs
vicon_position_kalman_init();
optflow_speed_kalman_init();
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
// Executiontime debugging
time_debug.x = 0;
time_debug.y = 0;
time_debug.z = 0;
last_mainloop_idle = sys_time_clock_get_time_usec();
debug_message_buffer("Starting main loop");
led_off(LED_GREEN);
led_off(LED_RED);
while (1)
{
// Time Measurement
uint64_t loop_start_time = sys_time_clock_set_loop_start_time(); // loop_start_time should not be used anymore
///////////////////////////////////////////////////////////////////////////
/// CRITICAL 200 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(5000, COUNTER2, loop_start_time))
{
// Kalman Attitude filter, used on all systems
gyro_read();
sensors_read_acc();
sensors_pressure_bmp085_read_out();
// Read out magnetometer at its default 50 Hz rate
static uint8_t mag_count = 0;
if (mag_count == 3)
{
sensors_read_mag();
//attitude_observer_correct_magnet(global_data.magnet_corrected);
mag_count = 0;
}else if(mag_count==1){
hmc5843_start_read();
mag_count++;
}
else
{
mag_count++;
}
// Correction step of observer filter
attitude_tobi_laurens();
if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VICON_ONLY ||
global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VISION_VICON_BACKUP)
{
vicon_position_kalman();
}
else if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_GPS_ONLY)
{
outdoor_position_kalman();
}
control_quadrotor_attitude();
//debug counting number of executions
count++;
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// Camera Shutter - This takes 50 usecs!!!
///////////////////////////////////////////////////////////////////////////
// Set camera shutter with 2.5ms resolution
else if (us_run_every(5000, COUNTER1, loop_start_time)) //was 2500 !!!
{
camera_shutter_handling(loop_start_time);
// Measure time for debugging
time_debug.x = max(time_debug.x, sys_time_clock_get_time_usec()
- loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 50 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(20000, COUNTER3, loop_start_time))
{
// Read infrared sensor
//adc_read();
// Control the quadrotor position
control_quadrotor_position();
// Read remote control
remote_control();
control_camera_angle();
// //float_vect3 opt;
// static float_vect3 opt_int;
// uint8_t valid = optical_flow_get_dxy(80, &global_data.optflow.x, &global_data.optflow.y, &global_data.optflow.z);
// if (valid)
// {
// opt_int.x += global_data.optflow.x;
// opt_int.y += global_data.optflow.y;
//
// }
//
// uint8_t supersampling = 10;
// for (int i = 0; i < supersampling; ++i)
// {
// global_data.sonar_distance += sonar_distance_get(ADC_5_CHANNEL);
// }
//
// global_data.sonar_distance /= supersampling;
//
// opt_int.z = valid;
// static unsigned int i = 0;
// if (i == 10)
// {
// mavlink_msg_optical_flow_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_loop_start_time(), 0, global_data.optflow.x, global_data.optflow.y, global_data.optflow.z, global_data.sonar_distance_filtered);
//
// i = 0;
// }
// i++;
//optical_flow_debug_vect_send();
//debug_vect("opt_int", opt_int);
// optical_flow_start_read(80);
if (global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_INTEGRATING
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_NON_INTEGRATING
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VICON_AS_OFFSET
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VISION_AS_OFFSET
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ODOMETRY_ADD_VISION_AS_OFFSET
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VICON
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_GPS_OPTICAL_FLOW
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_GLOBAL_VISION
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VISUAL_ODOMETRY_GLOBAL_VISION)
{
optflow_speed_kalman();
}
// Send the raw sensor/ADC values
communication_send_raw_data(loop_start_time);
float_vect3 yy; yy.x = global_data.yaw_lowpass; yy.y = 0.f; yy.z = 0.f;
debug_vect("yaw_low", yy);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER4, loop_start_time))
{
//*** this happens in handle_controller_timeouts already!!!!! ***
// //update global_data.state
// if (global_data.param[PARAM_VICON_MODE] == 1)
// {
// //VICON_MODE 1 only accepts vicon position
// global_data.state.position_fix = global_data.state.vicon_ok;
// }
// else
// {
// //VICON_MODEs 0, 2, 3 accepts vision additionally, so check vision
// global_data.state.position_fix = global_data.state.vision_ok;
// }
update_system_statemachine(loop_start_time);
update_controller_setpoints();
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(
global_data.param[PARAM_SEND_DEBUGCHAN],
sys_time_clock_get_loop_start_time_boot_ms(),
global_data.attitude_setpoint.x,
global_data.attitude_setpoint.y,
global_data.position_yaw_control_output,
global_data.thrust_control_output);
//STARTING AND LANDING
quadrotor_start_land_handler(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 100 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER6, loop_start_time))
{
if (global_data.param[PARAM_SEND_SLOT_DEBUG_6])
{
debug_vect("att_ctrl_o", global_data.attitude_control_output);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// UNCRITICAL SLOW 5 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(200000, COUNTER8, loop_start_time))
{
// The onboard controllers go into failsafe mode once
// position data is missing
handle_controller_timeouts(loop_start_time);
// Send buffered data such as debug text messages
// Empty one message out of the buffer
debug_message_send_one();
// Toggle status led
led_toggle(LED_RED);
// Toggle active mode led
if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED)
{
led_on(LED_GREEN);
}
else
{
led_off(LED_GREEN);
}
handle_eeprom_write_request();
handle_reset_request();
update_controller_parameters();
communication_send_controller_feedback();
communication_send_remote_control();
// Pressure sensor driver works, but not tested regarding stability
// sensors_pressure_bmp085_read_out();
if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
{
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
91, global_data.yaw_pos_setpoint);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 1 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(1000000, COUNTER9, loop_start_time))
{
// Send system state, mode, battery voltage, etc.
send_system_state();
// Send position setpoint offset
//debug_vect("pos offs", global_data.position_setpoint_offset);
// Send current onboard time
mavlink_msg_system_time_send(MAVLINK_COMM_1, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());
mavlink_msg_system_time_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());
//update state from received parameters
sync_state_parameters();
//debug number of execution
count = 0;
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//Send GPS information
float_vect3 gps;
gps.x = gps_utm_north / 100.0f;//m
gps.y = gps_utm_east / 100.0f;//m
gps.z = gps_utm_zone;// gps_week;
debug_vect("GPS", gps);
}
else if (global_data.param[PARAM_GPS_MODE] == 9
|| global_data.param[PARAM_GPS_MODE] == 8)
{
if (global_data.param[PARAM_GPS_MODE] == 8)
{
gps_set_local_origin();
// gps_local_home_init = false;
}
if (gps_lat == 0)
{
debug_message_buffer("GPS Signal Lost");
}
else
{
float_vect3 gps_local, gps_local_velocity;
gps_get_local_position(&gps_local);
debug_vect("GPS local", gps_local);
gps_get_local_velocity(&gps_local_velocity);
debug_vect("GPS loc velocity", gps_local_velocity);
}
}
if (global_data.state.gps_mode)
{
gps_send_local_origin();
}
beep_on_low_voltage();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER7, loop_start_time))
{
//led_toggle(LED_YELLOW);
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//get thru all gps messages
debug_message_send_one();
}
communication_send_attitude_position(loop_start_time);
// Send parameter
communication_queued_send();
// //infrared distance
// float_vect3 infra;
// infra.x = global_data.ground_distance;
// infra.y = global_data.ground_distance_unfiltered;
// infra.z = global_data.state.ground_distance_ok;
// debug_vect("infrared", infra);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 200 Hz functions //
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER5, loop_start_time))
{
if (global_data.state.status == MAV_STATE_STANDBY)
{
//Check if parameters should be written or read
param_handler();
}
}
///////////////////////////////////////////////////////////////////////////
else
{
// All Tasks are fine and we have no starvation
last_mainloop_idle = loop_start_time;
}
// Read out comm at max rate - takes only a few microseconds in worst case
communication_receive();
// MCU load measurement
uint64_t loop_stop_time = sys_time_clock_get_time_usec();
global_data.cpu_usage = measure_avg_cpu_load(loop_start_time,
loop_stop_time, min_mainloop_time);
global_data.cpu_peak = measure_peak_cpu_load(loop_start_time,
loop_stop_time, min_mainloop_time);
time_debug.y = max(time_debug.y, global_data.cpu_usage);
time_debug.z = max(time_debug.z, global_data.cpu_peak);
if (loop_start_time - last_mainloop_idle >= 20000)
{
debug_message_buffer(
"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
}
if (global_data.cpu_usage > 800)
{
// CPU load higher than 80%
debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
}
} // End while(1)
}
/**
* @brief Initialize the whole system
*
* All functions that need to be called before the first mainloop iteration
* should be placed here.
*/
void main_init_generic(void)
{
// Reset to safe values
global_data_reset();
// Load default eeprom parameters as fallback
global_data_reset_param_defaults();
// LOWLEVEL INIT, ONLY VERY BASIC SYSTEM FUNCTIONS
hw_init();
enableIRQ();
led_init();
led_on(LED_GREEN);
buzzer_init();
sys_time_init();
sys_time_periodic_init();
sys_time_clock_init();
ppm_init();
pwm_init();
// Lowlevel periphel support init
adc_init();
// FIXME SDCARD
// MMC_IO_Init();
spi_init();
i2c_init();
// Sensor init
sensors_init();
debug_message_buffer("Sensor initialized");
// Shutter init
shutter_init();
shutter_control(0);
// Debug output init
debug_message_init();
debug_message_buffer("Text message buffer initialized");
// MEDIUM LEVEL INIT, INITIALIZE I2C, EEPROM, WAIT FOR MOTOR CONTROLLERS
// Try to reach the EEPROM
eeprom_check_start();
// WAIT FOR 2 SECONDS FOR THE USER TO NOT TOUCH THE UNIT
while (sys_time_clock_get_time_usec() < 2000000)
{
}
// Do the auto-gyro calibration for 1 second
// Get current temperature
led_on(LED_RED);
gyro_init();
// uint8_t timeout = 3;
// // Check for SD card
// while (sys_time_clock_get_time_usec() < 2000000)
// {
// while (GetDriveInformation() != F_OK && timeout--)
// {
// debug_message_buffer("MMC/SD-Card not found ! retrying..");
// }
// }
//
// if (GetDriveInformation() == F_OK)
// {
// debug_message_buffer("MMC/SD-Card SUCCESS: FOUND");
// }
// else
// {
// debug_message_buffer("MMC/SD-Card FAILURE: NOT FOUND");
// }
//FIXME redo init because of SD driver decreasing speed
//spi_init();
led_off(LED_RED);
// Stop trying to reach the EEPROM - if it has not been found by now, assume
// there is no EEPROM mounted
if (eeprom_check_ok())
{
param_read_all();
debug_message_buffer("EEPROM detected - reading parameters from EEPROM");
for (int i = 0; i < ONBOARD_PARAM_COUNT * 2 + 20; i++)
{
param_handler();
//sleep 1 ms
sys_time_wait(1000);
}
}
else
{
debug_message_buffer("NO EEPROM - reading onboard parameters from FLASH");
}
// Set mavlink system
mavlink_system.compid = MAV_COMP_ID_IMU;
mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID];
//Magnet sensor
hmc5843_init();
acc_init();
// Comm parameter init
mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID]; // System ID, 1-255
mavlink_system.compid = global_data.param[PARAM_COMPONENT_ID]; // Component/Subsystem ID, 1-255
// Comm init has to be
// AFTER PARAM INIT
comm_init(MAVLINK_COMM_0);
comm_init(MAVLINK_COMM_1);
// UART initialized, now initialize COMM peripherals
communication_init();
gps_init();
us_run_init();
servos_init();
//position_kalman3_init();
// Calibration starts (this can take a few seconds)
// led_on(LED_GREEN);
// led_on(LED_RED);
// Read out first time battery
global_data.battery_voltage = battery_get_value();
global_data.state.mav_mode = MAV_MODE_PREFLIGHT;
global_data.state.status = MAV_STATE_CALIBRATING;
send_system_state();
float_vect3 init_state_accel;
init_state_accel.x = 0.0f;
init_state_accel.y = 0.0f;
init_state_accel.z = -1000.0f;
float_vect3 init_state_magnet;
init_state_magnet.x = 1.0f;
init_state_magnet.y = 0.0f;
init_state_magnet.z = 0.0f;
//auto_calibration();
attitude_observer_init(init_state_accel, init_state_magnet);
debug_message_buffer("Attitude Filter initialized");
led_on(LED_RED);
send_system_state();
debug_message_buffer("System is initialized");
// Calibration stopped
led_off(LED_RED);
global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
global_data.state.status = MAV_STATE_STANDBY;
send_system_state();
debug_message_buffer("Checking if remote control is switched on:");
// Initialize remote control status
remote_control();
remote_control();
if (radio_control_status() == RADIO_CONTROL_ON && global_data.state.remote_ok)
{
global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED;
debug_message_buffer("RESULT: remote control switched ON");
debug_message_buffer("Now in MAV_MODE_TEST2 position hold tobi_laurens");
led_on(LED_GREEN);
}
else
{
global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
debug_message_buffer("RESULT: remote control switched OFF");
led_off(LED_GREEN);
}
}
