Esempio n. 1
0
bool calibration_enter(void)
{
	// If not flying
	if (!sys_state_is_flying())
	{
		calibration_prev_state = sys_get_state();
		calibration_prev_mode = sys_get_mode();
		// Lock vehicle during calibration
		sys_set_mode((uint8_t)MAV_MODE_LOCKED);
		sys_set_state((uint8_t)MAV_STATE_CALIBRATING);
		debug_message_buffer("Starting calibration.");

		mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
				global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
				global_data.motor_block, communication_get_uart_drop_rate());
		mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
				global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
				global_data.motor_block, communication_get_uart_drop_rate());
		debug_message_send_one();
		debug_message_send_one();
		return true;
	}
	else
	{
		//Can't calibrate during flight
		debug_message_buffer("Can't calibrate during flight!!!");
		return false;
	}

}
Esempio n. 2
0
static uint32_t get_global_time(func_cb_ptr p, uint32_t time) {
    app_state_t* s = (app_state_t*)sys_get_state();

    if(s->sync_state == SYNCED || s->sync_state == SYNCING) {
        // we are synced, and thus had at least one time sync exchange
        // if we are level 1, then just return our time
        if(s->level == 1) {
            return time;
        } else {
            uint32_t delta_refresh = 0;
            uint32_t cur_time = sys_time32();
            // check for overflow
            if(cur_time < s->last_refresh) {
                cur_time += 0x7F000000;
            }
            delta_refresh = cur_time - s->last_refresh;
            // only try to refresh if we are not already syncing.
            if (s->sync_state == SYNCED && delta_refresh > REFRESH_INTERVAL) {
                DEBUG("TPSN_NET: Refresh needed refresh: %d\n", delta_refresh);
                s->sync_state = SYNCING;

                start_sync();
            }
            // even though we might be syncing, reply with the current estimate.
            return time + s->clock_drift;
        }
    } else {
        return NOT_SYNCED;
    }

}
Esempio n. 3
0
static int8_t par_sensor_control(func_cb_ptr cb, sensor_driver_command_t command, 
			sos_pid_t app_id, sensor_id_t sensor, sample_context_t *param, void *context) {
	par_sensor_state_t *s = (par_sensor_state_t *)sys_get_state();

	// Return if driver is in error state.
	if (s->state == DRIVER_ERROR) return -EINVAL;

	// Get sensor <-> channel mapping for requested sensor
	uint16_t channel = get_channel(sensor, s->map, NUM_SENSORS); 

	// Return if requested sensor is not supported by this driver.
	if (channel == 0) return -EINVAL;

	switch(command) {
		case SENSOR_REGISTER_REQUEST_COMMAND: {
			if (param == NULL) return -EINVAL;
			return sys_adc_get_data(ADC_REGISTER_REQUEST, app_id, channel, param, context); 
		}
		case SENSOR_GET_DATA_COMMAND: {
			if (param == NULL) return -EINVAL;
			return sys_adc_get_data(ADC_GET_DATA, app_id, channel, param, context); 
		}
		case SENSOR_STOP_DATA_COMMAND: {
			return sys_adc_stop_data(app_id, channel);
		}
		default: return -EINVAL;
	}

	return SOS_OK;
}
Esempio n. 4
0
//--------------------------------------------------------
//static int8_t input0 (func_cb_ptr p, void *data, uint16_t length) {
static int8_t input0 (func_cb_ptr p, token_type_t *t) {
	update_background_state_t *s = (update_background_state_t *)sys_get_state();	

	// Get token from port.

	if (s->state == UPD_STATE_INIT) {	

		s->backMat = (CYCLOPS_Matrix *)capture_token_data(t, s->pid);
		if (s->backMat == NULL) return -ENOMEM;
		
		s->state = UPD_STATE_PROCESS;								
	} else {		
		CYCLOPS_Matrix *M = (CYCLOPS_Matrix *)get_token_data(t);
		//check that input matrix's depth is 1 byte
		if( (M->depth != CYCLOPS_1BYTE) || (s->backMat->depth != CYCLOPS_1BYTE) )
				return -EINVAL;
		token_type_t *my_token = create_token(s->backMat, sizeof(CYCLOPS_Matrix), s->pid);
		if (my_token == NULL) return -EINVAL;
		set_token_type(my_token, CYCLOPS_MATRIX);
		//SOS_CALL(s->put_token, put_token_func_t, s->output0, my_token);
		dispatch(s->output0, my_token);
		destroy_token(my_token);
	}
	return SOS_OK;
}
Esempio n. 5
0
static int8_t update_param (func_cb_ptr p, void *data, uint16_t length) {
	element_state_t *s = (element_state_t *)sys_get_state();
	s->computation_time = *((uint8_t *)data);
	s->bit_mask = *((uint8_t *)((uint8_t *)data + 1));
	DEBUG("Computation time updated to %d seconds.\n", s->computation_time);
	DEBUG("Bit mask updated to 0x%x.\n", s->bit_mask);
	return SOS_OK;
}
Esempio n. 6
0
static void user_isr() {
	test_sensor_state_t *s = (test_sensor_state_t*)sys_get_state();
	sensor_id_t sensor[4] = {LIGHT_AMBIENT_SENSOR, LIGHT_PAR_SENSOR, INTERNAL_TEMPERATURE_SENSOR, INTERNAL_VOLTAGE_SENSOR, };

	LED_DBG(LED_RED_TOGGLE);
	sys_sensor_start_sampling(sensor, 4, &(s->ctx[0]), NULL);
	sensor[0] = TEMPERATURE_SENSOR;
	sensor[1] = HUMIDITY_SENSOR;
	sys_sensor_start_sampling(sensor, 2, &(s->ctx[1]), NULL);
}
Esempio n. 7
0
//--------------------------------------------------------
//static int8_t input3 (func_cb_ptr p, void *data, uint16_t length) {
static int8_t input3 (func_cb_ptr p, token_type_t *t) {
    over_thresh_state_t *s = (over_thresh_state_t *)sys_get_state();

    if (t == NULL) return -EINVAL;

    // Get token from port.
    s->col = *((uint8_t *)get_token_data(t));
    s->state++;

    return process_input(s);
}
Esempio n. 8
0
//--------------------------------------------------------
//static int8_t input1 (func_cb_ptr p, void *data, uint16_t length)
static int8_t input1 (func_cb_ptr p, token_type_t *t) {
    over_thresh_state_t *s = (over_thresh_state_t *)sys_get_state();

    if (t == NULL) return -EINVAL;

    s->A = (CYCLOPS_Matrix *)capture_token_data(t, s->pid);
    if (s->A == NULL) return -ENOMEM;

    s->state++;

    return process_input(s);
}
Esempio n. 9
0
static void start_sync() {
    app_state_t* s = (app_state_t*)sys_get_state();

    LED_DBG(LED_YELLOW_TOGGLE);

    tpsn_req_t* tpsn_req_ptr = (tpsn_req_t *) sys_malloc(sizeof(tpsn_req_t));
    tpsn_req_ptr->type = TPSN_REQUEST;
    tpsn_req_ptr->seq_no = s->current_seq_no;
    DEBUG("TPSN_NET: Transmitting TIMESYNC packet to node %d with seq_no=%d\n", s->parent_id, tpsn_req_ptr->seq_no);
    sys_post_net(s->pid, MSG_TIMESTAMP, sizeof(tpsn_req_t), tpsn_req_ptr, SOS_MSG_RELEASE, s->parent_id);

    sys_timer_start(SYNC_TIMER_ID, 50, TIMER_ONE_SHOT);

}
Esempio n. 10
0
static int8_t par_sensor_feedback(func_cb_ptr cb, sensor_driver_command_t command, 
						uint16_t channel, void *context) {
	par_sensor_state_t *s = (par_sensor_state_t *)sys_get_state();

	// Return if driver is in error state.
	if (s->state == DRIVER_ERROR) return -EINVAL;

	// Get sensor <-> channel mapping for requested sensor
	sensor_id_t sensor = get_sensor(channel, s->map, NUM_SENSORS); 

	// Return if requested sensor is not supported by this driver.
	if (sensor == MAX_NUM_SENSORS) return -EINVAL;

	switch(command) {
		case SENSOR_ENABLE_COMMAND: {
			switch(s->state) {
				case DRIVER_ENABLE: {
					// Re-configure the sensor according to passed context or
					// default settings.
					return SOS_OK;
				}
				case DRIVER_DISABLE: {
					// Turn ON the sensor, and configure it according to
					// passed context or default settings.
					s->state = DRIVER_ENABLE;
					return SOS_OK;
				}
				default: return -EINVAL;
			}
		}
		case SENSOR_DISABLE_COMMAND: {
			switch(s->state) {
				case DRIVER_DISABLE: {
					// Already disabled. Do nothing.
					return SOS_OK;
				}
				case DRIVER_ENABLE: {
					// Turn OFF the sensor.
					s->state = DRIVER_DISABLE;
					return SOS_OK;
				}
				default: return -EINVAL;
			}
			return SOS_OK;
		}
		default: return -EINVAL;
	}

	return SOS_OK;
}
Esempio n. 11
0
static int8_t input0 (func_cb_ptr p, token_type_t *t) {
  element_state_t *s = (element_state_t *)sys_get_state();

  s->value = *((uint8_t *)get_token_data(t));

  // Get token from port.
  // If want to break the chain of calls here, then copy the token into a private
  // data structure(global), and return appropriate value (SOS_OK).

  //SOS_CALL(s->put_token, put_token_func_t, s->output0, &threshold, sizeof(uint8_t));
  LED_DBG(LED_GREEN_TOGGLE);

  sys_post_uart(TRANSMIT_MOD_PID, 0x80, sizeof(uint8_t), &(s->value), 0, BCAST_ADDRESS);
  return SOS_OK;
}
Esempio n. 12
0
void
convolution_init()
{
        //TODO: Move to some common function
	char *progdir = sys_get_config()->dir_clprogs;

	size_t path_len = strlen(progdir) + strlen(kernel_filename) + 2 * sizeof(char);
	char *progpath = malloc(sizeof(char) * path_len);
	sprintf(progpath, "%s/%s",progdir, kernel_filename);
	g_debug("convolution_init: %s", progpath);
	device_result_t err = device_kernel_create(sys_get_state()->context,
						   progpath, "main",
						   &kernel);
	free(progpath);

	if( err != DEVICE_OK ) {
		g_error("Error while creating convolution kernel");
	}
}
Esempio n. 13
0
static int8_t input0 (func_cb_ptr p, token_type_t *t) {
    element_state_t *s = (element_state_t *)sys_get_state();

    // Get token from port.
    // If want to break the chain of calls here, then copy the token into a private
    // data structure(global), and return appropriate value (SOS_OK).

    // Process input: Extract 3 LSB's and pass it on to the next function.
    // We need a separate place to hold the output as we are modifying the input.
    // Remember, this module does not own the input token, so should not
    // overwrite it.
    uint8_t out_value = (*((uint8_t *)get_token_data(t))) & s->bit_mask;

    token_type_t *my_token = create_token(&out_value, sizeof(uint8_t), s->pid);
    if (my_token == NULL) return -ENOMEM;
    //SOS_CALL(s->put_token, put_token_func_t, s->output0, my_token);
    dispatch(s->output0, my_token);
    destroy_token(my_token);
    return SOS_OK;
}
Esempio n. 14
0
static int8_t input0 (func_cb_ptr p, token_type_t *t) {
  element_state_t *s = (element_state_t *)sys_get_state();
	
  // Get token from port.
  // If want to break the chain of calls here, then copy the token into a private
  // data structure(global), and return appropriate value (SOS_OK).

  // Process input: Extract 3 LSB's and pass it on to the next function.
  // We need a separate place to hold the output as we are modifying the input.
  // Remember, this module does not own the input token, so should not
  // overwrite it.
  uint8_t *data = (uint8_t *)capture_token_data(t, s->pid);
  if (data == NULL) return -ENOMEM;

   s->value = (*data) & s->bit_mask;
   DEBUG("\n");
   DEBUG("TRUNCATE_LONG ACCEPTED TOKEN %d.\n", *data);
   DEBUG("\n");
   destroy_token_data(data, t->type, t->length);

   sys_timer_start(TIMER_PID, ((uint32_t)s->computation_time)*1024L, TIMER_ONE_SHOT);

   return -EBUSY;
}
Esempio n. 15
0
File: source.c Progetto: nesl/sos-2x
static int8_t update_param (func_cb_ptr p, void *data, uint16_t length) {
	element_state_t *s = (element_state_t *)sys_get_state();
	s->sample_rate_in_sec = *((uint8_t *)data);
	DEBUG("Sample rate updated to %d seconds.\n", s->sample_rate_in_sec);
	return SOS_OK;
}
Esempio n. 16
0
static int8_t update_param (func_cb_ptr p, void *data, uint16_t length) {
    element_state_t *s = (element_state_t *)sys_get_state();
    s->bit_mask = *((uint8_t *)data);
    DEBUG("Bit mask updated to 0x%x.\n", s->bit_mask);
    return SOS_OK;
}
Esempio n. 17
0
void
convolution_apply(device_buffer_t* src, device_buffer_t* dst,
                  convolution_t* conv)
{
        device_context_t *ctx = sys_get_state()->context;
        
        cl_mem conv_buffer;
        cl_int cl_error;

        size_t local_work_size[] = { conv->w, conv->h };
        size_t global_work_size[2];
        device_buffer_getprop(src, &global_work_size[0], &global_work_size[1], NULL);

        int conv_size[] = { (int)conv->w, (int)conv->h };
        int image_size[] = { (int)global_work_size[0], (int)global_work_size[1] };

        conv_buffer = clCreateBuffer(ctx->context, CL_MEM_READ_ONLY,
                                     sizeof(float)*conv->w*conv->h,
                                     NULL, &cl_error);
        clEnqueueWriteBuffer(ctx->queue, conv_buffer, CL_TRUE,
                             0, sizeof(float)*conv->w*conv->h, conv->matrix, 0, NULL, NULL);

        clSetKernelArg(kernel.kernel, 0, sizeof(src->cl_object), (void*)&src->cl_object);
        clSetKernelArg(kernel.kernel, 1, sizeof(dst->cl_object), (void*)&dst->cl_object);
        clSetKernelArg(kernel.kernel, 2, sizeof(int), (void*)&image_size[0]);
        clSetKernelArg(kernel.kernel, 3, sizeof(int), (void*)&image_size[1]);
        clSetKernelArg(kernel.kernel, 4, sizeof(int), (void*)&conv_size[0]);
        clSetKernelArg(kernel.kernel, 5, sizeof(int), (void*)&conv_size[1]);
        clSetKernelArg(kernel.kernel, 6, sizeof(conv_buffer), (void*)&conv_buffer);
        clSetKernelArg(kernel.kernel, 7, sizeof(float), (void*)&conv->bias);
        clSetKernelArg(kernel.kernel, 8, sizeof(float), (void*)&conv->divisor);

	cl_mem buffers[2];
	buffers[0] = src->cl_object;
	buffers[1] = dst->cl_object;

	cl_error = clEnqueueAcquireGLObjects(ctx->queue, 2, buffers, 0, NULL, NULL);
	if(cl_error != CL_SUCCESS) {
		g_error("convolution_apply: Couldn't aquire CL objects");
	}

        int i=0, r;
        for(i=0; i<2; i++) {
                if(local_work_size[i] > MAX_BLOCK_DIM)
                        local_work_size[i] = MAX_BLOCK_DIM;
                r = global_work_size[i] % local_work_size[i];
                if(r != 0)
                        global_work_size[i] += local_work_size[i] - r;
        }
        
        cl_error = clEnqueueNDRangeKernel(ctx->queue, kernel.kernel, 2, NULL,
                                          global_work_size, local_work_size,
                                          0, NULL, NULL);
        
        if(cl_error != CL_SUCCESS ) {
		g_warning("convolution_apply: Couldn't launch the kernel: %d", cl_error);
                
	}

	cl_error = clEnqueueReleaseGLObjects(ctx->queue, 2 , buffers, 0, NULL, NULL);
	if(cl_error != CL_SUCCESS ) {
		g_warning("convolution_apply: Couldn't release CL objects");
	}
        
        clReleaseMemObject(conv_buffer);
}
Esempio n. 18
0
static int8_t input2 (func_cb_ptr p, token_type_t *t) {
  	element_state_t *s = (element_state_t *)sys_get_state();
	DEBUG("COMBINE: Accept - token %d on input 2.\n", *((uint8_t *)get_token_data(t)));
	return process_input(s, *((uint8_t *)get_token_data(t)));
}
Esempio n. 19
0
static int8_t par_sensor_data_ready_cb(func_cb_ptr cb, adc_feedback_t fb, sos_pid_t app_id, 
							uint16_t channels, sensor_data_msg_t* adc_buf) {
	par_sensor_state_t *s = (par_sensor_state_t *)sys_get_state();
	sensor_data_msg_t *b = adc_buf;
	
	// Get sensor ID from sensor <-> channel mapping
	sensor_id_t sensor = get_sensor(channels, s->map, NUM_SENSORS);
	if (sensor == MAX_NUM_SENSORS) {
		if (b != NULL) sys_free(b);
		return -EINVAL;
	}
	
	switch(fb) {
		case ADC_SENSOR_SEND_DATA: {
			// Sanity check: Verify if there is any buffer to send.
			if (b == NULL) return -EINVAL;
			b->status = SENSOR_DATA;
			b->sensor = sensor;
			break;
		}
		case ADC_SENSOR_CHANNEL_UNBOUND: {
			// 'b' should not point to any buffer here.
			if (b != NULL) sys_free(b);
			// Status buffer: Allocate space for 'b' with 0 samples.
			b = (sensor_data_msg_t *)sys_malloc(sizeof(sensor_data_msg_t));
			if (b == NULL) return -EINVAL;
			b->status = SENSOR_DRIVER_UNREGISTERED;
			b->sensor = sensor;
			b->num_samples = 0;
			break;
		}
		case ADC_SENSOR_SAMPLING_DONE: {
			// 'b' should not point to any buffer here.
			if (b != NULL) sys_free(b);
			// Status buffer: Allocate space for 'b' with 0 samples.
			b = (sensor_data_msg_t *)sys_malloc(sizeof(sensor_data_msg_t));
			if (b == NULL) return -EINVAL;
			b->status = SENSOR_SAMPLING_STOPPED;
			b->sensor = sensor;
			b->num_samples = 0;
			break;
		}
		case ADC_SENSOR_ERROR: {
			// 'b' should not point to any buffer here.
			if (b != NULL) sys_free(b);
			// Status buffer: Allocate space for 'b' with 0 samples.
			b = (sensor_data_msg_t *)sys_malloc(sizeof(sensor_data_msg_t));
			if (b == NULL) return -EINVAL;
			b->status = SENSOR_SAMPLING_ERROR;
			b->sensor = sensor;
			b->num_samples = 0;
			break;
		}
		default: {
			// 'b' should not point to any buffer here.
			if (b != NULL) sys_free(b);
			// Status buffer: Allocate space for 'b' with 0 samples.
			b = (sensor_data_msg_t *)sys_malloc(sizeof(sensor_data_msg_t));
			if (b == NULL) return -EINVAL;
			b->status = SENSOR_STATUS_UNKNOWN;
			b->sensor = sensor;
			b->num_samples = 0;
			break;
		}
	}

	// Post buffer to application
	sys_post(app_id, MSG_DATA_READY, sizeof(sensor_data_msg_t) + 
			(b->num_samples*sizeof(uint16_t)), b, SOS_MSG_RELEASE);

	return SOS_OK;
}