void ControlInput::task(void){
control_receiver->execute();
update_status();
//If the control receiver is OK we'll get data and publish:
if(status == STATUS_OK){
//Get Raw controller values:
//HACK-ALERT: ROLL AND YAW ARE ONLY INVERTED DUE TO THE DX6 transmitter!
control_receiver->get_all_channels(raw_values);
raw_values.roll *= -1;
raw_values.yaw *= -1;
//convert to attitude:
control_socket.pitch = convert_joystick_to_degree(raw_values.pitch);
control_socket.roll = convert_joystick_to_degree(raw_values.roll);
control_socket.yaw = convert_joystick_to_degree(raw_values.yaw);
control_socket.throttle = convert_joystick_to_throttle(raw_values.throttle);
//Update switch and clicker:
aux_states[0] = raw_values.aux1;
aux_states[1] = raw_values.aux2;
if(aux_states[0] != last_aux_states[0]) handle_aux1(aux_states[0]);
if(aux_states[1] != last_aux_states[1]) handle_aux2(aux_states[1]);
last_aux_states[0] = aux_states[0];
last_aux_states[1] = aux_states[1];
//If we are in fixed altitude mode, we'll integrate the throttle stick
//to get a new altitude.
if(control_socket.control_mode == CONTROLMODE_FIXED_ALTITUDE){
process_altitude_setpoint();
}
control_socket.altitude = contrain(control_socket.altitude, 0, 300);
}else{
//THE CONTROL RECEIVER ISN'T OK!!
control_socket.reset();
}
control_socket.publish();
check_calibration();
check_arm();
if(debugging_stream) handle_debug_stream();
}
static int start_url_load(struct boot_task *task, const char *name,
struct pb_url *url, struct load_url_result **result)
{
if (!url)
return 0;
*result = load_url_async(task, url, boot_process, task);
if (!*result) {
update_status(task->status_fn, task->status_arg,
BOOT_STATUS_ERROR,
_("Error loading %s"), name);
return -1;
}
return 0;
}
/*
processPatch:
Performs all processing necessary on the given patch.
Expects a fully prepared patch.
- read in & range compress the data (rciq).
- fft the data along azimuth.
- range migrate the data (rmpatch).
- azimuth compress the data (acpatch).
the data is returned in the patch's trans array.
*/
void processPatch(patch *p,const getRec *signalGetRec,const rangeRef *r,
const satellite *s)
{
int i;
update_status("Range compressing");
if (!quietflag) printf(" RANGE COMPRESSING CHANNELS...\n");
elapse(0);
rciq(p,signalGetRec,r);
if (!quietflag) elapse(1);
if (s->debugFlag & AZ_RAW_T) debugWritePatch(p,"az_raw_t");
update_status("Starting azimuth compression");
if (!quietflag) printf(" TRANSFORMING LINES...\n");
elapse(0);
cfft1d(p->n_az,NULL,0);
for (i=0; i<p->n_range; i++) cfft1d(p->n_az,&p->trans[i*p->n_az],-1);
if (!quietflag) elapse(1);
if (s->debugFlag & AZ_RAW_F) debugWritePatch(p,"az_raw_f");
if (!(s->debugFlag & NO_RCM))
{
update_status("Range cell migration");
if (!quietflag) printf(" START RANGE MIGRATION CORRECTION...\n");
elapse(0);
rmpatch(p,s);
if (!quietflag) elapse(1);
if (s->debugFlag & AZ_MIG_F) debugWritePatch(p,"az_mig_f");
}
update_status("Finishing azimuth compression");
if (!quietflag) printf(" INVERSE TRANSFORMING LINES...\n");
elapse(0);
acpatch(p,s);
if (!quietflag) elapse(1);
/* if (!quietflag) printf(" Range-Doppler done...\n");*/
}
void abts_size_equal(abts_case *tc, size_t expected, size_t actual, int lineno)
{
update_status();
if (tc->failed) return;
if (expected == actual) return;
tc->failed = TRUE;
if (verbose) {
/* Note that the comparison is type-exact, reporting must be a best-fit */
fprintf(stderr, "Line %d: expected %lu, but saw %lu\n", lineno,
(unsigned long)expected, (unsigned long)actual);
fflush(stderr);
}
}
void abts_str_nequal(abts_case *tc, const char *expected, const char *actual,
size_t n, int lineno)
{
update_status();
if (tc->failed) return;
if (!strncmp(expected, actual, n)) return;
tc->failed = TRUE;
if (verbose) {
fprintf(stderr, "Line %d: expected something other than <%s>, but saw <%s>\n",
lineno, expected, actual);
fflush(stderr);
}
}
void abts_str_equal(abts_case *tc, const char *expected, const char *actual, int lineno)
{
update_status();
if (tc->failed) return;
if (!expected && !actual) return;
if (expected && actual)
if (!strcmp(expected, actual)) return;
tc->failed = TRUE;
if (verbose) {
fprintf(stderr, "Line %d: expected <%s>, but saw <%s>\n", lineno, expected, actual);
fflush(stderr);
}
}
/* Handle the user requesting a status change */
static void
user_status_change (DbusmenuMenuitem * item, guint timestamp, gpointer pstatus)
{
StatusProviderStatus status = GPOINTER_TO_INT(pstatus);
GList * provider;
/* Set each provider to this status */
for (provider = status_providers; provider != NULL; provider = g_list_next(provider)) {
status_provider_set_status(STATUS_PROVIDER(provider->data), status);
}
/* See what we really are now */
update_status();
return;
}
static void popup_timer_callback(void *data) {
APP_LOG(APP_LOG_LEVEL_DEBUG, "Popup timer callback: %d", s_popup_state);
// State 0: do nothing, probably a race condition
// State 1: Pending timed out, so return to state 0
if (2 == s_popup_state) {
// State 2: Close the window, return to state 0
window_stack_pop(true);
}
APP_LOG(APP_LOG_LEVEL_DEBUG, "Clearing popup state");
s_popup_state = 0;
set_status_text("");
update_status();
}
// Constantly writes garbage into the texture memory
void* texture_loop(void *ptr)
{
viewport_t *viewport = (viewport_t *)ptr;
int value = 0;
int i;
while(1)
{
memset((unsigned char*)viewport->screen->pixels + 0x1800000, value, 1920*1080*2);
value++;
if(value >= 255) value = 0;
viewport->status.blits++;
update_status(&viewport->status);
}
}
void setup(){
// remove zombies on SIGCHLD
sigchld(0);
gScreen = DefaultScreen(gDisplay);
gRoot = RootWindow(gDisplay, gScreen);
XSelectInput(gDisplay, gRoot, ROOT_EVENT_MASK);
// initialize subsystems
inputs_init();
clientlist_init();
layout_init();
systray_init();
update_status();
}
void
laudio_stop(void)
{
struct pcm_packet *pkt;
int ret;
update_status(LAUDIO_STOPPING);
ret = ioctl(oss_fd, SNDCTL_DSP_HALT_OUTPUT, NULL);
if (ret < 0)
DPRINTF(E_LOG, L_LAUDIO, "Failed to halt output: %s\n", strerror(errno));
for (pkt = pcm_pkt_head; pcm_pkt_head; pkt = pcm_pkt_head)
{
pcm_pkt_head = pkt->next;
free(pkt);
}
pcm_pkt_head = NULL;
pcm_pkt_tail = NULL;
update_status(LAUDIO_OPEN);
}
void start_scheduler(void) {
while (1) {
//timer interrupt
if (sampling_flag == 1) {
timer_rst();
if (curr_channel == 1) {
// used to indicate scheduler cycle in GPIO pin RA5
update_status(SYSTEM_START);
PORTAbits.RA5 = !PORTAbits.RA5;
} else if (curr_channel == 100) {
update_status(SYSTEM_GYRO);
done = gyro_task();
} else if (curr_channel == 200) {
update_status(SYSTEM_ACCELERO);
done = get_acceleration();
} else if (curr_channel == 300) {
update_status(SYSTEM_COMPASS);
done = get_compass();
// //done = Compass_test_single();
// } else if (curr_channel == 250) {
} else if (curr_channel == 400) {
update_status(SYSTEM_FUSION);
ComplementaryFilter();
fusion_final();
} else if (curr_channel == 700) {
update_status(SYSTEM_UART);
//done = uart_task();
}
} else {
//update status if a task terminate
if (done && (check_status() == SYSTEM_UART) && TXSTAbits.TRMT) {
update_status(SYSTEM_IDLE);
done = 0;
} else if (done && ((check_status() == SYSTEM_GYRO) ||
check_status() == SYSTEM_ACCELERO ||
check_status() == SYSTEM_COMPASS ||
check_status() == SYSTEM_FUSION
)) {
update_status(SYSTEM_IDLE);
done = 0;
}
}
}
}
void viewport_draw(viewport_t *viewport)
{
texture_iterate(viewport);
// usleep(4000);
draw_thing(viewport);
viewport->current_texture = get_next_texture(viewport);
viewport->status.frames++;
update_status(&viewport->status);
// if(!viewport->button_down) viewport->xrot += 0.1;
};
void testmod_d::update(double time){
//
// update input models and parameters
//
update_inp(time);
if (update_status()){
//
// Perform local model calculations
//
so[0] = si[2] * sin( si[1] * si[0] );
dso_dsi[0][1] = si[2] * si[0] * cos( si[1] * si[0] );
dso_dsi[0][2] = sin( si[1] * si[0] );
dso_dsi[0][0] = si[2] * si[1] * cos( si[1] * si[0] );
chain_partials();
}
};
/*
update the state of the sensor
*/
void AP_RangeFinder_Benewake::update(void)
{
bool signal_ok;
if (get_reading(state.distance_cm, signal_ok)) {
// update range_valid state based on distance measured
last_reading_ms = AP_HAL::millis();
if (signal_ok) {
update_status();
} else {
// if signal is weak set status to out-of-range
set_status(RangeFinder::RangeFinder_OutOfRangeHigh);
}
} else if (AP_HAL::millis() - last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData);
}
}
static int
laudio_alsa_start(uint64_t cur_pos, uint64_t next_pkt)
{
int ret;
ret = snd_pcm_prepare(hdl);
if (ret < 0)
{
DPRINTF(E_LOG, L_LAUDIO, "Could not prepare PCM device: %s\n", snd_strerror(ret));
return -1;
}
DPRINTF(E_DBG, L_LAUDIO, "Start local audio curpos %" PRIu64 ", next_pkt %" PRIu64 "\n", cur_pos, next_pkt);
DPRINTF(E_DBG, L_LAUDIO, "PCM will start after %d samples (%d packets)\n", pcm_buf_threshold, pcm_buf_threshold / AIRTUNES_V2_PACKET_SAMPLES);
/* Make pcm_pos the rtptime of the packet containing cur_pos */
pcm_pos = next_pkt;
while (pcm_pos > cur_pos)
pcm_pos -= AIRTUNES_V2_PACKET_SAMPLES;
pcm_start_pos = next_pkt + pcm_buf_threshold;
/* Compensate threshold, as it's taken into account by snd_pcm_delay() */
//pcm_pos += pcm_buf_threshold;
DPRINTF(E_DBG, L_LAUDIO, "PCM pos %" PRIu64 ", start pos %" PRIu64 "\n", pcm_pos, pcm_start_pos);
pcm_pkt_head = NULL;
pcm_pkt_tail = NULL;
pcm_last_error = 0;
pcm_recovery = 0;
ret = laudio_alsa_set_start_threshold(pcm_buf_threshold);
if (ret < 0)
{
DPRINTF(E_LOG, L_LAUDIO, "Could not set PCM start threshold for local audio start\n");
return -1;
}
update_status(LAUDIO_STARTED);
return 0;
}
void update_all(int gno)
{
update_set_lists(gno);
update_world(gno);
update_view(gno);
update_status(gno, cur_statusitem, -1);
update_ticks(gno);
update_autos(gno);
updatelegends(gno);
updatesymbols(gno, -1);
update_label_proc();
update_locator_items(gno);
update_draw();
update_frame_items(gno);
update_graph_items();
update_hotlinks();
}
static void stopservice(struct svdir *s)
{
if (s->pid && !custom(s, 't')) {
kill(s->pid, SIGTERM);
s->ctrl |= C_TERM;
update_status(s);
}
if (s->sd_want == W_DOWN) {
kill(s->pid, SIGCONT);
custom(s, 'd');
return;
}
if (s->sd_want == W_EXIT) {
kill(s->pid, SIGCONT);
custom(s, 'x');
}
}
void AP_AccelCal::clear()
{
if (!_started) {
return;
}
AccelCalibrator *cal;
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
cal->clear();
}
_step = 0;
_started = false;
_saving = false;
update_status();
}
int main(int argc, char **argv)
{
bindtextdomain("tray_eject", LOCALE_DIR);
textdomain("tray_eject");
gtk_init(&argc, &argv);
count = 0;
dev = g_hash_table_new_full(g_str_hash, g_str_equal, key_destroy,
value_destroy);
icon = (GtkStatusIcon *)
gtk_status_icon_new_from_file(ICON_PATH "dev1.png");
update_status();
menu = gtk_menu_new();
item = gtk_menu_item_new_with_label(_("Safely remove all"));
gtk_widget_show(item);
gtk_menu_shell_append(GTK_MENU_SHELL(menu), item);
g_signal_connect(G_OBJECT(item), "activate", G_CALLBACK(eject), NULL);
sep = gtk_separator_menu_item_new();
gtk_widget_show(sep);
gtk_menu_shell_append(GTK_MENU_SHELL(menu), sep);
g_signal_connect(G_OBJECT(icon), "popup-menu",
G_CALLBACK(popup), NULL);
if (init_hal() < 0)
return 1;
dialog = gtk_dialog_new_with_buttons("Error", NULL,
GTK_DIALOG_MODAL | GTK_DIALOG_DESTROY_WITH_PARENT,
GTK_STOCK_OK, NULL);
gtk_window_set_icon_from_file(GTK_WINDOW(dialog), ICON_PATH "error.png", NULL);
msg = gtk_label_new(_("Unable to unmount this device. Some\napplication is likely to be using it.\nPlease close the offending application\nand try again."));
gtk_box_pack_start(GTK_BOX(GTK_DIALOG(dialog)->vbox), msg, TRUE, TRUE, 10);
gtk_widget_show(msg);
gtk_main();
return 0;
}
void viewport_draw(viewport_t *viewport)
{
// usleep(14000);
clear_viewport();
//printf("viewport_draw 2\n");
draw_thing(viewport);
//printf("viewport_draw 3\n");
draw_overlays();
//printf("viewport_draw 4\n");
texture_iterate(viewport);
viewport->status.frames++;
update_status(&viewport->status);
if(!viewport->button_down) viewport->xrot += 0.1;
};
void z80dma_device::device_reset()
{
m_status = 0;
m_rdy = 0;
m_force_ready = 0;
m_num_follow = 0;
m_dma_enabled = 0;
m_read_num_follow = m_read_cur_follow = 0;
m_reset_pointer = 0;
// disable interrupts
WR3 &= ~0x20;
m_ip = 0;
m_ius = 0;
m_vector = 0;
update_status();
}
int generate_alps_status(
std::vector<std::string> &status,
const char *apbasil_path,
const char *apbasil_protocol)
{
int rc;
char inventory_command[MAXLINE * 2];
std::string alps_output;
status.clear();
alps_nodes.clear();
snprintf(inventory_command, sizeof(inventory_command), APBASIL_QUERY,
(apbasil_protocol != NULL) ? apbasil_protocol : DEFAULT_APBASIL_PROTOCOL,
(apbasil_path != NULL) ? apbasil_path : DEFAULT_APBASIL_PATH);
rc = get_command_output(inventory_command, alps_output);
if (rc == PBSE_NONE)
{
rc = parse_alps_output(alps_output);
if (rc == PBSE_NONE)
{
if (LOGLEVEL >= 7)
{
/* log the command if output successfully parsed */
snprintf(log_buffer, sizeof(log_buffer),
"Successful inventory command is: %s", inventory_command);
log_event(PBSEVENT_JOB | PBSEVENT_SYSLOG, PBS_EVENTCLASS_JOB, __func__, log_buffer);
}
if (!strcmp(apbasil_protocol, "1.7"))
rc = get_knl_information(apbasil_path);
if (rc == PBSE_NONE)
update_status(status);
}
}
return(rc);
} /* END generate_alps_status() */
int
handle_add_grp (int cli_sd)
{
char grp_name [PFS_NAME_LEN];
char grp_id [PFS_NAME_LEN];
char sd_id [PFS_ID_LEN];
char * in_buf;
memset (grp_name, 0, PFS_NAME_LEN);
in_buf = readline (cli_sd);
if (in_buf == NULL) goto error;
strncpy (grp_name, in_buf, PFS_NAME_LEN);
free (in_buf);
memset (grp_id, 0, PFS_ID_LEN);
in_buf = readline (cli_sd);
if (in_buf == NULL) goto error;
strncpy (grp_id, in_buf, PFS_ID_LEN);
free (in_buf);
memset (sd_id, 0, PFS_ID_LEN);
in_buf = readline (cli_sd);
if (in_buf == NULL) goto error;
strncpy (sd_id, in_buf, PFS_ID_LEN);
free (in_buf);
if (pfs_create_dir_with_id (pfsd->pfs, grp_id) != 0)
goto error;
if (pfs_group_add (pfsd->pfs, grp_name, grp_id) != 0)
goto error;
#ifdef DEBUG
printf ("*** PFS_GRP_CREATE %.*s : %s\n", PFS_ID_LEN, grp_id, grp_name);
#endif
if (update_status (cli_sd, grp_id, sd_id) != 0)
goto error;
writeline (cli_sd, OK, strlen (OK));
return 0;
error:
return -1;
}
void tcp_data_handler::on_write(int fd)
{
if(m_connect_status == STATUS_CONNECTING)
{
m_write_flag = 0 ;
if(m_reactor) m_reactor->mod_handler(fd,this,base_reactor::EVENT_READ) ;
update_status(STATUS_CONNECTED) ;
return ;
}
if(m_sbuf.data_size() > 0 )
{
int to_send = m_sbuf.data_size() > m_max_write_size ? m_max_write_size : m_sbuf.data_size();
int send_size = ::send(fd,m_sbuf.data(),to_send,0) ;
if(send_size >0)
{
m_sbuf.pop_data(send_size) ;
m_sbuf.adjust() ;
}
else if ( send_size < 0)
{
if (errno != EAGAIN && errno != EINTR)
{
handle_error(ERROR_TYPE_SYSTEM) ;
return ;
}
}
}
if(m_sbuf.data_size() == 0)
{
if(m_reactor && (m_write_flag == 1) )
{
m_reactor->mod_handler(fd,this,base_reactor::EVENT_READ) ;
m_write_flag = 0 ;
}
}
}
gboolean data_available(GIOChannel *s, GIOCondition c, gpointer data) {
char buffer[4096];
gsize total_read = 0;
GIOStatus result = 0;
g_assert(c == G_IO_IN);
result = g_io_channel_read_chars(s, buffer, sizeof(buffer), &total_read, NULL);
if (result != G_IO_STATUS_NORMAL || total_read == 0) {
return FALSE;
}
progress_total = MIN(progress_total+total_read, COUNT);
update_status();
if (progress_total==COUNT) {
g_main_loop_quit(loop);
}
return TRUE;
}
void z80dma_device::timerproc()
{
int done;
if (--m_cur_cycle)
{
return;
}
if (m_is_read && !is_ready()) return;
if (m_is_read)
{
do_read();
done = 0;
m_is_read = false;
m_cur_cycle = (PORTA_IS_SOURCE ? PORTA_CYCLE_LEN : PORTB_CYCLE_LEN);
}
else
{
done = do_write();
m_is_read = true;
m_cur_cycle = (PORTB_IS_SOURCE ? PORTA_CYCLE_LEN : PORTB_CYCLE_LEN);
}
if (done)
{
m_dma_enabled = 0; //FIXME: Correct?
m_status = 0x19;
m_status |= !is_ready() << 1; // ready line status
if(TRANSFER_MODE == TM_TRANSFER) m_status |= 0x10; // no match found
update_status();
if (LOG) logerror("Z80DMA '%s' End of Block\n", tag());
if (INT_ON_END_OF_BLOCK)
{
trigger_interrupt(INT_END_OF_BLOCK);
}
}
}
void AP_AccelCal::collect_sample()
{
if (_status != ACCEL_CAL_WAITING_FOR_ORIENTATION) {
return;
}
for(uint8_t i=0; i<_num_clients; i++) {
if (client_active(i) && !_clients[i]->_acal_ready_to_sample()) {
_printf("Not ready to sample");
return;
}
}
AccelCalibrator *cal;
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
cal->collect_sample();
}
update_status();
}
void abts_run_test(abts_suite *ts, abts_case_fntype f, void *arg)
{
abts_case tc;
if (!should_test_run(ts->name)) {
return;
}
tc.failed = 0;
tc.parent = ts;
ts->num_test++;
update_status();
f(&tc, arg);
if (tc.failed) {
ts->num_failed++;
}
}
void SoundDriverDialog::show() {
updating=true;
sound_driver->clear();
for (int i=0;i<SoundDriverManager::get_driver_count();i++) {
sound_driver->add_string( SoundDriverManager::get_driver(i)->get_name() );
}
sound_driver->select( SoundDriverManager::get_current_driver_index() );
update_parameters();
update_status();
Window::show();
updating=false;
}
