void HwmonDriver::readEvents(mxml_node_t *const) {
int err = sensors_init(NULL);
if (err) {
logg->logMessage("Failed to initialize libsensors! (%d)", err);
return;
}
sensors_sysfs_no_scaling = 1;
int chip_nr = 0;
const sensors_chip_name *chip;
while ((chip = sensors_get_detected_chips(NULL, &chip_nr))) {
int feature_nr = 0;
const sensors_feature *feature;
while ((feature = sensors_get_features(chip, &feature_nr))) {
// Keep in sync with HwmonCounter::read
// Can this counter be read?
double value;
const sensors_subfeature *const subfeature = sensors_get_subfeature(chip, feature, getInput(feature->type));
if ((subfeature == NULL) || (sensors_get_value(chip, subfeature->number, &value) != 0)) {
continue;
}
// Get the name of the counter
int len = sensors_snprintf_chip_name(NULL, 0, chip) + 1;
char *chip_name = new char[len];
sensors_snprintf_chip_name(chip_name, len, chip);
len = snprintf(NULL, 0, "hwmon_%s_%d_%d", chip_name, chip_nr, feature->number) + 1;
char *const name = new char[len];
snprintf(name, len, "hwmon_%s_%d_%d", chip_name, chip_nr, feature->number);
delete [] chip_name;
setCounters(new HwmonCounter(getCounters(), name, chip, feature));
}
}
}
/* Return 0 on success, and an exit error code otherwise */
static int read_config_file(const char *config_file_name)
{
FILE *config_file;
int err;
if (config_file_name) {
if (!strcmp(config_file_name, "-"))
config_file = stdin;
else
config_file = fopen(config_file_name, "r");
if (!config_file) {
fprintf(stderr, "Could not open config file\n");
perror(config_file_name);
return 1;
}
} else {
/* Use libsensors default */
config_file = NULL;
}
err = sensors_init(config_file);
if (err) {
fprintf(stderr, "sensors_init: %s\n", sensors_strerror(err));
if (config_file)
fclose(config_file);
return 1;
}
if (config_file && fclose(config_file) == EOF)
perror(config_file_name);
return 0;
}
int main(void)
{
// Enable peripherals
system_init();
pins_init();
us1_init();
us0_init();
twi_init();
// Initialize state machines
qcfp_init();
eq_init();
// Once everything is initialized, enable interrupts globally
interrupts_enable();
// Enable Expansion module 3 and 4 (sensors plugged into these temporarily)
AT91C_BASE_PIOA->PIO_CODR = AT91C_PIO_PA20;
AT91C_BASE_PIOA->PIO_CODR = AT91C_PIO_PA23;
sensors_init();
eq_post_timer(gpio_led_dance, 250, eq_timer_periodic);
while(1)
{
eq_dispatch();
eq_dispatch_timers();
}
return 0;
}
int main(void)
{
_delay_ms(100);
LCD_Initalize();
LCD_WriteText("Anal Intruder 1");
LCD_GoTo(0, 1);
LCD_WriteText("8=======D (.)(.)");
pad_init();
motor_init();
brzeczyk_init();
sensors_init();
sei();
play(power, 11);
while(1)
{
while(!pad_get_state()) // sprawdzenie po³¹czenia z padem
{
m1_stop();
m2_stop();
LCD_Clear();
LCD_WriteText("Pad conn error!");
LCD_GoTo(0, 1);
LCD_WriteText("Reconnecting...");
_delay_ms(500);
pad_init();
tryb = 0;
lcd_old = -1;
motor_lcd = false;
}
lcd_tryb();
switch(tryb)
{
case 0:
if(!(tab[4] & (1 << 4))) // jeœli wciœniêty trojkat
tryb = 1;
else if(!(tab[4] & (1 << 5))) // jeœli wciœniête jest kolko
tryb = 2;
break;
case 1:
pad_loop();
break;
case 2:
sensors_loop();
break;
}
if(!(tab[3] & (1 << 3)))
{ // jeœli wciœniêty start
tryb = 0;
m1_stop();
m2_stop();
srednia = 0;
}
}
}
static J4statusPluginContext *
_j4status_sensors_init(J4statusCoreInterface *core)
{
gchar **sensors = NULL;
gboolean show_details = FALSE;
guint64 interval = 0;
if ( sensors_init(NULL) != 0 )
return NULL;
GKeyFile *key_file;
key_file = j4status_config_get_key_file("Sensors");
if ( key_file != NULL )
{
sensors = g_key_file_get_string_list(key_file, "Sensors", "Sensors", NULL, NULL);
show_details = g_key_file_get_boolean(key_file, "Sensors", "ShowDetails", NULL);
interval = g_key_file_get_uint64(key_file, "Sensors", "Interval", NULL);
g_key_file_free(key_file);
}
J4statusPluginContext *context;
context = g_new0(J4statusPluginContext, 1);
context->core = core;
context->config.show_details = show_details;
if ( sensors == NULL )
_j4status_sensors_add_sensors(context, NULL);
else
{
sensors_chip_name chip;
gchar **sensor;
for ( sensor = sensors ; *sensor != NULL ; ++sensor )
{
if ( sensors_parse_chip_name(*sensor, &chip) != 0 )
continue;
_j4status_sensors_add_sensors(context, &chip);
sensors_free_chip_name(&chip);
}
}
g_strfreev(sensors);
if ( context->sections == NULL )
{
g_message("Missing configuration: No sensor to monitor, aborting");
_j4status_sensors_uninit(context);
return NULL;
}
g_timeout_add_seconds(MAX(2, interval), _j4status_sensors_update, context);
return context;
}
jstring Java_com_cloudminds_smartrobot_fragment_HomeFragment_sensorsInit(JNIEnv* env,
jobject thiz) {
set_axes_hz(99.0);
int ret = 0;
struct version ver;
ret = get_version(&ver);
struct axes_data naxes;
ret = sensors_init("/dev/ttyACM0");
set_axes_hz(99.0);
return (*env)->NewStringUTF(env, "sensorsInit");
}
void setup()
{
Serial.begin(9600);
lcd.begin(16, 2);
detect_serial();
movement_init();
sensors_init();
if (COUNTDOWN_ENABLED) {
calibrate_countdown();
}
path();
}
void hi_module_init(void)
{
if (!g_file_test("/usr/share/misc/pci.ids", G_FILE_TEST_EXISTS)) {
static SyncEntry se = {
.fancy_name = N_("Update PCI ID listing"),
.name = "GetPCIIds",
.save_to = "pci.ids",
.get_data = NULL
};
sync_manager_add_entry(&se);
}
#if defined(ARCH_x86) || defined(ARCH_x86_64)
{
static SyncEntry se = {
.fancy_name = N_("Update CPU feature database"),
.name = "RecvCPUFlags",
.save_to = "cpuflags.conf",
.get_data = NULL
};
sync_manager_add_entry(&se);
}
#endif /* defined(ARCH_x86) */
init_memory_labels();
init_cups();
sensors_init();
}
void hi_module_deinit(void)
{
moreinfo_del_with_prefix("DEV");
sensors_shutdown();
g_hash_table_destroy(memlabels);
g_module_close(cups);
}
ModuleAbout *hi_module_get_about(void)
{
static ModuleAbout ma[] = {
{
.author = "Leandro A. F. Pereira",
.description = N_("Gathers information about hardware devices"),
.version = VERSION,
.license = "GNU GPL version 2"}
};
return ma;
}
/**
* Initialize internal object arrays and display lmsensors HUD help.
* \param displayhelp true if the list of detected devices should be
displayed on the console.
* \return number of detected lmsensor devices.
*/
int
hud_get_num_sensors(bool displayhelp)
{
/* Return the number of sensors detected. */
mtx_lock(&gsensor_temp_mutex);
if (gsensors_temp_count) {
mtx_unlock(&gsensor_temp_mutex);
return gsensors_temp_count;
}
int ret = sensors_init(NULL);
if (ret) {
mtx_unlock(&gsensor_temp_mutex);
return 0;
}
list_inithead(&gsensors_temp_list);
/* Scan /sys/block, for every object type we support, create and
* persist an object to represent its different statistics.
*/
build_sensor_list();
if (displayhelp) {
list_for_each_entry(struct sensors_temp_info, sti, &gsensors_temp_list, list) {
char line[64];
switch (sti->mode) {
case SENSORS_TEMP_CURRENT:
snprintf(line, sizeof(line), " sensors_temp_cu-%s", sti->name);
break;
case SENSORS_TEMP_CRITICAL:
snprintf(line, sizeof(line), " sensors_temp_cr-%s", sti->name);
break;
case SENSORS_VOLTAGE_CURRENT:
snprintf(line, sizeof(line), " sensors_volt_cu-%s", sti->name);
break;
case SENSORS_CURRENT_CURRENT:
snprintf(line, sizeof(line), " sensors_curr_cu-%s", sti->name);
break;
case SENSORS_POWER_CURRENT:
snprintf(line, sizeof(line), " sensors_pow_cu-%s", sti->name);
break;
}
puts(line);
}
}
mtx_unlock(&gsensor_temp_mutex);
return gsensors_temp_count;
}
void mppt_init()
{
battery_amps = 0;
battery_volt = 0;
source_amps = 0;
source_volt = 0;
battery_watts = 0;
source_watts = 0;
state = on;
sensors_init();
pwm_init();
pwm_period(PWM_PERIOD);
}
LMSensorList::LMSensorList()
{
sensors_init(NULL);
int sensor_i = 0;
const sensors_chip_name *sc;
while ((sc = sensors_get_detected_chips(NULL, &sensor_i))) {
int feature_i = 0;
const sensors_feature *sf;
while ((sf = sensors_get_features(sc, &feature_i)))
{
LMSensor *lms = new LMSensor(sc, sf);
m_sensors.push_back(lms);
}
}
}
static struct temperature* temperature_init(void)
{
int chip_nr = 0;
struct temperature* temp = 0;
if (sensors_init(0))
{
fprintf(stderr, u8"Could not initialize libsensors.\n");
return 0;
}
temp = calloc(1, sizeof(struct temperature));
if (!temp)
{
fprintf(stderr, u8"Could not allocate memory for temperature struct.\n");
return 0;
}
while ((temp->chip = sensors_get_detected_chips(0, &chip_nr)))
{
if (!strcmp(chip_name, temp->chip->prefix))
{
int feature_nr = 0;
sensors_feature const* feat = 0;
while ((feat = sensors_get_features(temp->chip, &feature_nr)))
{
if (feat->type == SENSORS_FEATURE_TEMP)
{
sensors_subfeature const* sub = sensors_get_subfeature(temp->chip, feat, SENSORS_SUBFEATURE_TEMP_INPUT);
if (!sub)
{
fprintf(stderr, u8"Could not get chip subfeature.\n");
return 0;
}
++temp->count;
temp->numbers = realloc(temp->numbers, temp->count * sizeof(int));
temp->numbers[temp->count - 1] = sub->number;
}
}
return temp;
}
}
return 0;
}
/**
* initialize - Performs all initialization procedures
*
* Returns a CHARLIE_STAT_* constant that indicates the succes or
* failure of the initialization.
*/
static uint8_t initialize(void)
{
charlie.shutdown = FALSE;
init_atmega();
init_ports();
debug_init_ports();
rs232_init();
spi_init_master();
i2c_init();
if (rtc_init()) {
return CHARLIE_STAT_RTC_ERROR;
}
time_init();
time_sync_to_realtime();
sched_init();
if (card_init()) {
return CHARLIE_STAT_CARD_ERROR;
}
if (fsys_init()) {
return CHARLIE_STAT_FSYS_ERROR;
}
if (fsys_check_read_only()) {
return CHARLIE_STAT_FSYS_READONLY;
}
cfg_load();
if (enc28j60_init(&cfg.mac_addr)) {
return CHARLIE_STAT_NET_ERROR;
}
net_init();
adc_init();
sensors_init();
pump_init();
plants_init();
sei();
return CHARLIE_STAT_OK;
}
/* sensors 线程主体 */
void thread_sensors(void)
{
//传感器初始化
sensors_init();
//三个传感器的子线程
// thread_air_press_init();
// thread_hmc_init();
// thread_mpu_init();
//主循环
while(1)
{
// send_raw_data();
rt_thread_delay(20);
}
}
static int l_readAll(lua_State *L) {
sensors_init(NULL);
const sensors_chip_name *scn;
const sensors_feature *sf;
const sensors_subfeature *ss;
int n, n1, n2, err;
double r;
char scns[80];
lua_newtable(L);
//Push new table on to stack. Index -1
for(n = 0; (scn = sensors_get_detected_chips(NULL, &n)) != NULL; ) {
// Iterate over detected chips
for(n1 = 0; (sf = sensors_get_features(scn, &n1)) != NULL; )
// Iterate over features of chip
for(n2 = 0; (ss = sensors_get_all_subfeatures(scn, sf, &n2)) != NULL; ) {
// Iterate over subfeatures of features from chip
sprintf(scns, "%s-%x-%x__%s", scn->prefix, scn->bus.nr, scn->addr, ss->name);
// Set scns to (chip.prefix - chip.bus.nr - chip.addr __ subfeature.name )
err = sensors_get_value(scn, ss->number, &r);
// Get value from chip using subfeature.number, store result in r. If err != 0 then an error occured.
if (err == 0) {
// Sensor data. Addres in char* scns, name in ss->name, result in r
lua_pushnumber(L, r); //Push table value on stack. Table is now at -2
lua_setfield(L, -2, scns); //Set 3rd argument as key in table at stack index -2 to stack index -1
} else {
return(luaL_error(L, "Can't read sensors!"));
}
}
}
sensors_cleanup();
return(1);
}
int main (int argc, char **argv) {
int i1 = 0, i2 = 0, i3 = 0, iter, err;
const sensors_chip_name *sens;
const sensors_feature_data *sfd;
char *label;
double res;
FILE *foo;
if (!(foo = fopen(CONF_FILE, "r"))) {
printf ("Zoinks! Couldn't open conf file. Bailing out.\n");
exit(1);
}
if (sensors_init(foo)) {
printf ("Were in trouble! sensors_init returned nonzero\n");
exit(1);
}
while (sens = sensors_get_detected_chips(&i1)) {
printf("Chip: %s-%#x-%#x\n", sens->prefix, sens->bus, sens->addr);
printf("Features:\n");
i2 = i3 = 0;
while (sfd = sensors_get_all_features(*sens, &i2, &i3)) {
printf("*************************\n");
printf("\tName: %s\n", sfd->name);
printf("\tNumber: %d\n", sfd->number);
printf("\tMapping: %d\n", sfd->mapping);
printf("\tUnused: %d\n", sfd->unused);
printf("\tMode: %d\n", sfd->mode);
if ((sfd->mode & SENSORS_MODE_R) &&
!(err = sensors_get_feature(*sens, sfd->number, &res))) {
printf("\tValue: %f\n", res);
}
else if (err) {
printf("Error reading sensor: %d\n", err);
}
}
printf("*************************\n\n");
}
return 0;
}
/*
* This function is called once right after the controller starts up. This
* is where the initialization functions of the different libraries are called.
*/
void setup() {
lcd_init();
sensors_init();
// This one doesn't only set the interrupt handler, it also configures
// the IO pins used for the wake-up button and for switching power to
// the LCD backlight. I simply had no idea how to name that one.
interrupt_init();
// Switch off the internal LED on pin13/PB7 in order to save power. That
// probably doesn't help much, but every mA counts. :)
DDRB |= _BV(PB7);
PORTB &= ~_BV(PB7);
// Switch on the LCD and print the greeting line. Actually it doesn't only
// control the backlight but also the contrast.
lcd_light_state(1);
lcd_home();
lcd_string("Started! ");
}
static int
sensor_init(void)
{
#ifdef solaris2
clock_t t = time(NULL);
#else
int res;
char filename[] = CONFIG_FILE_NAME;
clock_t t = clock();
FILE *fp = fopen(filename, "r");
if (!fp)
return 1;
if (res = sensors_init(fp))
return 2;
_sensor_load(t); /* I'll let the linux people decide whether they want to load right away */
#endif
return 0;
}
static int
sensor_init(void)
{
int res;
#ifndef solaris2
char filename[] = CONFIG_FILE_NAME;
time_t t = time(NULL);
FILE *fp = fopen(filename, "r");
int i = 0;
DEBUGMSG(("ucd-snmp/lmSensors", "=> sensor_init\n"));
for (i = 0; i < N_TYPES; i++)
{
sensor_array[i].n = 0;
sensor_array[i].current_len = 0;
sensor_array[i].sensor = NULL;
}
if (!fp)
{
res = 1;
goto leaving;
}
if (sensors_init(fp))
{
res = 2;
goto leaving;
}
_sensor_load(t); /* I'll let the linux people decide whether they want to load right away */
leaving:
#endif /* not solaris2 */
DEBUGMSG(("ucd-snmp/lmSensors", "<= sensor_init\n"));
return res;
}
void Hwmon::setup() {
// hwmon does not currently work with perf
if (gSessionData->perf.isSetup()) {
return;
}
int err = sensors_init(NULL);
if (err) {
logg->logMessage("Failed to initialize libsensors! (%d)", err);
return;
}
sensors_sysfs_no_scaling = 1;
int chip_nr = 0;
const sensors_chip_name *chip;
while ((chip = sensors_get_detected_chips(NULL, &chip_nr))) {
int feature_nr = 0;
const sensors_feature *feature;
while ((feature = sensors_get_features(chip, &feature_nr))) {
counters = new HwmonCounter(counters, chip, feature);
}
}
}
unsigned int get_sensor_num(void)
{
int a, b, c, num;
const sensors_chip_name * chip;
const sensors_feature * features;
const sensors_subfeature * subfeatures;
a = num = 0;
sensors_init(NULL);
while ((chip = sensors_get_detected_chips(NULL, &a)))
{
b = 0;
while ((features = sensors_get_features(chip, &b)))
{
c = 0;
while ((subfeatures = sensors_get_all_subfeatures(chip, features, &c)))
{
if (subfeatures->type == SENSORS_SUBFEATURE_FAN_INPUT)
{
// chip = chip->addr
// sensor = features->number
num++; break;
}
if (subfeatures->type != SENSORS_SUBFEATURE_TEMP_INPUT)
{
num++; break;
}
}
}
}
return num;
}
void main(void)
{
uint8_t register i = 0;
int state = 1;
sensors_init();
third_board_init();
uint8_t nbr;
while (1)
{
read_line_sens(&line_sens);
read_enemy_sens(&enemy_sens);
third_board_set(1, LINE(1)); //pui ce index vrei, maparea cu porturile o gasesti in sensors.c
// third_board_set(2, LINE(0)); // daca vrei sa testezi sens de linie, aceeasi treaba ca mai sus
//incearca si asta, ar trebui sa-ti arate pe toate ledurile
//third_board_all(ENEMY(2));
#ifdef LOG
log_enemy(enemy_sens);
log_line(line_sens);
#endif
}
return (0);
}
void netsnmp_sensor_arch_init( void ) {
FILE *fp = fopen("/etc/sensors.conf", "r");
DEBUGMSGTL(("sensors:arch", "Initialise v3 LM Sensors module\n"));
sensors_init( fp );
}
int init_module(void)
{
return sensors_init();
}
int main() {
int i;
HTS221 hts221;
pc.baud(115200);
void hts221_init(void);
// Set LED to RED until init finishes
SetLedColor(0x1);
pc.printf(BLU "Hello World from AT&T Shape!\r\n\n\r");
pc.printf(GRN "Initialize the HTS221\n\r");
i = hts221.begin();
if( i )
pc.printf(BLU "HTS221 Detected! (0x%02X)\n\r",i);
else
pc.printf(RED "HTS221 NOT DETECTED!!\n\r");
printf("Temp is: %0.2f F \n\r",CTOF(hts221.readTemperature()));
printf("Humid is: %02d %%\n\r",hts221.readHumidity());
sensors_init();
read_sensors();
// Initialize the modem
printf(GRN "Modem initializing... will take up to 60 seconds" DEF "\r\n");
do {
i=mdm_init();
if (!i) {
pc.printf(RED "Modem initialization failed!" DEF "\n");
}
} while (!i);
//Software init
software_init_mdm();
// Resolve URL to IP address to connect to
resolve_mdm();
//Create a 1ms timer tick function:
OneMsTicker.attach(OneMsFunction, 0.001f) ;
iTimer1Interval_ms = SENSOR_UPDATE_INTERVAL_MS;
// Open the socket (connect to the server)
sockopen_mdm();
// Set LED BLUE for partial init
SetLedColor(0x4);
// Send and receive data perpetually
while(1) {
static unsigned ledOnce = 0;
if (bTimerExpiredFlag)
{
bTimerExpiredFlag = false;
sprintf(SENSOR_DATA.Temperature, "%0.2f", CTOF(hts221.readTemperature()));
sprintf(SENSOR_DATA.Humidity, "%02d", hts221.readHumidity());
read_sensors(); //read available external sensors from a PMOD and the on-board motion sensor
char modem_string[512];
GenerateModemString(&modem_string[0]);
printf(BLU "Sending to modem : %s" DEF "\n", modem_string);
sockwrite_mdm(modem_string);
sockread_mdm(&MySocketData, 1024, 20);
// If any non-zero response from server, make it GREEN one-time
// then the actual FLOW responses will set the color.
if ((!ledOnce) && (MySocketData.length() > 0))
{
ledOnce = 1;
SetLedColor(0x2);
}
printf(BLU "Read back : %s" DEF "\n", &MySocketData[0]);
char * myJsonResponse;
if (extract_JSON(&MySocketData[0], &myJsonResponse[0]))
{
printf(GRN "JSON : %s" DEF "\n", &myJsonResponse[0]);
parse_JSON(&myJsonResponse[0]);
}
else
{
printf(RED "JSON : %s" DEF "\n", &myJsonResponse[0]); //most likely an incomplete JSON string
parse_JSON(&myJsonResponse[0]); //This is risky, as the string may be corrupted
}
} //bTimerExpiredFlag
} //forever loop
}
static int sensors_load_conf (void)
{
static int call_once = 0;
FILE *fh = NULL;
featurelist_t *last_feature = NULL;
const sensors_chip_name *chip;
int chip_num;
int status;
if (call_once)
return 0;
call_once = 1;
if (conffile != NULL)
{
fh = fopen (conffile, "r");
if (fh == NULL)
{
char errbuf[1024];
ERROR ("sensors plugin: fopen(%s) failed: %s", conffile,
sstrerror (errno, errbuf, sizeof (errbuf)));
return (-1);
}
}
status = sensors_init (fh);
if (fh)
fclose (fh);
if (status != 0)
{
ERROR ("sensors plugin: Cannot initialize sensors. "
"Data will not be collected.");
return (-1);
}
#if SENSORS_API_VERSION < 0x400
chip_num = 0;
while ((chip = sensors_get_detected_chips (&chip_num)) != NULL)
{
int feature_num0 = 0;
int feature_num1 = 0;
while (42)
{
const sensors_feature_data *feature;
int feature_type;
featurelist_t *fl;
feature = sensors_get_all_features (*chip,
&feature_num0, &feature_num1);
/* Check if all features have been read. */
if (feature == NULL)
break;
/* "master features" only */
if (feature->mapping != SENSORS_NO_MAPPING)
{
DEBUG ("sensors plugin: sensors_load_conf: "
"Ignoring subfeature `%s', "
"because (feature->mapping "
"!= SENSORS_NO_MAPPING).",
feature->name);
continue;
}
/* skip ignored in sensors.conf */
if (sensors_get_ignored (*chip, feature->number) == 0)
{
DEBUG ("sensors plugin: sensors_load_conf: "
"Ignoring subfeature `%s', "
"because "
"`sensors_get_ignored' told "
"me so.",
feature->name);
continue;
}
feature_type = sensors_feature_name_to_type (
feature->name);
if (feature_type == SENSOR_TYPE_UNKNOWN)
{
DEBUG ("sensors plugin: sensors_load_conf: "
"Ignoring subfeature `%s', "
"because its type is "
"unknown.",
feature->name);
continue;
}
fl = calloc (1, sizeof (*fl));
if (fl == NULL)
{
ERROR ("sensors plugin: calloc failed.");
continue;
}
fl->chip = chip;
fl->data = feature;
fl->type = feature_type;
if (first_feature == NULL)
first_feature = fl;
else
last_feature->next = fl;
last_feature = fl;
} /* while sensors_get_all_features */
} /* while sensors_get_detected_chips */
/* #endif SENSORS_API_VERSION < 0x400 */
#elif (SENSORS_API_VERSION >= 0x400) && (SENSORS_API_VERSION < 0x500)
chip_num = 0;
while ((chip = sensors_get_detected_chips (NULL, &chip_num)) != NULL)
{
const sensors_feature *feature;
int feature_num = 0;
while ((feature = sensors_get_features (chip, &feature_num)) != NULL)
{
const sensors_subfeature *subfeature;
int subfeature_num = 0;
/* Only handle voltage, fanspeeds and temperatures */
if ((feature->type != SENSORS_FEATURE_IN)
&& (feature->type != SENSORS_FEATURE_FAN)
&& (feature->type != SENSORS_FEATURE_TEMP)
&& (feature->type != SENSORS_FEATURE_POWER))
{
DEBUG ("sensors plugin: sensors_load_conf: "
"Ignoring feature `%s', "
"because its type is not "
"supported.", feature->name);
continue;
}
while ((subfeature = sensors_get_all_subfeatures (chip,
feature, &subfeature_num)) != NULL)
{
featurelist_t *fl;
if ((subfeature->type != SENSORS_SUBFEATURE_IN_INPUT)
&& (subfeature->type != SENSORS_SUBFEATURE_FAN_INPUT)
&& (subfeature->type != SENSORS_SUBFEATURE_TEMP_INPUT)
&& (subfeature->type != SENSORS_SUBFEATURE_POWER_INPUT))
continue;
fl = calloc (1, sizeof (*fl));
if (fl == NULL)
{
ERROR ("sensors plugin: calloc failed.");
continue;
}
fl->chip = chip;
fl->feature = feature;
fl->subfeature = subfeature;
if (first_feature == NULL)
first_feature = fl;
else
last_feature->next = fl;
last_feature = fl;
} /* while (subfeature) */
} /* while (feature) */
} /* while (chip) */
#endif /* (SENSORS_API_VERSION >= 0x400) && (SENSORS_API_VERSION < 0x500) */
if (first_feature == NULL)
{
sensors_cleanup ();
INFO ("sensors plugin: lm_sensors reports no "
"features. Data will not be collected.");
return (-1);
}
return (0);
} /* int sensors_load_conf */
int sensors_main(int argc, char *argv[])
{
/* inform about start */
printf("[sensors] Initializing..\n");
fflush(stdout);
int ret = OK;
/* start sensor reading */
if (sensors_init() != OK) {
fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n");
/* Clean up */
close(fd_gyro);
close(fd_accelerometer);
close(fd_magnetometer);
close(fd_barometer);
close(fd_adc);
fprintf(stderr, "[sensors] rebooting system.\n");
fflush(stderr);
fflush(stdout);
usleep(100000);
/* Sensors are critical, immediately reboot system on failure */
reboot();
/* Not ever reaching here */
} else {
/* flush stdout from init routine */
fflush(stdout);
}
bool gyro_healthy = false;
bool acc_healthy = false;
bool magn_healthy = false;
bool baro_healthy = false;
bool adc_healthy = false;
bool hil_enabled = false; /**< HIL is disabled by default */
bool publishing = false; /**< the app is not publishing by default, only if HIL is disabled on first run */
int magcounter = 0;
int barocounter = 0;
int adccounter = 0;
unsigned int mag_fail_count = 0;
unsigned int mag_success_count = 0;
unsigned int baro_fail_count = 0;
unsigned int baro_success_count = 0;
unsigned int gyro_fail_count = 0;
unsigned int gyro_success_count = 0;
unsigned int acc_fail_count = 0;
unsigned int acc_success_count = 0;
unsigned int adc_fail_count = 0;
unsigned int adc_success_count = 0;
ssize_t ret_gyro;
ssize_t ret_accelerometer;
ssize_t ret_magnetometer;
ssize_t ret_barometer;
ssize_t ret_adc;
int nsamples_adc;
int16_t buf_gyro[3];
int16_t buf_accelerometer[3];
int16_t buf_magnetometer[7];
float buf_barometer[3];
int16_t mag_offset[3] = {0, 0, 0};
int16_t acc_offset[3] = {0, 0, 0};
int16_t gyro_offset[3] = {0, 0, 0};
bool mag_calibration_enabled = false;
#pragma pack(push,1)
struct adc_msg4_s {
uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */
int32_t am_data1; /**< ADC convert result 1 (4 bytes) */
uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */
int32_t am_data2; /**< ADC convert result 2 (4 bytes) */
uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */
int32_t am_data3; /**< ADC convert result 3 (4 bytes) */
uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */
int32_t am_data4; /**< ADC convert result 4 (4 bytes) */
};
#pragma pack(pop)
struct adc_msg4_s buf_adc;
size_t adc_readsize = 1 * sizeof(struct adc_msg4_s);
float battery_voltage_conversion;
battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION];
if (-1 == (int)battery_voltage_conversion) {
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f;
}
#ifdef CONFIG_HRT_PPM
int ppmcounter = 0;
#endif
/* initialize to 100 to execute immediately */
int paramcounter = 100;
int excessive_readout_time_counter = 0;
int read_loop_counter = 0;
/* Empty sensor buffers, avoid junk values */
/* Read first two values of each sensor into void */
(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro));
(void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
(void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer));
struct sensor_combined_s raw = {
.timestamp = hrt_absolute_time(),
.gyro_raw = {buf_gyro[0], buf_gyro[1], buf_gyro[2]},
.gyro_raw_counter = 0,
.gyro_rad_s = {0, 0, 0},
.accelerometer_raw = {buf_accelerometer[0], buf_accelerometer[1], buf_accelerometer[2]},
.accelerometer_raw_counter = 0,
.accelerometer_m_s2 = {0, 0, 0},
.magnetometer_raw = {buf_magnetometer[0], buf_magnetometer[1], buf_magnetometer[2]},
.magnetometer_raw_counter = 0,
.baro_pres_mbar = 0,
.baro_alt_meter = 0,
.baro_temp_celcius = 0,
.battery_voltage_v = BAT_VOL_INITIAL,
.adc_voltage_v = {0, 0 , 0},
.baro_raw_counter = 0,
.battery_voltage_counter = 0,
.battery_voltage_valid = false,
};
/* condition to wait for */
pthread_mutex_init(&sensors_read_ready_mutex, NULL);
pthread_cond_init(&sensors_read_ready, NULL);
/* advertise the topic and make the initial publication */
sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
publishing = true;
/* advertise the rc topic */
struct rc_channels_s rc;
memset(&rc, 0, sizeof(rc));
int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
/* subscribe to system status */
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
printf("[sensors] rate: %u Hz\n", (unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC));
struct hrt_call sensors_hrt_call;
/* Enable high resolution timer callback to unblock main thread, run after 2 ms */
hrt_call_every(&sensors_hrt_call, 2000, SENSOR_INTERVAL_MICROSEC, &sensors_timer_loop, NULL);
while (1) {
pthread_mutex_lock(&sensors_read_ready_mutex);
struct timespec time_to_wait = {0, 0};
/* Wait 2 seconds until timeout */
time_to_wait.tv_nsec = 0;
time_to_wait.tv_sec = time(NULL) + 2;
if (pthread_cond_timedwait(&sensors_read_ready, &sensors_read_ready_mutex, &time_to_wait) == OK) {
pthread_mutex_unlock(&sensors_read_ready_mutex);
bool gyro_updated = false;
bool acc_updated = false;
bool magn_updated = false;
bool baro_updated = false;
bool adc_updated = false;
/* store the time closest to all measurements */
uint64_t current_time = hrt_absolute_time();
raw.timestamp = current_time;
if (paramcounter == 100) {
// XXX paramcounter is not a good name, rename / restructure
// XXX make counter ticks dependent on update rate of sensor main loop
/* Check HIL state */
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* switching from non-HIL to HIL mode */
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
hil_enabled = true;
publishing = false;
int ret = close(sensor_pub);
printf("[sensors] Closing sensor pub: %i \n", ret);
/* switching from HIL to non-HIL mode */
} else if (!publishing && !hil_enabled) {
/* advertise the topic and make the initial publication */
sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
hil_enabled = false;
publishing = true;
}
/* Update RC scalings and function mappings */
rc.chan[0].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC1_REV]);
rc.chan[0].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM];
rc.chan[1].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC2_REV]);
rc.chan[1].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM];
rc.chan[2].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC3_REV]);
rc.chan[2].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM];
rc.chan[3].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC4_REV]);
rc.chan[3].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM];
rc.chan[4].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2)
* global_data_parameter_storage->pm.param_values[PARAM_RC5_REV]);
rc.chan[4].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM];
rc.function[0] = global_data_parameter_storage->pm.param_values[PARAM_THROTTLE_CHAN] - 1;
rc.function[1] = global_data_parameter_storage->pm.param_values[PARAM_ROLL_CHAN] - 1;
rc.function[2] = global_data_parameter_storage->pm.param_values[PARAM_PITCH_CHAN] - 1;
rc.function[3] = global_data_parameter_storage->pm.param_values[PARAM_YAW_CHAN] - 1;
rc.function[4] = global_data_parameter_storage->pm.param_values[PARAM_OVERRIDE_CHAN] - 1;
gyro_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET];
gyro_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET];
gyro_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET];
mag_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET];
mag_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET];
mag_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET];
paramcounter = 0;
}
paramcounter++;
/* try reading gyro */
uint64_t start_gyro = hrt_absolute_time();
ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro));
int gyrotime = hrt_absolute_time() - start_gyro;
if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime);
/* GYROSCOPE */
if (ret_gyro != sizeof(buf_gyro)) {
gyro_fail_count++;
if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&gyro_present_enabled);
gyro_healthy = false;
gyro_success_count = 0;
}
} else {
gyro_success_count++;
if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&gyro_present_enabled_healthy);
gyro_healthy = true;
gyro_fail_count = 0;
}
gyro_updated = true;
}
gyrotime = hrt_absolute_time() - start_gyro;
if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
/* try reading acc */
uint64_t start_acc = hrt_absolute_time();
ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
/* ACCELEROMETER */
if (ret_accelerometer != sizeof(buf_accelerometer)) {
acc_fail_count++;
if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) {
fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&acc_present_enabled);
gyro_healthy = false;
acc_success_count = 0;
}
} else {
acc_success_count++;
if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&acc_present_enabled_healthy);
acc_healthy = true;
acc_fail_count = 0;
}
acc_updated = true;
}
int acctime = hrt_absolute_time() - start_acc;
if (acctime > 500) printf("ACC: %d us\n", acctime);
/* MAGNETOMETER */
if (magcounter == 4) { /* 120 Hz */
uint64_t start_mag = hrt_absolute_time();
/* start calibration mode if requested */
if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) {
ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0);
printf("[sensors] enabling mag calibration mode\n");
mag_calibration_enabled = true;
} else if (mag_calibration_enabled && !vstatus.preflight_mag_calibration) {
ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0);
printf("[sensors] disabling mag calibration mode\n");
mag_calibration_enabled = false;
}
ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
int errcode_mag = (int) * get_errno_ptr();
int magtime = hrt_absolute_time() - start_mag;
if (magtime > 2000) {
printf("MAG (pure read): %d us\n", magtime);
}
if (ret_magnetometer != sizeof(buf_magnetometer)) {
mag_fail_count++;
if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) {
fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (magn_healthy && mag_fail_count >= MAGN_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&magn_present_enabled);
magn_healthy = false;
mag_success_count = 0;
}
} else {
mag_success_count++;
if (!magn_healthy && mag_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&magn_present_enabled_healthy);
magn_healthy = true;
mag_fail_count = 0;
}
magn_updated = true;
}
magtime = hrt_absolute_time() - start_mag;
if (magtime > 2000) {
printf("MAG (overall time): %d us\n", magtime);
fprintf(stderr, "[sensors] TIMEOUT HMC5883L ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag));
}
magcounter = 0;
}
magcounter++;
/* BAROMETER */
if (barocounter == 5 && (fd_barometer > 0)) { /* 100 Hz */
uint64_t start_baro = hrt_absolute_time();
*get_errno_ptr() = 0;
ret_barometer = read(fd_barometer, buf_barometer, sizeof(buf_barometer));
if (ret_barometer != sizeof(buf_barometer)) {
baro_fail_count++;
if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (baro_healthy && baro_fail_count >= BARO_HEALTH_COUNTER_LIMIT_ERROR) {
/* switched from healthy to unhealthy */
baro_healthy = false;
baro_success_count = 0;
// global_data_send_subsystem_info(&baro_present_enabled);
}
} else {
baro_success_count++;
if (!baro_healthy && baro_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) {
/* switched from unhealthy to healthy */
baro_healthy = true;
baro_fail_count = 0;
// global_data_send_subsystem_info(&baro_present_enabled_healthy);
}
baro_updated = true;
}
barocounter = 0;
int barotime = hrt_absolute_time() - start_baro;
if (barotime > 2000) printf("BARO: %d us\n", barotime);
}
barocounter++;
/* ADC */
if (adccounter == 5) {
ret_adc = read(fd_adc, &buf_adc, adc_readsize);
nsamples_adc = ret_adc / sizeof(struct adc_msg_s);
if (ret_adc < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) {
adc_fail_count++;
if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (adc_healthy && adc_fail_count >= ADC_HEALTH_COUNTER_LIMIT_ERROR) {
adc_healthy = false;
adc_success_count = 0;
}
} else {
adc_success_count++;
if (!adc_healthy && adc_success_count >= ADC_HEALTH_COUNTER_LIMIT_OK) {
adc_healthy = true;
adc_fail_count = 0;
}
adc_updated = true;
}
adccounter = 0;
}
adccounter++;
#ifdef CONFIG_HRT_PPM
bool ppm_updated = false;
/* PPM */
if (ppmcounter == 5) {
/* require at least two channels
* to consider the signal valid
*/
if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) {
/* Read out values from HRT */
for (int i = 0; i < ppm_decoded_channels; i++) {
rc.chan[i].raw = ppm_buffer[i];
/* Set the range to +-, then scale up */
rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor;
}
rc.chan_count = ppm_decoded_channels;
rc.timestamp = ppm_last_valid_decode;
/* publish a few lines of code later if set to true */
ppm_updated = true;
}
ppmcounter = 0;
}
ppmcounter++;
#endif
/* Copy values of gyro, acc, magnetometer & barometer */
/* GYROSCOPE */
if (gyro_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32767 : buf_gyro[1]); // x of the board is y of the sensor
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor
raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32767 : buf_gyro[2]); // z of the board is -z of the sensor
/* scale measurements */
// XXX request scaling from driver instead of hardcoding it
/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f;
raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f;
raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f;
raw.gyro_raw_counter++;
}
/* ACCELEROMETER */
if (acc_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor
raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor
// XXX read range from sensor
float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_raw_counter++;
}
/* MAGNETOMETER */
if (magn_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : -buf_magnetometer[1]; // x of the board is -y of the sensor
raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? -32767 : buf_magnetometer[0]; // y on the board is x of the sensor
raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32767 : buf_magnetometer[2]; // z of the board is z of the sensor
// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here
raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - mag_offset[0]) / 4096.0f) * 0.88f;
raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - mag_offset[1]) / 4096.0f) * 0.88f;
raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - mag_offset[2]) / 4096.0f) * 0.88f;
/* store mode */
raw.magnetometer_mode = buf_magnetometer[3];
raw.magnetometer_raw_counter++;
}
/* BAROMETER */
if (baro_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
raw.baro_pres_mbar = buf_barometer[0]; // Pressure in mbar
raw.baro_alt_meter = buf_barometer[1]; // Altitude in meters
raw.baro_temp_celcius = buf_barometer[2]; // Temperature in degrees celcius
raw.baro_raw_counter++;
}
/* ADC */
if (adc_updated) {
/* copy sensor readings to global data*/
if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
/* Voltage in volts */
raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (uint16_t)(buf_adc.am_data1 * battery_voltage_conversion)));
if ((buf_adc.am_data1 * battery_voltage_conversion) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
raw.battery_voltage_valid = false;
raw.battery_voltage_v = 0.f;
} else {
raw.battery_voltage_valid = true;
}
raw.battery_voltage_counter++;
}
}
uint64_t total_time = hrt_absolute_time() - current_time;
/* Inform other processes that new data is available to copy */
if ((gyro_updated || acc_updated || magn_updated || baro_updated) && publishing) {
/* Values changed, publish */
orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw);
}
#ifdef CONFIG_HRT_PPM
if (ppm_updated) {
orb_publish(ORB_ID(rc_channels), rc_pub, &rc);
}
#endif
if (total_time > 2600) {
excessive_readout_time_counter++;
}
if (total_time > 2600 && excessive_readout_time_counter > 100 && excessive_readout_time_counter % 100 == 0) {
fprintf(stderr, "[sensors] slow update (>2600 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter);
} else if (total_time > 6000) {
if (excessive_readout_time_counter < 100 || excessive_readout_time_counter % 100 == 0) fprintf(stderr, "[sensors] WARNING: Slow update (>6000 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter);
}
read_loop_counter++;
#ifdef CONFIG_SENSORS_DEBUG_ENABLED
if (read_loop_counter % 1000 == 0) printf("[sensors] read loop counter: %d\n", read_loop_counter);
fflush(stdout);
if (sensors_timer_loop_counter % 1000 == 0) printf("[sensors] timer/trigger loop counter: %d\n", sensors_timer_loop_counter);
#endif
}
}
/* Never really getting here */
printf("[sensors] sensor readout stopped\n");
close(fd_gyro);
close(fd_accelerometer);
close(fd_magnetometer);
close(fd_barometer);
close(fd_adc);
printf("[sensors] exiting.\n");
return ret;
}
int main(int argc, char**argv)
{
// padd current password with zeros
password_padded = calloc(16, sizeof(uint8_t));
memcpy(password_padded, password, MIN(strlen(password), 16));
// initialize sensors
sensors_init();
// init random seed
srand(time(NULL));
// cleanup at exit
//atexit(sensors_cleanup);
signal(SIGINT, exit_cleanup);
int sockfd,bytes_recved;
struct sockaddr_in servaddr,cliaddr;
socklen_t len;
unsigned char mesg[1000];
sockfd=socket(AF_INET,SOCK_DGRAM,0);
bzero(&servaddr,sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr=htonl(INADDR_ANY);
servaddr.sin_port=htons(RMCP_UDP_PORT);
printf("Starting IPMI Server on Port %d\n",RMCP_UDP_PORT);
if (bind(sockfd,(struct sockaddr *)&servaddr,sizeof(servaddr)) == -1) {
printf("Could not open UDP socket on Port %d, exiting.\n",RMCP_UDP_PORT);
exit_cleanup();
}
for (;;) {
len = sizeof(cliaddr);
bytes_recved = recvfrom(sockfd,mesg,1000,0,(struct sockaddr *)&cliaddr,&len);
#ifdef DEBUG
printf("-------------------------------------------------------\n");
// print rec bytes
printf("REC: ");
for (int i = 0; i <= bytes_recved;i++) {
printf("0x%02x ",mesg[i]);
}
printf("\n");
#endif
mesg[bytes_recved] = 0;
// create internal packet
protocol_data* packet_in = malloc(sizeof(protocol_data));
packet_in->data = mesg;
packet_in->length = bytes_recved;
// handle answer packet
protocol_data* packet_out = rmcp_process_packet(packet_in);
if (packet_out->length > 0) {
sendto(sockfd,packet_out->data,packet_out->length,0,(struct sockaddr *)&cliaddr,sizeof(cliaddr));
} else {
printf("Error in udp-server.c: No Packetdata.\n");
}
// cleanup
if (packet_out->length > 0) {
free(packet_out->data);
}
free(packet_in);
free(packet_out);
}
}
static GList *libsensors_plugin_get_sensors(void) {
const sensors_chip_name *chip_name;
int i;
GList *sensors = NULL;
#if SENSORS_API_VERSION < 0x400
FILE *file;
g_debug("%s: using libsensors version < 4", __FUNCTION__);
/* try to open config file, otherwise try alternate config
* file - if neither succeed, exit */
if ((file = fopen (LIBSENSORS_CONFIG_FILE, "r")) == NULL) {
if ((file = fopen (LIBSENSORS_ALTERNATIVE_CONFIG_FILE, "r")) == NULL) {
g_debug("%s: error opening libsensors config file... ", __FUNCTION__);
return sensors;
}
}
/* at this point should have an open config file, if is not
* valid, close file and return */
if (sensors_init(file) != 0) {
fclose(file);
g_debug("%s: error initing libsensors from config file...", __FUNCTION__);
return sensors;
}
fclose(file);
/* libsensors exposes a number of chips - ... */
i = 0;
while ((chip_name = sensors_get_detected_chips (&i)) != NULL) {
char *chip_name_string;
const sensors_feature_data *data;
int n1 = 0, n2 = 0;
chip_name_string = get_chip_name_string(chip_name);
/* ... each of which has one or more 'features' ... */
while ((data = sensors_get_all_features (*chip_name, &n1, &n2)) != NULL) { // error
// fill in list for us
check_sensor_with_data(&sensors, chip_name_string,
chip_name, &n1, &n2, data);
}
g_free (chip_name_string);
}
#else
g_debug("%s: using libsensors version >= 4", __FUNCTION__);
int nr = 0;
if (sensors_init(NULL) != 0) {
g_debug("%s: error initing libsensors", __FUNCTION__);
return sensors;
}
i = 0;
while ((chip_name = sensors_get_detected_chips(NULL, &nr)))
{
char *chip_name_string, *label;
const sensors_subfeature *input_feature;
const sensors_subfeature *low_feature;
const sensors_subfeature *high_feature;
const sensors_feature *main_feature;
SensorType type;
gint nr1 = 0;
gdouble value, low, high;
gchar *path;
gboolean visible;
IconType icon;
chip_name_string = get_chip_name_string(chip_name);
if (chip_name_string == NULL) {
g_debug("%s: %d: error getting name string for sensor: %s\n",
__FILE__, __LINE__, chip_name->path);
continue;
}
while ((main_feature = sensors_get_features(chip_name, &nr1)))
{
switch (main_feature->type)
{
case SENSORS_FEATURE_IN:
type = VOLTAGE_SENSOR;
input_feature = sensors_get_subfeature(chip_name,
main_feature,
SENSORS_SUBFEATURE_IN_INPUT);
low_feature = sensors_get_subfeature(chip_name,
main_feature,
SENSORS_SUBFEATURE_IN_MIN);
high_feature = sensors_get_subfeature(chip_name,
main_feature,
SENSORS_SUBFEATURE_IN_MAX);
break;
case SENSORS_FEATURE_FAN:
type = FAN_SENSOR;
input_feature = sensors_get_subfeature(chip_name,
main_feature,
SENSORS_SUBFEATURE_FAN_INPUT);
low_feature = sensors_get_subfeature(chip_name,
main_feature,
SENSORS_SUBFEATURE_FAN_MIN);
// no fan max feature
high_feature = NULL;
break;
case SENSORS_FEATURE_TEMP:
type = TEMP_SENSOR;
input_feature = sensors_get_subfeature(chip_name,
main_feature,
SENSORS_SUBFEATURE_TEMP_INPUT);
low_feature = sensors_get_subfeature(chip_name,
main_feature,
SENSORS_SUBFEATURE_TEMP_MIN);
high_feature = sensors_get_subfeature(chip_name,
main_feature,
SENSORS_SUBFEATURE_TEMP_MAX);
if (!high_feature)
high_feature = sensors_get_subfeature(chip_name,
main_feature,
SENSORS_SUBFEATURE_TEMP_CRIT);
break;
default:
g_debug("%s: %d: error determining type for: %s\n",
__FILE__, __LINE__, chip_name_string);
continue;
}
if (!input_feature)
{
g_debug("%s: %d: could not get input subfeature for: %s\n",
__FILE__, __LINE__, chip_name_string);
continue;
}
// if still here we got input feature so get label
label = sensors_get_label(chip_name, main_feature);
if (!label)
{
g_debug("%s: %d: error: could not get label for: %s\n",
__FILE__, __LINE__, chip_name_string);
continue;
}
g_assert(chip_name_string && label);
icon = get_sensor_icon(type);
visible = (type == TEMP_SENSOR ? TRUE : FALSE);
sensors_applet_plugin_default_sensor_limits(type,
&low,
&high);
if (low_feature) {
sensors_get_value(chip_name, low_feature->number, &low);
}
if (high_feature) {
sensors_get_value(chip_name, high_feature->number, &high);
}
if (sensors_get_value(chip_name, input_feature->number, &value) < 0) {
g_debug("%s: %d: error: could not get value for input feature of sensor: %s\n",
__FILE__, __LINE__, chip_name_string);
free(label);
continue;
}
g_debug("for chip %s (type %s) got label %s and value %f", chip_name_string,
(type == TEMP_SENSOR ? "temp" :
(type == FAN_SENSOR ? "fan" :
(type == VOLTAGE_SENSOR ? "voltage" : "error"))), label, value);
path = g_strdup_printf ("sensor://%s/%d", chip_name_string, input_feature->number);
g_hash_table_insert(hash_table, g_strdup(path), (void *)chip_name);
sensors_applet_plugin_add_sensor_with_limits(&sensors,
path,
label,
label,
type,
visible,
low,
high,
icon,
DEFAULT_GRAPH_COLOR);
}
g_free(chip_name_string);
}
#endif
return sensors;
}
int
main(int argc, char **argv)
{
msp430_init();
rs232_init(0);
sensors_init();
rs232_print("uip_init()...\n");
uip_init();
/* rs232_print("resolv_init()...\n");
resolv_init();*/
#if 0
uip_ipaddr(addr, 193,10,67,150);
uip_sethostaddr(addr);
/* uip_ipaddr(addr, 193,10,66,195);
resolv_conf(addr);*/
#else
uip_ipaddr(addr, 172,16,0,2);
uip_sethostaddr(addr);
/* uip_ipaddr(addr, 193,10,66,195);
resolv_conf(addr);*/
#endif
rs232_print("dispatcher_init()...\n");
dispatcher_init();
rs232_print("slip_drv_init()...\n");
slip_drv_init(NULL);
rs232_print("ctk_init()...\n");
/* ctk_init();*/
rs232_print("ctk_vncserver_init()...\n");
/* ctk_vncserver_init(NULL); */
rs232_print("program_handler_init()...\n");
/* program_handler_init();*/
/* rs232_print("processes_init()...\n");
processes_init(NULL);*/
rs232_print("webserver_init()...\n");
/* webserver_init(NULL);*/
sensorcheck_init(NULL);
/* program_handler_add(&directory_dsc, "Directory", 1);*/
/* program_handler_add(&about_dsc, "About", 1);*/
/* program_handler_add(&webserver_dsc, "Web server", 1);*/
/* program_handler_add(&www_dsc, "Web browser", 1);*/
/* program_handler_add(&calc_dsc, "Calculator", 0);*/
/* program_handler_add(&processes_dsc, "Processes", 0);*/
rs232_print("program_handler_addd()...\n");
/* program_handler_add(&sensorview_dsc, "Sensors", 1);*/
rs232_print("dispatcher_run()...\n");
beep();
dispatcher_run();
return 0;
argv = argv;
argc = argc;
}
