int control_loop_run( void )
{
int first_time = 1;
next_period = get_utime( ) + us_period;
while( running )
{
if( send_motor_signals( ) )
{
break;
}
if( javiator_port_get_data( &javiator_data ) )
{
fprintf( stderr, "ERROR: connection to JAviator broken\n" );
heli_state = HELI_STATE_SHUTDOWN;
perform_shut_down( );
break;
}
if( check_terminal_connection( ) )
{
heli_state = HELI_STATE_SHUTDOWN;
if( first_time )
{
fprintf( stderr, "ERROR: connection to Terminal broken\n" );
first_time = 0;
}
}
else
{
first_time = 1;
}
get_command_data( );
get_control_params( );
prepare_sensor_data( );
switch( heli_state )
{
case HELI_STATE_GROUND:
perform_ground_actions( );
break;
case HELI_STATE_FLYING:
compute_motor_signals( );
break;
case HELI_STATE_SHUTDOWN:
perform_shut_down( );
break;
default:
fprintf( stderr, "ERROR: invalid altitude mode %d\n", heli_state );
}
send_report_to_terminal( );
wait_for_next_period( );
}
iir_lp_filter_destroy( &iir_acc_x );
iir_lp_filter_destroy( &iir_acc_y );
iir_lp_filter_destroy( &iir_acc_z );
iir_lp_filter_destroy( &iir_cmd_roll );
iir_lp_filter_destroy( &iir_cmd_pitch );
iir_lp_filter_destroy( &iir_cmd_yaw );
iir_lp_filter_destroy( &iir_cmd_z );
median_filter_destroy( &med_bat_level );
position_ekf_destroy ( &ekf_pos_z );
controller_destroy ( &ctrl_roll );
controller_destroy ( &ctrl_pitch );
controller_destroy ( &ctrl_yaw );
controller_destroy ( &ctrl_z );
return( 0 );
}
/*
* main() - loop forever, reading the hdaps values and
* parking/unparking as necessary
*/
int main (int argc, char** argv)
{
struct utsname sysinfo;
struct list *p = NULL;
int c, park_now, protect_factor;
int x = 0, y = 0, z = 0;
int fd, i, ret, threshold = 15, adaptive = 0,
pidfile = 0, parked = 0, forceadd = 0;
double unow = 0, parked_utime = 0;
struct option longopts[] =
{
{"device", required_argument, NULL, 'd'},
{"sensitivity", required_argument, NULL, 's'},
{"adaptive", no_argument, NULL, 'a'},
{"verbose", no_argument, NULL, 'v'},
{"background", no_argument, NULL, 'b'},
{"pidfile", optional_argument, NULL, 'p'},
{"dry-run", no_argument, NULL, 't'},
{"poll-sysfs", no_argument, NULL, 'y'},
{"hardware-logic", no_argument, NULL, 'H'},
{"software-logic", no_argument, NULL, 'S'},
{"version", no_argument, NULL, 'V'},
{"help", no_argument, NULL, 'h'},
{"no-leds", no_argument, NULL, 'L'},
{"syslog", no_argument, NULL, 'l'},
{"force", no_argument, NULL, 'f'},
{"force-rotational", no_argument, NULL, 'r'},
{NULL, 0, NULL, 0}
};
if (uname(&sysinfo) < 0 || strcmp("2.6.27", sysinfo.release) <= 0) {
protect_factor = 1000;
kernel_interface = UNLOAD_HEADS;
}
else {
protect_factor = 1;
kernel_interface = PROTECT;
}
openlog(PACKAGE_NAME, LOG_PID, LOG_DAEMON);
while ((c = getopt_long(argc, argv, "d:s:vbap::tyHSVhLlfr", longopts, NULL)) != -1) {
switch (c) {
case 'd':
add_disk(optarg);
break;
case 's':
threshold = atoi(optarg);
break;
case 'b':
background = 1;
break;
case 'a':
adaptive = 1;
break;
case 'v':
verbose = 1;
break;
case 'p':
pidfile = 1;
if (optarg == NULL) {
snprintf(pid_file, sizeof(pid_file), "%s", PID_FILE);
} else {
snprintf(pid_file, sizeof(pid_file), "%s", optarg);
}
break;
case 't':
printlog(stdout, "Dry run, will not actually park heads or freeze queue.");
dry_run = 1;
break;
case 'y':
poll_sysfs = 1;
break;
case 'H':
hardware_logic = 1;
position_interface = INTERFACE_FREEFALL;
break;
case 'S':
force_software_logic = 1;
break;
case 'V':
version();
break;
case 'l':
dosyslog = 1;
break;
case 'L':
use_leds = 0;
break;
case 'f':
forceadd = 1;
break;
case 'r':
forcerotational = 1;
break;
case 'h':
default:
usage();
break;
}
}
printlog(stdout, "Starting "PACKAGE_NAME);
if (disklist && forceadd) {
char protect_method[FILENAME_MAX] = "";
p = disklist;
while (p != NULL) {
snprintf(protect_method, sizeof(protect_method), QUEUE_METHOD_FMT, p->name);
if (kernel_interface == UNLOAD_HEADS)
fd = open (p->protect_file, O_RDWR);
else
fd = open (protect_method, O_RDWR);
if (fd > 0) {
if (kernel_interface == UNLOAD_HEADS)
ret = write(fd, FORCE_UNLOAD_HEADS, strlen(FORCE_UNLOAD_HEADS));
else
ret = write(fd, FORCE_PROTECT_METHOD, strlen(FORCE_PROTECT_METHOD));
if (ret == -1)
printlog(stderr, "Could not forcely enable UNLOAD feature for %s", p->name);
else
printlog(stdout, "Forcely enabled UNLOAD for %s", p->name);
close(fd);
}
else
printlog(stderr, "Could not open %s for forcely enabling UNLOAD feature", p->protect_file);
p = p->next;
}
}
if (disklist == NULL) {
printlog(stdout, "WARNING: You did not supply any devices to protect, trying autodetection.");
if (autodetect_devices() < 1)
printlog(stderr, "Could not detect any devices.");
}
if (disklist == NULL)
usage(argv);
/* Let's see if we're on a ThinkPad or on an *Book */
if (!position_interface)
select_interface(0);
if (!position_interface)
select_interface(1);
if (!position_interface && !hardware_logic) {
printlog(stdout, "Could not find a suitable interface");
return -1;
}
else
printlog(stdout, "Selected interface: %s", interface_names[position_interface]);
if (hardware_logic) {
/* Open the file representing the hardware decision */
freefall_fd = open (FREEFALL_FILE, FREEFALL_FD_FLAGS);
if (freefall_fd < 0) {
printlog(stdout,
"ERROR: Failed openning the hardware logic file (%s). "
"It is probably not supported on your system.",
strerror(errno));
return errno;
}
else {
printlog (stdout, "Uses hardware logic from " FREEFALL_FILE);
}
}
if (!poll_sysfs && !hardware_logic) {
if (position_interface == INTERFACE_HDAPS) {
hdaps_input_nr = device_find_byphys("hdaps/input1");
hdaps_input_fd = device_open(hdaps_input_nr);
if (hdaps_input_fd < 0) {
printlog(stdout,
"WARNING: Could not find hdaps input device (%s). "
"You may be using an incompatible version of the hdaps module. "
"Falling back to reading the position from sysfs (uses more power).\n"
"Use '-y' to silence this warning.",
strerror(errno));
poll_sysfs = 1;
}
else {
printlog(stdout, "Selected HDAPS input device: /dev/input/event%d", hdaps_input_nr);
}
} else if (position_interface == INTERFACE_AMS) {
hdaps_input_nr = device_find_byname("Apple Motion Sensor");
hdaps_input_fd = device_open(hdaps_input_nr);
if (hdaps_input_fd < 0) {
printlog(stdout,
"WARNING: Could not find AMS input device, do you need to set joystick=1?");
poll_sysfs = 1;
}
else {
printlog(stdout, "Selected AMS input device /dev/input/event%d", hdaps_input_nr);
}
} else if (position_interface == INTERFACE_HP3D) {
hdaps_input_nr = device_find_byname("ST LIS3LV02DL Accelerometer");
hdaps_input_fd = device_open(hdaps_input_nr);
if (hdaps_input_fd < 0) {
printlog(stdout,
"WARNING: Could not find HP3D input device");
poll_sysfs = 1;
}
else {
printlog(stdout, "Selected HP3D input device /dev/input/event%d", hdaps_input_nr);
}
} else if (position_interface == INTERFACE_APPLESMC) {
hdaps_input_nr = device_find_byname("applesmc");
hdaps_input_fd = device_open(hdaps_input_nr);
if (hdaps_input_fd < 0) {
printlog(stdout,
"WARNING: Could not find APPLESMC input device");
poll_sysfs = 1;
}
else {
printlog(stdout, "Selected APPLESMC input device /dev/input/event%d", hdaps_input_nr);
}
}
}
if (position_interface != INTERFACE_HP3D && position_interface != INTERFACE_FREEFALL) {
/* LEDs are not supported yet on other systems */
use_leds = 0;
}
if (use_leds) {
fd = open(HP3D_LED_FILE, O_WRONLY);
if (fd < 0)
use_leds = 0;
else
close(fd);
}
if (background) {
verbose = 0;
if (pidfile) {
fd = open (pid_file, O_WRONLY | O_CREAT, 0644);
if (fd < 0) {
printlog (stderr, "Could not create pidfile: %s", pid_file);
return 1;
}
}
daemon(0,0);
if (pidfile) {
char buf[BUF_LEN];
snprintf (buf, sizeof(buf), "%d\n", getpid());
ret = write (fd, buf, strlen(buf));
if (ret < 0) {
printlog (stderr, "Could not write to pidfile %s", pid_file);
return 1;
}
if (close (fd)) {
printlog (stderr, "Could not close pidfile %s", pid_file);
return 1;
}
}
}
mlockall(MCL_FUTURE);
if (verbose) {
p = disklist;
while (p != NULL) {
printf("disk: %s\n", p->name);
p = p->next;
}
printf("threshold: %i\n", threshold);
printf("read_method: %s\n", poll_sysfs ? "poll-sysfs" : (hardware_logic ? "hardware-logic" : "input-dev"));
}
/* check the protect attribute exists */
/* wait for it if it's not there (in case the attribute hasn't been created yet) */
p = disklist;
while (p != NULL && !dry_run) {
fd = open (p->protect_file, O_RDWR);
if (background)
for (i = 0; fd < 0 && i < 100; ++i) {
usleep (100000); /* 10 Hz */
fd = open (p->protect_file, O_RDWR);
}
if (fd < 0) {
printlog (stderr, "Could not open %s\nDoes your kernel/drive support IDLE_IMMEDIATE with UNLOAD?", p->protect_file);
free_disk(disklist);
return 1;
}
close (fd);
p = p->next;
}
/* see if we can read the sensor */
/* wait for it if it's not there (in case the attribute hasn't been created yet) */
if (!hardware_logic) {
ret = read_position_from_sysfs (&x, &y, &z);
if (background)
for (i = 0; ret && i < 100; ++i) {
usleep (100000); /* 10 Hz */
ret = read_position_from_sysfs (&x, &y, &z);
}
if (ret) {
printlog(stderr, "Could not read position from sysfs.");
return 1;
}
}
/* adapt to the driver's sampling rate */
if (position_interface == INTERFACE_HDAPS)
sampling_rate = read_int(HDAPS_SAMPLING_RATE_FILE);
else if (position_interface == INTERFACE_HP3D)
sampling_rate = read_int(HP3D_SAMPLING_RATE_FILE);
if (sampling_rate <= 0)
sampling_rate = DEFAULT_SAMPLING_RATE;
if (verbose)
printf("sampling_rate: %d\n", sampling_rate);
signal(SIGUSR1, SIGUSR1_handler);
signal(SIGTERM, SIGTERM_handler);
while (running) {
if (!hardware_logic) { /* The decision is made by the software */
/* Get statistics and decide what to do */
if (poll_sysfs) {
usleep (1000000/sampling_rate);
ret = read_position_from_sysfs (&x, &y, &z);
unow = get_utime(); /* microsec */
} else {
double oldunow = unow;
int oldx = x, oldy = y, oldz = z;
ret = read_position_from_inputdev (&x, &y, &z, &unow);
/*
* The input device issues events only when the position changed.
* The analysis state needs to know how long the position remained
* unchanged, so send analyze() a fake retroactive update before sending
* the new one.
*/
if (!ret && oldunow && unow-oldunow > 1.5/sampling_rate)
analyze(oldx, oldy, unow-1.0/sampling_rate, threshold, adaptive, parked);
}
if (ret) {
if (verbose)
printf("readout error (%d)\n", ret);
continue;
}
park_now = analyze(x, y, unow, threshold, adaptive, parked);
}
else /* if (hardware_logic) */ {
unsigned char count; /* Number of fall events */
if (!parked) {
/* Wait for the hardware to notify a fall */
ret = read(freefall_fd, &count, sizeof(count));
}
else {
/*
* Poll to check if we no longer are falling
* (hardware_logic polls only when parked)
*/
usleep (1000000/sampling_rate);
fcntl (freefall_fd, F_SETFL, FREEFALL_FD_FLAGS|O_NONBLOCK);
ret = read(freefall_fd, &count, sizeof(count));
fcntl (freefall_fd, F_SETFL, FREEFALL_FD_FLAGS);
/*
* If the error is EAGAIN then it is not a real error but
* a sign that the fall has ended
*/
if (ret != sizeof(count) && errno == EAGAIN) {
count = 0; /* set fall events count to 0 */
ret = sizeof(count); /* Validate count */
}
}
/* handle read errors */
if (ret != sizeof(count)) {
if (verbose)
printf("readout error (%d)\n", ret);
continue;
}
/* Display the read values in verbose mode */
if (verbose)
printf ("HW=%u\n", (unsigned) count);
unow = get_utime(); /* microsec */
park_now = (count > 0);
}
if (park_now && !pause_now) {
if (!parked || unow>parked_utime+REFREEZE_SECONDS) {
/* Not frozen or freeze about to expire */
p = disklist;
while (p != NULL) {
write_protect(p->protect_file,
(FREEZE_SECONDS+FREEZE_EXTRA_SECONDS) * protect_factor);
p = p->next;
}
/*
* Write protect before any output (xterm, or
* whatever else is handling our stdout, may be
* swapped out).
*/
if (!parked) {
printlog(stdout, "parking");
if (use_leds)
write_int (HP3D_LED_FILE, 1);
}
parked = 1;
parked_utime = unow;
}
} else {
if (parked &&
(pause_now || unow>parked_utime+FREEZE_SECONDS)) {
/* Sanity check */
p = disklist;
while (p != NULL) {
if (!dry_run && !read_int(p->protect_file))
printlog(stderr, "Error! Not parked when we "
"thought we were... (paged out "
"and timer expired?)");
/* Freeze has expired */
write_protect(p->protect_file, 0); /* unprotect */
if (use_leds)
write_int (HP3D_LED_FILE, 0);
p = p->next;
}
parked = 0;
printlog(stdout, "un-parking");
}
while (pause_now) {
pause_now = 0;
printlog(stdout, "pausing for %d seconds", SIGUSR1_SLEEP_SEC);
sleep(SIGUSR1_SLEEP_SEC);
}
}
}
free_disk(disklist);
printlog(stdout, "Terminating "PACKAGE_NAME);
closelog();
if (pidfile)
unlink(pid_file);
munlockall();
return ret;
}
int control_loop_run( void )
{
int first_time = 1;
long long start, end;
long long loop_start;
uint32_t rem_time;
next_period = get_utime( ) + us_period;
while( running )
{
trace_data.value_3 = (int16_t)( period * 1000 );
trace_data.value_4 = (int16_t)( heli_state );
/* start of STAT_TO_JAV */
start = get_utime( );
loop_start = start;
rem_time = (uint32_t)( start % MAX_TIME );
trace_data.value_5 = (int16_t)( rem_time >> 16 );
trace_data.value_6 = (int16_t)( rem_time );
if( send_motor_signals( ) )
{
break;
}
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_7 = (int16_t)( rem_time >> 16 );
trace_data.value_8 = (int16_t)( rem_time );
calc_stats( end - start, STAT_TO_JAV );
/* start of STAT_FROM_JAV */
start = get_utime( );
if( javiator_port_get_data( &javiator_data ) )
{
fprintf( stderr, "ERROR: connection to JAviator broken\n" );
heli_state = HELI_STATE_SHUTDOWN;
perform_shut_down( );
break;
}
if( ubisense_enabled )
{
if( (new_data_x = new_data_y = ubisense_port_is_new_data( )) )
{
ubisense_port_get_data( &sensor_data );
}
}
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_9 = (int16_t)( rem_time >> 16 );
trace_data.value_10 = (int16_t)( rem_time );
calc_stats( end - start, STAT_FROM_JAV );
/* start of STAT_FROM_TERM */
start = get_utime( );
if( check_terminal_connection( ) )
{
heli_state = HELI_STATE_SHUTDOWN;
if( first_time )
{
fprintf( stderr, "ERROR: connection to Terminal broken\n" );
first_time = 0;
}
}
else
{
first_time = 1;
}
get_command_data( );
get_control_params( );
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_11 = (int16_t)( rem_time >> 16 );
trace_data.value_12 = (int16_t)( rem_time );
calc_stats( end - start, STAT_FROM_TERM );
/* start of STAT_CONTROL */
start = get_utime( );
prepare_sensor_data( );
switch( heli_state )
{
case HELI_STATE_GROUND:
perform_ground_actions( );
break;
case HELI_STATE_FLYING:
compute_motor_signals( );
break;
case HELI_STATE_SHUTDOWN:
perform_shut_down( );
break;
default:
fprintf( stderr, "ERROR: invalid altitude mode %d\n", heli_state );
}
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_13 = (int16_t)( rem_time >> 16 );
trace_data.value_14 = (int16_t)( rem_time );
calc_stats( end - start, STAT_CONTROL );
/* start of STAT_TO_TERM */
start = get_utime( );
send_report_to_terminal( );
send_trace_data_to_terminal( );
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_15 = (int16_t)( rem_time >> 16 );
trace_data.value_16 = (int16_t)( rem_time );
calc_stats( end - start, STAT_TO_TERM );
calc_stats( end - loop_start, STAT_ALL );
/* start of STAT_SLEEP */
start = get_utime( );
wait_for_next_period( );
end = get_utime( );
calc_stats( end - start, STAT_SLEEP );
if( ++loop_count < 0 )
{
fprintf( stderr, "WARNING: stats overrun\n" );
memset( stats, 0, sizeof( stats ) );
loop_count = 0;
}
}
outlier_filter_destroy( &cof_out_x );
outlier_filter_destroy( &cof_out_y );
iir_lp_filter_destroy ( &iir_acc_x );
iir_lp_filter_destroy ( &iir_acc_y );
iir_lp_filter_destroy ( &iir_acc_z );
iir_lp_filter_destroy ( &iir_cmd_roll );
iir_lp_filter_destroy ( &iir_cmd_pitch );
iir_lp_filter_destroy ( &iir_cmd_yaw );
iir_lp_filter_destroy ( &iir_cmd_z );
average_filter_destroy( &avg_bmu_maps );
median_filter_destroy ( &med_bmu_temp );
median_filter_destroy ( &med_bmu_batt );
attitude_ekf_destroy ( &ekf_att_roll );
attitude_ekf_destroy ( &ekf_att_pitch );
attitude_ekf_destroy ( &ekf_att_yaw );
position_ekf_destroy ( &ekf_pos_x );
position_ekf_destroy ( &ekf_pos_y );
position_ekf_destroy ( &ekf_pos_z );
controller_destroy ( &ctrl_roll );
controller_destroy ( &ctrl_pitch );
controller_destroy ( &ctrl_yaw );
controller_destroy ( &ctrl_x );
controller_destroy ( &ctrl_y );
controller_destroy ( &ctrl_z );
print_stats( );
return( 0 );
}
