int make_ack_get_temp_status(u8*ackbuf)
{
u16 hum=0;
u16 temp=0;
char am2301_buf[AM2301_DATA_CNT];
get_sensor(am2301_buf);
printf_hex(am2301_buf,AM2301_DATA_CNT);
/*change ascii to num*/
hum = (am2301_buf[INDEX_H100]-0x30)*100 + (am2301_buf[INDEX_H10]-0x30)*10 + (am2301_buf[INDEX_H1]-0x30);
temp = (am2301_buf[INDEX_T100]-0x30)*100 + (am2301_buf[INDEX_T10]-0x30)*10 + (am2301_buf[INDEX_T1]-0x30);
make_ack_head( ackbuf, PROTOCOL_ACK_GET_TEMP_STATUS);
ackbuf[4] = TEMP_NUM;
ackbuf[5] = TEMP_NUM;
ackbuf[6] = hum/0x100;
ackbuf[7] = hum%0x100;
ackbuf[8] = temp/0x100;
ackbuf[9] = temp%0x100;
ackbuf[10]=make_crc_num(ackbuf, 10);
return 11;
}
int make_ack_get_sensor_data(u8* _buf)
{
make_ack_head( _buf, PROTOCOL_ACK_GET_SENSOR_DATA);
_buf[4]=buf[4];
_buf[5]=AM2301_DATA_CNT;
get_sensor(g_conf_info.con_gpio.sensor);
memcpy(_buf+6, g_conf_info.con_gpio.sensor, sizeof (g_conf_info.con_gpio.sensor));
_buf[5+30+1]=make_crc_num(_buf, 36);
return 36+1;
}
int OLD_SENSOR(const char *cmd, const char *param, unsigned flags, AGENT_RESULT *result)
{
char key[MAX_STRING_LEN];
int ret;
assert(result);
init_result(result);
if(num_param(param) > 1)
{
return SYSINFO_RET_FAIL;
}
if(get_param(param, 1, key, MAX_STRING_LEN) != 0)
{
return SYSINFO_RET_FAIL;
}
if(strcmp(key,"temp1") == 0)
{
ret = get_sensor("temp1", flags, result);
}
else if(strcmp(key,"temp2") == 0)
{
ret = get_sensor("temp2", flags, result);
}
else if(strcmp(key,"temp3") == 0)
{
ret = get_sensor("temp3", flags, result);
}
else
{
ret = SYSINFO_RET_FAIL;
}
return ret;
}
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;
}
void read_car_state(struct car* c) {
if(c->state == STATE_DRIVING_BLIND) {
recorder_read(&c->recorder, &c->sensor);
}
else {
get_sensor(&c->sensor);
// only write the first full lap (0 == partial lap)
if(c->laps == 1) {
recorder_write(&c->recorder, &c->sensor);
}
}
c->magnet = get_magnet_sensor();
if(c->sensor.left == SENSOR_BLACK) {
c->track_direction = DIRECTION_LEFT;
}
else if(c->sensor.right == SENSOR_BLACK) {
c->track_direction = DIRECTION_RIGHT;
}
}
//Hanterar de meddelanden som fås från styrenheten
//Det som sker till mesta del är konverteringar mellan tal till strängar eller tvärtom
//och förstås användandet av funktioner som var definierade i gyro.c och ultraljud.c
void message_handler(uart_message *message_in){
int ANGLE = 0;
double CALC_ANGLE = 0;
uint8_t c;
switch (get_cmd(message_in)){
case 1: //rotate
{
char *rot[1];
rot[0] = malloc(4);
send_message("accept", rotate, 1);
ANGLE = (int) strtol((*message_in).data[1].data, (char **)NULL, 10);
CALC_ANGLE = rotate_to(ANGLE);
snprintf(rot[0], 4, "%d", (int) CALC_ANGLE);
send_message("rotate", rot, 1);
free(rot[0]);
}
break;
case 2: //distance
c = (int) strtol((*message_in).data[1].data, (char **)NULL, 10);
if(c == 0){
send_message("accept", distance, 1);
char* attr[6];
int i; for(i = 0; i < 6; ++i){
attr[i] = malloc(4);
snprintf(attr[i], 4, "%u", get_sensor(i)->medDist);
}
send_message("distance", attr, 6);
for(i = 0; i < 6; ++i){
free(attr[i]);
}
}else if(c > 0 && c <= 6){
send_message("accept", distance, 1);
char* attr[2];
attr[0] = malloc(2);
attr[1] = malloc(4);
snprintf(attr[0], 2, "%u", c);
snprintf(attr[1], 4, "%u", get_sensor(c-1)->medDist);
send_message("distance", attr, 2);
free(attr[0]);
free(attr[1]);
}else if(c == 7){
send_message("accept", distance, 1);
refresh_sensors();
char* attr[6];
int i; for(i = 0; i < 6; ++i){
attr[i] = malloc(4);
snprintf(attr[i], 4, "%u", get_sensor(i)->medDist);
}
send_message("distance", attr, 6);
for(i = 0; i < 6; ++i){
free(attr[i]);
}
}else if(c == 8){
send_message("accept", distance, 1);
median_of_dists(3);
char* attr[6];
int i; for(i = 0; i < 6; ++i){
attr[i] = malloc(4);
snprintf(attr[i], 4, "%u", get_sensor(i)->medDist);
}
send_message("distance", attr, 6);
for(i = 0; i < 6; ++i){
free(attr[i]);
}
}else{
send_message("denied", distance, 1);
}
break;
case 3: //accept
switch (get_validation(message_in)){
case 0: //false
break;
case 1: //true
break;
default:
break;
}
break;
default:
returnMessage[0] = (*message_in).data[0].data;
send_message("denied", returnMessage, 1);
break;
}
}
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;
}
