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;
}
}
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;
}
}
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;
}
//--------------------------------------------------------
//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;
}
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;
}
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);
}
//--------------------------------------------------------
//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);
}
//--------------------------------------------------------
//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);
}
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);
}
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;
}
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;
}
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");
}
}
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;
}
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;
}
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;
}
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;
}
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);
}
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)));
}
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;
}