int
show_config_gtmProxy(int flag, int idx, char *hostname)
{
char lineBuf[MAXLINE+1];
char editBuf[MAXPATH+1];
lineBuf[0] = 0;
if (flag)
strncat(lineBuf, "GTM Proxy: ", MAXLINE);
if (hostname)
{
snprintf(editBuf, MAXPATH, "host: %s", hostname);
strncat(lineBuf, editBuf, MAXLINE);
}
if (flag || hostname)
strncat(lineBuf, "\n", MAXLINE);
lockLogFile();
if (lineBuf[0])
elog(NOTICE, "%s", lineBuf);
print_simple_node_info(aval(VAR_gtmProxyNames)[idx], aval(VAR_gtmProxyPorts)[idx],
aval(VAR_gtmProxyDirs)[idx], sval(VAR_gtmPxyExtraConfig),
aval(VAR_gtmPxySpecificExtraConfig)[idx]);
unlockLogFile();
return 0;
}
/*
* Get all the servers --> VAR_allServers
*/
static void
addServer(char **name)
{
int ii, jj;
int flag;
confirm_var(VAR_allServers);
for (ii = 0; name[ii]; ii++)
{
flag = TRUE;
for (jj = 0; aval(VAR_allServers)[jj]; jj++)
{
if (strcmp(name[ii], aval(VAR_allServers)[jj]) != 0)
continue;
else
{
flag = FALSE;
break;
}
}
if (flag)
add_val(find_var(VAR_allServers), name[ii]);
}
}
int gtmProxyIdx(char *gtmProxyName)
{
int ii;
for (ii = 0; aval(VAR_gtmProxyNames)[ii]; ii++)
{
if (strcmp(aval(VAR_gtmProxyNames)[ii], gtmProxyName) == 0)
return ii;
}
return -1;
}
int coordIdx(char *coordName)
{
int ii;
if (is_none(coordName))
return -1;
for (ii = 0; aval(VAR_coordNames)[ii]; ii++)
{
if (strcmp(aval(VAR_coordNames)[ii], coordName) == 0)
return ii;
}
return -1;
}
int datanodeIdx(char *datanodeName)
{
int ii;
if (is_none(datanodeName))
return -1;
for (ii = 0; aval(VAR_datanodeNames)[ii]; ii++)
{
if (strcmp(aval(VAR_datanodeNames)[ii], datanodeName) == 0)
return ii;
}
return -1;
}
int getEffectiveGtmProxyIdxFromServerName(char *serverName)
{
int ii;
if (serverName == NULL)
return (-1);
for (ii = 0; aval(VAR_gtmProxyNames)[ii]; ii++)
{
if (strcmp(aval(VAR_gtmProxyServers)[ii], serverName) == 0)
return ii;
}
return -1;
}
NodeType getNodeType(char *nodeName)
{
int ii;
/* Check GTM */
if (strcmp(nodeName, sval(VAR_gtmName)) == 0)
return NodeType_GTM;
/* GTM_Proxy */
for (ii = 0; aval(VAR_gtmProxyNames)[ii]; ii++)
if (strcmp(nodeName, aval(VAR_gtmProxyNames)[ii]) == 0)
return NodeType_GTM_PROXY;
/* Coordinator */
for (ii = 0; aval(VAR_coordNames)[ii]; ii++)
if (strcmp(nodeName, aval(VAR_coordNames)[ii]) == 0)
return NodeType_COORDINATOR;
/* Datanode */
for (ii = 0; aval(VAR_datanodeNames)[ii]; ii++)
if (strcmp(nodeName, aval(VAR_datanodeNames)[ii]) == 0)
return NodeType_DATANODE;
/* Nodename */
for (ii = 0; aval(VAR_allServers)[ii]; ii++)
if (strcmp(nodeName, aval(VAR_allServers)[ii]) == 0)
return NodeType_SERVER;
return NodeType_UNDEF;
}
int getDefaultWalSender(int isCoord)
{
int ii;
char *names = isCoord ? VAR_coordNames : VAR_datanodeNames;
char *walSender = isCoord ? VAR_coordMaxWALSenders : VAR_datanodeMaxWALSenders;
for (ii = 0; aval(names)[ii]; ii++)
{
if (doesExist(names, ii) && !is_none(aval(names)[ii]) && (atoi(aval(walSender)[ii]) >= 0))
return atoi(aval(walSender)[ii]);
}
return 0;
}
/*
* Cleanup -- nodeName must be valid. Instead, NULL will bereturned.
*/
cmd_t *
prepare_cleanGtmProxy(char *nodeName)
{
cmd_t *cmd;
int idx;
if ((idx = gtmProxyIdx(nodeName)) < 0)
return NULL;
cmd = initCmd(aval(VAR_gtmProxyServers)[idx]);
snprintf(newCommand(cmd), MAXLINE,
"rm -rf %s; mkdir -p %s; chmod 0700 %s;rm -f /tmp/.s.*%d*",
aval(VAR_gtmProxyDirs)[idx], aval(VAR_gtmProxyDirs)[idx], aval(VAR_gtmProxyDirs)[idx],
atoi(aval(VAR_gtmProxyPorts)[idx]));
return cmd;
}
void QRtmp::setConnData(const QByteArray &data)
{
m_connData = data;
AVal connData = aval(m_connData);
if(!RTMP_SetOpt(m_rtmp, &av_conn, &connData))
RTMP_Log(RTMP_LOGERROR, "Invalid AMF parameter");
}
/*
* Stop gtm proxy -------------------------------------------------------------
*/
static cmd_t *
prepare_stopGtmProxy(char *nodeName)
{
cmd_t *cmd;
int idx;
if ((idx = gtmProxyIdx(nodeName)) < 0)
{
elog(ERROR, "ERROR: Specified name %s is not GTM Proxy\n", nodeName);
return NULL;
}
cmd = initCmd(aval(VAR_gtmProxyServers)[idx]);
snprintf(newCommand(cmd), MAXLINE,
"gtm_ctl stop -Z gtm_proxy -D %s",
aval(VAR_gtmProxyDirs)[idx]);
return(cmd);
}
/*
* Test each node and build target server list
*/
void makeServerList(void)
{
/* Initialize */
reset_var(VAR_allServers);
/* GTM Master */
addServer(aval(VAR_gtmMasterServer));
/* GTM Slave */
if (isVarYes(VAR_gtmSlave))
addServer(aval(VAR_gtmSlaveServer));
/* GTM_Proxy */
if (isVarYes(VAR_gtmProxy))
addServer(aval(VAR_gtmProxyServers));
/* Coordinator Master */
if (find_var(VAR_coordMasterServers))
addServer(aval(VAR_coordMasterServers));
/* Coordinator Slave */
if (isVarYes(VAR_coordSlave))
addServer(aval(VAR_coordSlaveServers));
/* Datanode Master */
addServer(aval(VAR_datanodeMasterServers));
/* Datanode Slave */
if (isVarYes(VAR_datanodeSlave))
addServer(aval(VAR_datanodeSlaveServers));
/* Should add secondary slaves */
}
/*
* Reconnect to the current GTM master --------------------------------------------------
*
* When failed over, the current Master must have been updated.
* Remember to update gtm_proxy configuration file so that it
* connects to the new master at the next start.
* Please note that we assume GTM has already been failed over.
* First argument is gtm_proxy nodename
*/
static cmd_t *
prepare_reconnectGtmProxy(char *nodeName)
{
cmd_t *cmdGtmCtl, *cmdGtmProxyConf;
int idx;
FILE *f;
char date[MAXTOKEN+1];
if ((idx = gtmProxyIdx(nodeName)) < 0)
{
elog(ERROR, "ERROR: Specified name %s is not GTM Proxy\n", nodeName);
return(NULL);
}
/* gtm_ctl reconnect */
cmdGtmCtl = initCmd(aval(VAR_gtmProxyServers)[idx]);
snprintf(newCommand(cmdGtmCtl), MAXLINE,
"gtm_ctl reconnect -Z gtm_proxy -D %s -o \\\"-s %s -t %s\\\"",
aval(VAR_gtmProxyDirs)[idx], sval(VAR_gtmMasterServer), sval(VAR_gtmMasterPort));
/* gtm_proxy.conf */
appendCmdEl(cmdGtmCtl, (cmdGtmProxyConf = initCmd(aval(VAR_gtmProxyServers)[idx])));
if ((f = prepareLocalStdin(newFilename(cmdGtmProxyConf->localStdin), MAXPATH, NULL)) == NULL)
{
cleanCmd(cmdGtmCtl);
return(NULL);
}
fprintf(f,
"#===================================================\n"
"# Updated due to GTM Proxy reconnect\n"
"# %s\n"
"gtm_host = '%s'\n"
"gtm_port = %s\n"
"#----End of reconfiguration -------------------------\n",
timeStampString(date, MAXTOKEN),
sval(VAR_gtmMasterServer),
sval(VAR_gtmMasterPort));
fclose(f);
snprintf(newCommand(cmdGtmProxyConf), MAXLINE,
"cat >> %s/gtm_proxy.conf", aval(VAR_gtmProxyDirs)[idx]);
return(cmdGtmCtl);
}
int
init_gtm_proxy_all(void)
{
elog(NOTICE, "Initialize all the gtm proxies.\n");
if (!isVarYes(VAR_gtmProxy))
{
elog(ERROR, "ERROR: GTM Proxy is not configured.\n");
return(1);
}
return(init_gtm_proxy(aval(VAR_gtmProxyNames)));
}
int
show_config_gtmProxies(char **nameList)
{
int ii;
lockLogFile();
for(ii = 0; nameList[ii]; ii++)
show_config_gtmProxy(TRUE, ii, aval(VAR_gtmProxyServers)[ii]);
unlockLogFile();
return 0;
}
/*
* Note that 1 will be returned when a conflict is found
*/
int checkNameConflict(char *name, int is_gtm)
{
int ii;
/*
* GTM Master
*/
if (!is_gtm && strcasecmp(name, sval(VAR_gtmName)) == 0)
return 1;
/*
* GTM Proxy
*/
if (isVarYes(VAR_gtmProxy))
for (ii = 0; aval(VAR_gtmProxyNames)[ii]; ii++)
if (strcasecmp(name, aval(VAR_gtmProxyNames)[ii]) == 0)
return 1;
/*
* Coordinator
*/
for (ii = 0; aval(VAR_coordNames)[ii]; ii++)
if (strcasecmp(name, aval(VAR_coordNames)[ii]) == 0)
return 1;
/*
* Datanode
*/
for (ii = 0; aval(VAR_datanodeNames)[ii]; ii++)
if (strcasecmp(name, aval(VAR_datanodeNames)[ii]) == 0)
return 1;
return 0;
}
static void xml_start(void *data, const char *el, const char **attr) {
xml_parser *p = (xml_parser *)data;
p->m_current->name.assign(el);
for (size_t i = 0; attr[i]; i += 2) {
std::string aname(attr[i]);
std::string aval(attr[i+1]);
auto it = p->m_current->attrs.find(aname);
if (it == p->m_current->attrs.end()) {
p->m_current->attrs[aname] = std::vector<std::string>({aval});
} else {
it->second.emplace_back(aval);
}
}
p->on_start(*p->m_current);
}
int main(void) {
int n, i; /*n sera el grau del polinomi, i servira per iterar*/
double t, pt, ft, er; /* t, fx, pn i er serviran per guardar i printar amb precisio el nodes t equidistants entre 0 i 2*PI, el resultat de f(t), el resultat del polinomi interpolador p(t) i l'error relatiu entre p(t) i f(t), sobretot en el cas d'error relatiu*/
double *px, *py; /* els punters px i py serviran per guardar tots els nodes t i els seus respectius f(t) */
/*demanem i escanegem el grau del polinomi interpolador*/
printf("Grau del polinomi:\n");
scanf("%i", &n);
/*reservem memoria i comprovem que l'operacio pugui ser efectuada*/
px = (double *)malloc( (n+1)*sizeof(double *));
py = (double *)malloc( (n+1)*sizeof(double *));
if(px == NULL || py == NULL){
printf("No hi ha pru memoria");
exit(1);
}
/* volem que x tingui els n+1 primers nodes t equidistants entre 0 i 2*PI i que y tingui f(xi) en cada node i de x, no cal utilitzar la variable t en aquesta iteracio ja que es implicita */
for(i=0;i<(n+1);i++){
px[i]=i*((2*acos(-1))/(1000)); /* t = i*((2*acos(-1))/(1000)) */
py[i]=f(px[i]);
}
/* apliquem diferencies dvidides*/
dif_dividides(px,py,n);
/* printem una especie de capcelera de taula*/
printf("\t_____________________________________________________________\n\n");
printf("\t\tt\t\tf(t)\t\tPn(t)\t\ter(t)\n");
printf("\t_____________________________________________________________\n\n");
/* calculem t, pn(t), f(t), er(t) i ho printem en una linia seguint la forma de la taula que hem printat abans*/
for(i=0;i<=20;i++){
t = i *(2*acos(-1))/(1000);
pt = aval(px,py,n,t);
ft = f(t);
er = (fabs(pt-ft))/ft;
printf("\t%e \t%e \t%e \t%e\n",t,ft,pt,er);
}
/*final del main*/
return 0;
}
/*
* Kill gtm proxy -------------------------------------------------------------------
*
* Although gtm proxy does not have significant resources to carry over to the next
* run, it is a good habit to stop gtm proxy with stop command gracefully.
*/
static cmd_t *
prepare_killGtmProxy(char *nodeName)
{
cmd_t *cmd;
int idx;
pid_t gtmPxyPid;
if ((idx = gtmProxyIdx(nodeName)) < 0)
{
elog(ERROR, "ERROR: Specified name %s is not GTM Proxy\n", nodeName);
return NULL;
}
cmd = initCmd(aval(VAR_gtmProxyServers)[idx]);
gtmPxyPid = get_gtmProxy_pid(aval(VAR_gtmProxyServers)[idx], aval(VAR_gtmProxyDirs)[idx]);
if (gtmPxyPid > 0)
snprintf(newCommand(cmd), MAXLINE,
"kill -9 %d; rm -rf /tmp/.s.'*'%d'*' %s/gtm_proxy.pid",
gtmPxyPid, atoi(aval(VAR_gtmProxyPorts)[idx]), aval(VAR_gtmProxyDirs)[idx]);
else
snprintf(newCommand(cmd), MAXLINE,
"killall -u %s -9 gtm; rm -rf /tmp/.s.'*'%d'*' %s/gtm_proxy.pid",
sval(VAR_pgxcUser), atoi(aval(VAR_gtmProxyPorts)[idx]), aval(VAR_gtmProxyDirs)[idx]);
return(cmd);
}
void
BMI160::measure()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct BMIReport bmi_report;
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMI160 in one pass.
*/
bmi_report.cmd = BMIREG_GYR_X_L | DIR_READ;
uint8_t status = read_reg(BMIREG_STATUS);
if (OK != transfer((uint8_t *)&bmi_report, ((uint8_t *)&bmi_report), sizeof(bmi_report))) {
return;
}
check_registers();
if ((!(status & (0x80))) && (!(status & (0x04)))) {
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
return;
}
_last_accel[0] = bmi_report.accel_x;
_last_accel[1] = bmi_report.accel_y;
_last_accel[2] = bmi_report.accel_z;
_got_duplicate = false;
uint8_t temp_l = read_reg(BMIREG_TEMP_0);
uint8_t temp_h = read_reg(BMIREG_TEMP_1);
report.temp = ((temp_h << 8) + temp_l);
report.accel_x = bmi_report.accel_x;
report.accel_y = bmi_report.accel_y;
report.accel_z = bmi_report.accel_z;
report.gyro_x = bmi_report.gyro_x;
report.gyro_y = bmi_report.gyro_y;
report.gyro_z = bmi_report.gyro_z;
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the bmi160 does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
perf_count(_good_transfers);
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again. We still increment
// _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
* Report buffers.
*/
accel_report arb;
gyro_report grb;
/*
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
_last_temperature = 23 + report.temp * 1.0f / 512.0f;
arb.temperature_raw = report.temp;
arb.temperature = _last_temperature;
/* return device ID */
arb.device_id = _device_id.devid;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
xraw_f = report.gyro_x;
yraw_f = report.gyro_y;
zraw_f = report.gyro_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
math::Vector<3> gval_integrated;
bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp;
grb.temperature = _last_temperature;
/* return device ID */
grb.device_id = _gyro->_device_id.devid;
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
}
if (gyro_notify) {
_gyro->parent_poll_notify();
}
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (gyro_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
perf_end(_sample_perf);
}
/*
* Check if there's any conflict among src and dest, checks duplicate in names, servers, ports and directories.
*
* The rules are:
*
* 1) Each node (gtm, gtm_proxy, coordinator, datanode) must have unique name.
*
* 2) A port, in a given host, must be owned (listed to) only by single node.
*
* 3) A directory, in a given host, must be owned (used) only by single node.
*/
static void checkResourceConflict(char *srcNames, char *srcServers, char *srcPorts, char *srcPoolers, char *srcDirs,
char *destNames, char *destServers, char *destPorts, char *destPoolers, char *destDirs,
int destOnly, int checkName)
{
int ii, jj;
if (!srcNames || !find_var(srcNames))
{
/* No source specified */
return;
}
if (!destOnly)
{
/* Check conflict among the source first */
for (ii = 0; aval(srcNames)[ii]; ii++)
{
if (is_none(aval(srcNames)[ii]))
continue;
/* Pooler and the port in the same name */
if (srcPoolers && (atoi(aval(srcPorts)[ii]) == atoi(aval(srcPoolers)[ii])))
{
if (atoi(aval(srcPorts)[ii]) > 0)
{
anyConfigErrors = TRUE;
elog(ERROR, "ERROR: Conflict in between port and pooler within %s variable.\n", srcNames);
}
}
if (checkName && srcNames && !doesExist(srcNames, ii))
assign_arrayEl(srcNames, ii, "none", NULL);
if (srcServers && !doesExist(srcServers, ii))
assign_arrayEl(srcServers, ii, "none", NULL);
if (srcPoolers && !doesExist(srcPoolers, ii))
assign_arrayEl(srcPoolers, ii, "-1", "-1");
if (srcPorts && !doesExist(srcPorts, ii))
assign_arrayEl(srcPorts, ii, "-1", "-1");
if (srcDirs && !doesExist(srcDirs, ii))
assign_arrayEl(srcDirs, ii, "none", NULL);
for (jj = ii+1; aval(srcNames)[jj]; jj++)
{
/* Name conflict */
if (checkName && srcNames && !doesExist(srcNames, jj))
assign_arrayEl(srcNames, jj, "none", NULL);
if (checkName && srcNames && (strcmp(aval(srcNames)[ii], aval(srcNames)[jj]) == 0))
{
anyConfigErrors = TRUE;
elog(ERROR, "ERROR: Conflict in resource name within %s variable.\n", srcNames);
}
if (srcServers && is_none(aval(srcServers)[ii]))
continue;
if (srcServers && !doesExist(srcServers, jj))
assign_arrayEl(srcServers, jj, "none", NULL);
if (srcServers && strcmp(aval(srcServers)[ii], aval(srcServers)[jj]) == 0)
{
/* Ports and Poolers */
if (srcPorts && !doesExist(srcPorts, jj))
assign_arrayEl(srcPorts, jj, "-1", "-1");
if (srcPoolers && !doesExist(srcPoolers, jj))
assign_arrayEl(srcPoolers, jj, "-1", "-1");
if((srcPorts && (atoi(aval(srcPorts)[ii]) > 0) && (atoi(aval(srcPorts)[ii]) == atoi(aval(srcPorts)[jj]))) ||
(srcPorts && srcPoolers && (atoi(aval(srcPorts)[ii]) > 0) && (atoi(aval(srcPorts)[ii]) == atoi(aval(srcPoolers)[jj]))) ||
(srcPoolers && (atoi(aval(srcPoolers)[ii]) > 0) && (atoi(aval(srcPoolers)[ii]) == atoi(aval(srcPoolers)[jj]))))
{
anyConfigErrors = TRUE;
elog(ERROR, "ERROR: Conflict in port and pooler numbers within %s variable.\n", srcNames);
}
/* Directories */
if (srcDirs && !doesExist(srcDirs, jj))
assign_arrayEl(srcDirs, jj, "none", NULL);
if (srcDirs && strcmp(aval(srcDirs)[ii], aval(srcDirs)[jj]) == 0)
{
if (!is_none(aval(srcDirs)[ii]))
{
anyConfigErrors = TRUE;
elog(ERROR, "ERROR: Conflict in directories within %s variable.\n", srcNames);
}
}
}
}
}
}
/* Check between src and destination */
if (destNames)
{
for (ii = 0; aval(srcNames)[ii]; ii++)
{
if (is_none(aval(srcNames)[ii]))
continue;
for (jj = 0; aval(destNames)[jj]; jj++)
{
/* Resource names */
if (checkName && (strcmp(aval(srcNames)[ii], aval(destNames)[jj]) == 0))
{
anyConfigErrors = TRUE;
elog(ERROR, "ERROR: Conflict in names between %s and %s variable.\n", srcNames, destNames);
}
if (destServers && !doesExist(destServers, jj))
assign_arrayEl(destServers, jj, "none", NULL);
if (srcServers && destServers && (strcmp(aval(srcServers)[ii], aval(destServers)[jj]) == 0) && !is_none(aval(srcServers)[ii]))
{
/* Ports and poolers */
if (destPorts && !doesExist(destPorts, jj))
assign_arrayEl(destPorts, jj, "-1", "-1");
if (destPoolers && !doesExist(destPoolers, jj))
assign_arrayEl(destPoolers, jj, "-1", "-1");
if ((srcPorts && destPorts && (atoi(aval(srcPorts)[ii]) == atoi(aval(destPorts)[jj])) && (atoi(aval(srcPorts)[ii]) > 0)) ||
(destPoolers && srcPorts && (destPoolers && (atoi(aval(srcPorts)[ii]) == atoi(aval(destPoolers)[jj]))) && (atoi(aval(srcPorts)[ii]) > 0)) ||
(srcPoolers && destPorts && (atoi(aval(srcPoolers)[ii]) == atoi(aval(destPorts)[jj])) && (atoi(aval(srcPoolers)[ii]) > 0)) ||
(srcPoolers && destPoolers && (atoi(aval(srcPoolers)[ii]) == atoi(aval(destPoolers)[jj])) && (atoi(aval(srcPoolers)[ii]) > 0)))
{
anyConfigErrors = TRUE;
elog(ERROR, "ERROR: Conflict in port/pooler in %s and %s variable.\n", srcNames, destNames);
}
/* Dir Names */
if (srcDirs && destDirs && !is_none(aval(srcDirs)[ii]) && (strcmp(aval(srcDirs)[ii], aval(destDirs)[jj]) == 0))
{
anyConfigErrors = TRUE;
elog(ERROR, "ERROR: Conflict in directory names in %s and %s variable.\n", srcNames, destNames);
}
}
}
}
}
}
int checkDirConflict(char *host, char *dir)
{
int ii;
/* GTM Master */
if ((strcasecmp(host, sval(VAR_gtmMasterServer)) == 0) && (strcmp(dir, sval(VAR_gtmMasterDir)) == 0))
return 1;
/* GTM Slave */
if (isVarYes(VAR_gtmSlave) && (strcasecmp(host, sval(VAR_gtmSlaveServer)) == 0) && (strcmp(dir, sval(VAR_gtmSlaveDir)) == 0))
return 1;
/* GTM Proxy */
if (isVarYes(VAR_gtmProxy))
for (ii = 0; aval(VAR_gtmProxyNames)[ii]; ii++)
if ((strcasecmp(host, aval(VAR_gtmProxyServers)[ii]) == 0) && (strcmp(dir, aval(VAR_gtmProxyDirs)[ii]) == 0))
return 1;
/* Coordinator Master */
for (ii = 0; aval(VAR_coordNames)[ii]; ii++)
if ((strcasecmp(host, aval(VAR_coordMasterServers)[ii]) == 0) && (strcmp(dir, aval(VAR_coordMasterDirs)[ii]) == 0))
return 1;
/* Coordinator Slave */
if (isVarYes(VAR_coordSlave))
for (ii = 0; aval(VAR_coordNames)[ii]; ii++)
if ((strcasecmp(host, aval(VAR_coordSlaveServers)[ii]) == 0) && (strcmp(dir, aval(VAR_coordSlaveDirs)[ii]) == 0))
return 1;
/* Datanode Master */
for (ii = 0; aval(VAR_datanodeNames)[ii]; ii++)
if ((strcasecmp(host, aval(VAR_datanodeMasterServers)[ii]) == 0) && (strcmp(dir, aval(VAR_datanodeMasterDirs)[ii]) == 0))
return 1;
/* Datanode Slave */
if (isVarYes(VAR_datanodeSlave))
if (doesExist(VAR_datanodeSlaveServers, ii) && doesExist(VAR_datanodeSlaveDirs, ii) &&
(strcasecmp(host, aval(VAR_datanodeSlaveServers)[ii]) == 0) && (strcmp(dir, aval(VAR_datanodeSlaveDirs)[ii]) == 0))
return 1;
return 0;
}
/*
* Note that 1 will be returned when a conflict is found.
*/
int checkPortConflict(char *host, int port)
{
int ii;
/* GTM Master */
if ((strcasecmp(host, sval(VAR_gtmMasterServer)) == 0) && (atoi(sval(VAR_gtmMasterPort)) == port))
return 1;
/* GTM Slave */
if (isVarYes(VAR_gtmSlave) && (strcasecmp(host, sval(VAR_gtmSlaveServer)) == 0) && (atoi(sval(VAR_gtmSlavePort)) == port))
return 1;
/* GTM Proxy */
if (isVarYes(VAR_gtmProxy))
for (ii = 0; aval(VAR_gtmProxyNames)[ii]; ii++)
if ((strcasecmp(host, aval(VAR_gtmProxyServers)[ii]) == 0) && (atoi(aval(VAR_gtmProxyPorts)[ii]) == port))
return 1;
/* Coordinator Master */
for (ii = 0; aval(VAR_coordNames)[ii]; ii++)
if ((strcasecmp(host, aval(VAR_coordMasterServers)[ii]) == 0) &&
((atoi(aval(VAR_coordPorts)[ii]) == port) || (atoi(aval(VAR_poolerPorts)[ii])) == port))
return 1;
/* Coordinator Slave */
if (isVarYes(VAR_coordSlave))
for (ii = 0; aval(VAR_coordNames)[ii]; ii++)
if (doesExist(VAR_coordSlaveServers, ii) && !is_none(aval(VAR_coordSlaveServers)[ii]) &&
(strcasecmp(host, aval(VAR_coordSlaveServers)[ii]) == 0) && (atoi(aval(VAR_coordPorts)[ii]) == port))
return 1;
/* Datanode Master */
for (ii = 0; aval(VAR_datanodeNames)[ii]; ii++)
if ((strcasecmp(host, aval(VAR_datanodeMasterServers)[ii]) == 0) && (atoi(aval(VAR_datanodePorts)[ii]) == port))
return 1;
/* Datanode Slave */
if (isVarYes(VAR_datanodeSlave))
for (ii = 0; aval(VAR_datanodeNames)[ii]; ii++)
if (doesExist(VAR_datanodeSlaveServers, ii) && !is_none(aval(VAR_datanodeSlaveServers)[ii]) &&
(strcasecmp(host, aval(VAR_datanodeSlaveServers)[ii]) == 0) && (atoi(aval(VAR_datanodePorts)[ii]) == port))
return 1;
return 0;
}
/*
* Scans initial configuration and set up "not configured" things.
*
* If, for example, gtm proxy is not configured,
* we set gtmProxy variable to "n".
*
* When gtmProxy varieble is already set to "n", remove gtm_proxy
* configuration information.
*
* Similar handling will be done for gtm slave, coordinator slaves
* and datanode slaves.
*/
void handle_no_slaves()
{
int is_empty;
int ii;
/* GTM Slave */
if (!find_var(VAR_gtmSlave))
reset_var_val(VAR_gtmSlave, "n");
if (!isVarYes(VAR_gtmSlave))
emptyGtmSlave();
else
{
confirm_var(VAR_gtmSlaveServer);
if (!sval(VAR_gtmSlaveServer) || is_none(sval(VAR_gtmSlaveServer)))
{
emptyGtmSlave();
reset_var_val(VAR_gtmSlaveServer, "n");
}
}
/* GTM Proxy */
if (!find_var(VAR_gtmProxy))
reset_var_val(VAR_gtmProxy, "n");
if (!isVarYes(VAR_gtmProxy))
emptyGtmProxies();
else
{
is_empty = TRUE;
for (ii = 0; aval(VAR_gtmProxyServers)[ii]; ii++)
{
if (is_none(aval(VAR_gtmProxyServers)[ii]))
continue;
else
{
is_empty = FALSE;
break;
}
}
if (is_empty)
{
reset_var_val(VAR_gtmProxy, "n");
emptyGtmProxies();
}
}
/* Coordinator Slaves */
if (!find_var(VAR_coordSlave))
reset_var_val(VAR_coordSlave, "n");
if (!isVarYes(VAR_coordSlave))
emptyCoordSlaves();
else
{
is_empty = TRUE;
if (find_var(VAR_coordSlaveServers))
{
for (ii = 0; aval(VAR_coordSlaveServers)[ii]; ii++)
{
if (is_none(aval(VAR_coordSlaveServers)[ii]))
continue;
else
{
is_empty = FALSE;
break;
}
}
if (is_empty)
{
reset_var_val(VAR_coordSlave, "n");
emptyCoordSlaves();
}
}
else
{
elog(WARNING, "WARNING: coordSlaveServers variable not found where coordSlave is set to \"y\"\n");
reset_var_val(VAR_coordSlave, "n");
emptyCoordSlaves();
}
}
/* Datanode Slaves */
if (!find_var(VAR_datanodeSlave))
reset_var_val(VAR_datanodeSlave, "n");
if (!isVarYes(VAR_datanodeSlave))
emptyDatanodeSlaves();
else
{
is_empty = TRUE;
if (find_var(VAR_datanodeSlaveServers))
{
for (ii = 0; aval(VAR_datanodeSlaveServers)[ii]; ii++)
{
if (is_none(aval(VAR_datanodeSlaveServers)[ii]))
continue;
else
{
is_empty = FALSE;
break;
}
}
if (is_empty)
{
reset_var_val(VAR_datanodeSlave, "n");
emptyDatanodeSlaves();
}
}
else
{
elog(WARNING, "WARNING: datanodeSlaveServers variable not found where datanodeSlave is set to \"y\"\n");
reset_var_val(VAR_datanodeSlave, "n");
emptyDatanodeSlaves();
}
}
}
void QRtmp::start()
{
m_error.clear();
m_stop = false;
if(m_hostname.isEmpty())
{
setError("You must specify a hostname (--host) or url (-r \"rtmp://host[:port]/playpath\") containing a hostname");
return;
}
if(m_playpath.isEmpty())
{
setError("You must specify a playpath (--playpath) or url (-r \"rtmp://host[:port]/playpath\") containing a playpath");
return;
}
if(m_proto == Undefined)
{
RTMP_Log(RTMP_LOGWARNING, "You haven't specified a protocol (--protocol) or rtmp url (-r), using default protocol RTMP");
m_proto = RTMP;
}
if(m_port == -1)
{
RTMP_Log(RTMP_LOGWARNING, "You haven't specified a port (--port) or rtmp url (-r), using default port");
m_port = 0;
}
int protocol = rtmpProto(m_proto);
if(m_port == 0)
{
if(protocol & RTMP_FEATURE_SSL)
m_port = 443;
else if(protocol & RTMP_FEATURE_HTTP)
m_port = 80;
else
m_port = 1935;
}
// calculate hash
{
unsigned char hash[RTMP_SWF_HASHLEN];
if(RTMP_HashSWF(m_swfUrl.data(), &m_swfSize, hash, m_swfAge) == 0)
m_swfHash = QByteArray::fromRawData(reinterpret_cast<const char*>(hash), sizeof(hash));
}
if(m_swfHash.isEmpty() && m_swfSize > 0)
{
RTMP_Log(RTMP_LOGWARNING, "Ignoring SWF size, supply also the hash with --swfhash");
m_swfSize = 0;
}
if(!m_swfHash.isEmpty() && m_swfSize == 0)
{
RTMP_Log(RTMP_LOGWARNING, "Ignoring SWF hash, supply also the swf size with --swfsize");
m_swfHash.clear();
}
if(m_tcUrl.isEmpty())
m_tcUrl = QString("%1://%2:%3/%4")
.arg(RTMPProtocolStringsLower[protocol])
.arg(QString(m_hostname))
.arg(m_port)
.arg(QString(m_app))
.toLocal8Bit();
if(dStartOffset > 0 && m_bLiveStream)
{
RTMP_Log(RTMP_LOGWARNING, "Can't seek in a live stream, ignoring --start option");
dStartOffset = 0;
}
// usherToken is unavailable for librtmp 2.3
AVal hostname = aval(m_hostname),
proxy = aval(m_proxy),
playpath = aval(m_playpath),
tcUrl = aval(m_tcUrl),
swfUrl = aval(m_swfUrl),
pageUrl = aval(m_pageUrl),
app = aval(m_app),
auth = aval(m_auth),
swfHash = aval(m_swfHash),
flashVer = aval(m_flashVer),
subscribePath = aval(m_subscribepath);
RTMP_SetupStream(m_rtmp, protocol, &hostname, m_port, &proxy, &playpath,
&tcUrl, &swfUrl, &pageUrl, &app, &auth,
&swfHash, m_swfSize, &flashVer, &subscribePath, /*&aval(m_usherToken),*/
dSeek, dStopOffset, m_bLiveStream, m_timeout);
/* Try to keep the stream moving if it pauses on us */
if(!m_bLiveStream && !(protocol & RTMP_FEATURE_HTTP))
m_rtmp->Link.lFlags |= RTMP_LF_BUFX;
QThread::start();
}
void
FXOS8700CQ::measure()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd[2];
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
int16_t mx;
int16_t my;
int16_t mz;
} raw_accel_mag_report;
#pragma pack(pop)
accel_report accel_report;
/* start the performance counter */
perf_begin(_accel_sample_perf);
check_registers();
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again.
_register_wait--;
perf_end(_accel_sample_perf);
return;
}
/* fetch data from the sensor */
memset(&raw_accel_mag_report, 0, sizeof(raw_accel_mag_report));
raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8700CQ_DR_STATUS);
raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8700CQ_DR_STATUS);
transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report));
if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) {
perf_end(_accel_sample_perf);
perf_count(_accel_duplicates);
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
accel_report.timestamp = hrt_absolute_time();
/*
* Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity.
* Temperature data is only valid between –40 °C and 125 °C. The temperature sensor
* output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the
* temperature sensor is uncalibrated and its output for a given temperature will vary from
* one device to the next
*/
write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_A | M_CTRL_REG1_OS(7));
_last_temperature = (read_reg(FXOS8700CQ_TEMP)) * 0.96f;
write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7));
accel_report.temperature = _last_temperature;
// report the error count as the sum of the number of bad
// register reads and bad values. This allows the higher level
// code to decide if it should use this sensor based on
// whether it has had failures
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
accel_report.x_raw = swap16RightJustify14(raw_accel_mag_report.x);
accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y);
accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z);
/* Save off the Mag readings todo: revist integrating theses */
_last_raw_mag_x = swap16(raw_accel_mag_report.mx);
_last_raw_mag_y = swap16(raw_accel_mag_report.my);
_last_raw_mag_z = swap16(raw_accel_mag_report.mz);
float xraw_f = accel_report.x_raw;
float yraw_f = accel_report.y_raw;
float zraw_f = accel_report.z_raw;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
/*
we have logs where the accelerometers get stuck at a fixed
large value. We want to detect this and mark the sensor as
being faulty
*/
if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
fabsf(_last_accel[1] - y_in_new) < 0.001f &&
fabsf(_last_accel[2] - z_in_new) < 0.001f &&
fabsf(x_in_new) > 20 &&
fabsf(y_in_new) > 20 &&
fabsf(z_in_new) > 20) {
_constant_accel_count += 1;
} else {
_constant_accel_count = 0;
}
if (_constant_accel_count > 100) {
// we've had 100 constant accel readings with large
// values. The sensor is almost certainly dead. We
// will raise the error_count so that the top level
// flight code will know to avoid this sensor, but
// we'll still give the data so that it can be logged
// and viewed
perf_count(_bad_values);
_constant_accel_count = 0;
}
_last_accel[0] = x_in_new;
_last_accel[1] = y_in_new;
_last_accel[2] = z_in_new;
accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
accel_report.x_integral = aval_integrated(0);
accel_report.y_integral = aval_integrated(1);
accel_report.z_integral = aval_integrated(2);
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
/* return device ID */
accel_report.device_id = _device_id.devid;
_accel_reports->force(&accel_report);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
}
_accel_read++;
/* stop the perf counter */
perf_end(_accel_sample_perf);
}
void QRtmp::setSecureToken(const QByteArray &data)
{
m_secureToken = data;
AVal secureToken = aval(m_secureToken);
RTMP_SetOpt(m_rtmp, &av_token, &secureToken);
}
int main(void) {
int n, i,h,count,count2,linia, linia2;
double *x, *y, *t, *z, val1, val2, val3, val4, pn, data;
/*s'obre el fitxer on es té totes les dades*/
FILE *fitxer = fopen("dades_apophis.dat","r");
FILE *resultat = fopen("resultats.dat","w");
if(fitxer==NULL){ /*es comprova que el fitxer*/
printf("Error a l'obrir el fitxer");
}
/*es fa un bucle per avaluar el polinomi interpolador en les distintes n's*/
for(i=6;i<=15;i+=3){
/*s'assigna un tamany de memoria per les variables x i y*/
x = (double *)malloc( (i+1)*sizeof(double *));
y = (double *)malloc( (i+1)*sizeof(double *));
/*es mira que no sigui NULL el valor obtingut*/
if(x == NULL || y == NULL){
printf("No hi ha prou memoria");
exit(1);
}
h = 414-(i+1); /*es calula en quina línia s'ha de començar a obtenir les variables */
/*contadors per saber en quina línia s'està llegint*/
count=0;
count2=0;
/*es llegeix cada línia i es guarden les variables de t,x,y,z en val1,val2... fins l'última línia*/
while ((fscanf(fitxer, "%le %le %le %le\n", &val1,&val2,&val3,&val4)) != EOF ){
if(count>=h){ /*quan s'arriba a la línia es guarden els valors de t i x del fitxer en l'array x i y*/
x[count2]=val1;
y[count2]=val2;
count2++;
printf("%d\n",count);
}
count++;
}
printf("end");
dif_dividides(x,y,i); /*obtenim el polinomi interpolador*/
pn = aval(x,y,i,2462225.5); /*s'avalua el polinomi interpolador en el punt 2462225.5 */
fprintf(resultat,"Per a n=%i, el resultat en avaluar el polinomi en t=246225.5 es %f\n", i,pn);
/*s'esborra la memoria de les variables x i y per tornar-les a omplir i es començar a llegir el fitxer des del principi*/
free(x);
free(y);
rewind(fitxer);
}
/*Jugador Filipe Luis del Chelsea, va neixer el 09/08/1985*/
n=7;
/*s'assigna un tamany de memoria per les variables t, x, y i z*/
t = (double *)malloc( (n+1)*sizeof(double *));
x = (double *)malloc( (n+1)*sizeof(double *));
y = (double *)malloc( (n+1)*sizeof(double *));
z = (double *)malloc( (n+1)*sizeof(double *));
data = datajuliana(28,1,2020); /*es crida a la funció datajuliana */
/*es calcula en quina línia es podrià trobar la data del jugador*/
linia = ((data-2453979.5)/20)-3;/*4 dies per sota de la data del jugador*/
linia2 = ((data-2453979.5)/20)+4;/*4 dies pe sobre de la dara del jugador*/
count=0;
count2=0;
while ((fscanf(fitxer, "%le %le %le %le\n", &val1,&val2,&val3,&val4)) != EOF ){
if(count>=linia && count<=linia2){/*obtenim els valors de les varibles del fitxer de les 8 línies i es guarden en 4 arrays*/
t[count2]=val1;
x[count2]=val2;
y[count2]=val3;
z[count2]=val4;
count2++;
}
count++;
}
/*es crea un fitxer anomenat resultats*/
fprintf(resultat,"En la data %f l\'asteroide estara en les cordenades: ",data);
/*es calcula els polinomis interpoladors de x,y i z i s'avalua en la data juliana del jugador i es guarda en el fitxer creat*/
dif_dividides(t,x,n);
pn = aval(t,x,n,data);
fprintf(resultat,"x:%f ",pn);
dif_dividides(t,y,n);
pn = aval(t,y,n,data);
fprintf(resultat,"y:%f ",pn);
dif_dividides(t,z,n);
pn = aval(t,z,n,data);
fprintf(resultat,"z:%f ",pn);
return 0;
}
void
MPU9250::measure()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct MPUReport mpu_report;
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the MPU9250 in one pass.
*/
if (OK != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED),
(uint8_t *)&mpu_report,
sizeof(mpu_report))) {
return;
}
check_registers();
if (check_duplicate(&mpu_report.accel_x[0])) {
return;
}
#ifdef USE_I2C
if (_mag->is_passthrough()) {
#endif
_mag->_measure(mpu_report.mag);
#ifdef USE_I2C
} else {
_mag->measure();
}
#endif
/*
* Convert from big to little endian
*/
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (check_null_data((uint32_t *)&report, sizeof(report) / 4)) {
return;
}
if (_register_wait != 0) {
// we are waiting for some good transfers before using the sensor again
// We still increment _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
* Swap axes and negate y
*/
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
/*
* Apply the swap
*/
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
/*
* Report buffers.
*/
accel_report arb;
gyro_report grb;
/*
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
_last_temperature = (report.temp) / 361.0f + 35.0f;
arb.temperature_raw = report.temp;
arb.temperature = _last_temperature;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
xraw_f = report.gyro_x;
yraw_f = report.gyro_y;
zraw_f = report.gyro_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
math::Vector<3> gval_integrated;
bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp;
grb.temperature = _last_temperature;
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
}
if (gyro_notify) {
_gyro->parent_poll_notify();
}
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (gyro_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
perf_end(_sample_perf);
}
int main() {
A aval(100);
B bval;
bval.change_A(aval);
}
