uint8_t encode_old_data_for_transmit(node_ref args) {
uint8_t ret = FAILURE;
for(;;) {
if(!args) break;
sensor_ref s = (sensor_ref)node_get_val(args);
if(!s) break;
uint16_t cur_size = 0;
int16_t * array = sensor_get_old_array(s);
uint16_t temp_ptr = write_index;
cur_size = sensor_get_size(s)*sizeof(uint16_t) + HEADER_SIZE;
if(cur_size > OUTPUT_BUFFER_SIZE) return FAILURE; // Not ever going to fit
if(temp_ptr + cur_size > OUTPUT_BUFFER_SIZE - write_index) { //If the output_buffer has no space for msg then return failure.
return FAILURE;
}
output_buffer[temp_ptr++] = 'L';
write_2_bytes_to_string(&output_buffer[temp_ptr], cur_size);
temp_ptr += 2;
output_buffer[temp_ptr++] = 'T';
output_buffer[temp_ptr++] = sensor_get_type(s);
output_buffer[temp_ptr++] = 'l';
output_buffer[temp_ptr++] = sensor_get_loc(s);
output_buffer[temp_ptr++] = 't'; //time
write_time_to_string(&output_buffer[temp_ptr], sensor_get_end_time(s));
temp_ptr += STRING_TO_TIME_LENGTH;
output_buffer[temp_ptr++] = 'P'; //period
write_time_to_string(&output_buffer[temp_ptr], sensor_get_period(s));
temp_ptr += STRING_TO_TIME_LENGTH;
output_buffer[temp_ptr++] = 'C';
write_2_bytes_to_string(&output_buffer[temp_ptr], sensor_get_size(s));
temp_ptr += 2;
output_buffer[temp_ptr++] = 'D';
//data
memcpy(&output_buffer[temp_ptr], array, sensor_get_size(s)*sizeof(uint16_t));
temp_ptr += sensor_get_size(s) * 2;
output_buffer[temp_ptr++] = 'X'; // end
write_index = temp_ptr;
ret = SUCCESS;
break;
}
return ret;
}
void
_on_sensor_event(sensor_h sensor, sensor_event_s *event, void *user_data)
{
sensor_type_e type;
sensor_get_type(sensor, &type);
switch (type)
{
case SENSOR_ACCELEROMETER:
_send_values("accelerometer", event->values[0], event->values[1], event->values[2]);
//abort ();
break;
case SENSOR_GYROSCOPE:
_send_values("gyroscope", event->values[0], event->values[1], event->values[2]);
break;
default:
break;
}
}
uint8_t encode_data_for_transmit(node_ref args) {
uint8_t ret = FAILURE;
for(;;) {
if(!args) break;
sensor_ref s = (sensor_ref)node_get_val(args);
if(!s) break;
uint16_t cur_size = 0;
int16_t * array = sensor_get_data_array(s);
uint16_t temp_ptr = write_index;
cur_size = sensor_get_size(s)*sizeof(uint16_t) + HEADER_SIZE;
if(cur_size > OUTPUT_BUFFER_SIZE) return FAILURE; // Not ever going to fit
if(temp_ptr + cur_size > OUTPUT_BUFFER_SIZE - write_index) { //If the output_buffer has no space for msg then return failure.
// Cannot fit it
// temp_ptr = 0;
// read_index = 0;
// write_index = 0;
return FAILURE;
}
#ifdef DEBUG_ENCODE
test_buffer_encode[test_itor_encode] = s->end_time->milliseconds;
test_itor_encode++;
if (test_itor_encode == 99) {
return FAILURE;
}
#endif
output_buffer[temp_ptr++] = 'L';
write_2_bytes_to_string(&output_buffer[temp_ptr], cur_size);
temp_ptr += 2;
output_buffer[temp_ptr++] = 'T';
output_buffer[temp_ptr++] = sensor_get_type(s);
output_buffer[temp_ptr++] = 'l';
output_buffer[temp_ptr++] = sensor_get_loc(s);
output_buffer[temp_ptr++] = 't'; //time
write_time_to_string(&output_buffer[temp_ptr], sensor_get_end_time(s));
// write_count_to_string(&output_buffer[temp_ptr]);
temp_ptr += STRING_TO_TIME_LENGTH;
output_buffer[temp_ptr++] = 'P'; //period
write_time_to_string(&output_buffer[temp_ptr], sensor_get_period(s));
temp_ptr += STRING_TO_TIME_LENGTH;
output_buffer[temp_ptr++] = 'C';
write_2_bytes_to_string(&output_buffer[temp_ptr], sensor_get_size(s));
temp_ptr += 2;
output_buffer[temp_ptr++] = 'D';
//data
memcpy(&output_buffer[temp_ptr], array, sensor_get_size(s)*sizeof(uint16_t));
temp_ptr += sensor_get_size(s) * 2;
output_buffer[temp_ptr++] = 'X'; // end
write_index = temp_ptr;
ret = SUCCESS;
break;
}
return ret;
}
/* When the roving module receives a packet that starts with @, it passes the rest
* of the command to this function.
*/
uint8_t parse_shemp_command(uint8_t * string, uint16_t length) {
// TODO Make this entire function much more modular
uint16_t cur_index = 0;
if(string[cur_index] == 'W') {
// Wait
cur_index++;
uint16_t wait_ms = read_number_from_string(&string[cur_index], 4);
wait(wait_ms);
// And then clear output buffer
reset_output_buffer();
} else if(string[cur_index] == 'R') {
// Relay
cur_index++;
if (string[cur_index] == 'N') {
set_relay(ON);
} else if (string[cur_index] == 'F') {
set_relay(OFF);
}
cur_index++;
} else if (string[cur_index] == 'H') {
// Sync with header
cur_index++;
server_wants_header = TRUE;
okay_to_transmit = FALSE;
//transmit_header();
} else if(string[cur_index] == 'K') {
// Okay to transmit
cur_index++;
server_wants_header = FALSE;
okay_to_transmit = TRUE;
} else if(string[cur_index] == 'S') {
cur_index++;
// Sensor configuration
uint8_t sensor_loc = 0;
uint8_t sensor_type = 0;
uint8_t sensor_state = 0;
time_ref sensor_period = 0;
uint16_t sensor_size = 0;
uint8_t sensor_index = 0;
#define ENABLED 'E'
#define DISABLED 'D'
while(cur_index < length) {
if(string[cur_index] == 'L') {
cur_index++;
// Location
sensor_loc = string[cur_index];
cur_index++;
} else if(string[cur_index] == 'T') {
cur_index++;
// Type
sensor_type = string[cur_index];
cur_index++;
} else if(string[cur_index] == 's') {
cur_index++;
// State
if(string[cur_index] == 'E') {
cur_index++;
sensor_state = ENABLED;
} else if (string[cur_index] == 'D') {
cur_index++;
sensor_state = DISABLED;
}
} else if(string[cur_index] == 'P') {
cur_index++;
sensor_period = new_time_from_string(&string[cur_index]);
cur_index += STRING_TO_TIME_LENGTH;
} else if(string[cur_index] == 'C') {
cur_index++;
sensor_size = read_number_from_string(&string[cur_index], 5);
cur_index += 5;
}
}
// Command is parsed, now to determine how to do it
if (sensor_loc == 'I') {
// Internal, internal voltage, current, or wattage
if (sensor_type == 'W') {
// Wattage can be enabled and disabled and period changed
if (sensor_state == DISABLED) disable_internal_wattage_sensor();
else if (sensor_state == ENABLED) {
// Else it should be enabled
if (sensor_period && sensor_size) {
// We have all the needed information
create_internal_wattage_sensor(sensor_period, sensor_size);
wait(1); //This ensures that all 3 sensors involved are in sync
enable_internal_wattage_sensor();
}
}
} else if (sensor_type == 'V') {
// Handle internal voltage sensor
if (sensor_state == DISABLED) disable_internal_voltage_sensor();
else if (sensor_state == ENABLED) {
// Else it should be enabled
if (sensor_period && sensor_size) {
// We have all the needed information
create_internal_voltage_sensor(sensor_period, sensor_size);
wait(1);
enable_internal_voltage_sensor();
}
}
} else if (sensor_type == 'I') {
// Handle internal current sensor
if (sensor_state == DISABLED) disable_internal_current_sensor();
else if (sensor_state == ENABLED) {
// Else it should be enabled
if (sensor_period && sensor_size) {
// We have all the needed information
create_internal_current_sensor(sensor_period, sensor_size);
wait(1);
enable_internal_current_sensor();
}
}
} else if (sensor_type == 'T') {
// Handle internal temperature sensor
// It can be enabled, disabled, period and size changed
if (sensor_state == DISABLED) disable_internal_temperature_sensor();
else if (sensor_state == ENABLED) {
// Else it should be enabled
if (sensor_period && sensor_size) {
// We have all the needed information
create_temperature_sensor('I',sensor_period, sensor_size);
wait(1);
enable_internal_temperature_sensor();
}
}
}
} else if (sensor_loc == 'A' || sensor_loc == 'B') {
sensor_index = sensor_loc - 'A';
if(sensor_state == DISABLED) {
// Should disable that sensor if it matches
if(sensor_type == sensor_get_type(aux_sensor[sensor_index])) {
sensor_disable_this(aux_sensor[sensor_index]);
}
} else if (sensor_state == ENABLED) {
if(sensor_period && sensor_size) {
// We have everything we need, but maybe we are already set up, do we have to change anything?
if(sensor_type != sensor_get_type(aux_sensor[sensor_index]) || time_cmp(sensor_period, sensor_get_period(aux_sensor[sensor_index])) || sensor_size != sensor_get_size(aux_sensor[sensor_index])) {
// It is a different type
// Or a different period
// Or a different size
sensor_delete_this(aux_sensor[sensor_index]);
aux_sensor[sensor_index] = 0;
}
sensor_ref s;
if(sensor_type == 'T') {
// External Temperature Sensor
s = create_temperature_sensor(sensor_loc, sensor_period, sensor_size);
}
else if(sensor_type == 'A') {
// External Audio Sensor
s = create_audio_sensor(sensor_loc, sensor_period, sensor_size);
}
else if(sensor_type == 'L') {
// External Light Sensor
s = create_light_sensor(sensor_loc, sensor_period, sensor_size);
}
if(s) {
aux_sensor[sensor_index] = s;
if(aux_sensor_enabled[sensor_index]) sensor_enable_this(aux_sensor[sensor_index]);
}
}
}
}
time_delete(&sensor_period);
}
return SUCCESS;
}
