/*
* Increase window in response to successful acknowledgment.
*/
static void tcp_illinois_cong_avoid(struct sock *sk, u32 ack, u32 rtt,
u32 in_flight, int flag)
{
struct tcp_sock *tp = tcp_sk(sk);
struct illinois *ca = inet_csk_ca(sk);
if (after(ack, ca->end_seq))
update_params(sk);
/* RFC2861 only increase cwnd if fully utilized */
if (!tcp_is_cwnd_limited(sk, in_flight))
return;
/* In slow start */
if (tp->snd_cwnd <= tp->snd_ssthresh)
tcp_slow_start(tp);
else {
u32 delta;
/* snd_cwnd_cnt is # of packets since last cwnd increment */
tp->snd_cwnd_cnt += ca->acked;
ca->acked = 1;
/* This is close approximation of:
* tp->snd_cwnd += alpha/tp->snd_cwnd
*/
delta = (tp->snd_cwnd_cnt * ca->alpha) >> ALPHA_SHIFT;
if (delta >= tp->snd_cwnd) {
tp->snd_cwnd = min(tp->snd_cwnd + delta / tp->snd_cwnd,
(u32) tp->snd_cwnd_clamp);
tp->snd_cwnd_cnt = 0;
}
}
}
static void
my_apm_get_power_status(struct apm_power_info *info)
{
if (!info) return;
if (time_after64(jiffies_64, last_update + speriod * HZ)) {
if (!update_params()) {
schedule_delayed_work(&battery_work, HZ);
}
}
info->ac_line_status = onAC ? APM_AC_ONLINE : APM_AC_OFFLINE;
if (batteryCharging) {
info->battery_status = APM_BATTERY_STATUS_CHARGING;
} else {
if (batteryPresent) {
info->battery_status = APM_BATTERY_STATUS_HIGH;
} else {
info->battery_status = APM_BATTERY_STATUS_NOT_PRESENT;
}
}
info->battery_life = rsoc;
info->time = time;
info->units = APM_UNITS_MINS;
info->battery_flag = (1 << info->battery_status);
}
Gimbal::Gimbal() :
CDev("Gimbal", GIMBAL_DEVICE_PATH),
_vehicle_command_sub(-1),
_att_sub(-1),
_vehicle_control_mode_sub(-1),
_manual_control_sub(-1),
_params_sub(-1),
_attitude_compensation_roll(true),
_attitude_compensation_pitch(true),
_attitude_compensation_yaw(true),
_initialized(false),
_actuator_controls_2_topic(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")),
_comms_errors(perf_alloc(PC_COUNT, "gimbal_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "gimbal_buffer_overflows"))
{
// disable debug() calls
_debug_enabled = false;
_params_handles.aux_mnt_chn = param_find("GMB_AUX_MNT_CHN");
_params_handles.use_mnt = param_find("GMB_USE_MNT");
update_params();
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
long double Method::optimize(Problem &problem)
{
long double error = numeric_limits<long double>::max();
error = problem.call_function();
update_params(problem);
error = error_function(error);
return error;
}
/* **********************
* update_params in ntw
* **********************/
tee_stat_t tee_msgm_update_params(tee_msgm_t msgm, tee_msg_op_info_t *params)
{
tee_msgm_ntw_struct *ntw_handle = NULL;
tee_stat_t update_params_stat = TEEC_SUCCESS;
OSA_ASSERT((msgm && params));
ntw_handle = (tee_msgm_ntw_struct *) msgm;
OSA_ASSERT(IS_MAGIC_VALID(ntw_handle->magic));
update_params_stat = update_params(msgm, params);
return update_params_stat;
}
static int
get_property(struct power_supply *b, enum power_supply_property psp,
union power_supply_propval *val)
{
if (time_after64(jiffies_64, last_update + speriod * HZ)) {
int retry = 10;
while (!update_params()) {
msleep(200);
if (!--retry) {
printk(KERN_ERR "ghi270_power: "
"Battery comm fail\n");
break;
}
}
}
switch (psp) {
case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
val->intval = 100;
break;
case POWER_SUPPLY_PROP_CURRENT_NOW:
val->intval = current_now;
break;
case POWER_SUPPLY_PROP_CHARGE_EMPTY_DESIGN:
val->intval = 0;
break;
case POWER_SUPPLY_PROP_CHARGE_NOW:
val->intval = rsoc;
break;
case POWER_SUPPLY_PROP_VOLTAGE_NOW:
val->intval = voltage;
break;
case POWER_SUPPLY_PROP_STATUS:
if (batteryCharging) {
val->intval = POWER_SUPPLY_STATUS_CHARGING;
} else {
val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
}
break;
case POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW:
val->intval = time;
break;
case POWER_SUPPLY_PROP_TEMP:
val->intval = temp;
break;
default:
return -EINVAL;
};
return 0;
}
/*******************************************************
* "Soft" real-time event handler for slow rate
********************************************************/
void slow_event_handler(void)
{
if(slow_event_count > slow_ticks_limit)
{
slow_event_count = 0;
if(control_flags.first_scan)
{
putsUART((unsigned char *)WelcomeMsg,&UART1);
//putsUART((unsigned char *)WelcomeMsg,&UART2);
control_flags.first_scan = 0;
}
// (RAM) Parameters update management
if(control_flags.PAR_update_req)
{
update_params();
control_flags.PAR_update_req = 0;
}
if(direction_flags.word != direction_flags_prev)
{
// RESET COUNTS
QEI1_Init();
QEI2_Init();
Timer1_Init();
Timer4_Init();
direction_flags_prev = direction_flags.word;
}
// EEPROM update management
if(control_flags.EE_update_req)
{
control_flags.EE_update_req = 0;
}
update_delta_joints();
update_delta_EE();//aggiornamento delle strutture dati
status_flags.homing_done = home_f.done;
// SACT protocol timeout manager (see SACT_protocol.c)
SACT_timeout();
SACT_SendSDP();
SACT_SendSSP();
// CONTROL MODE STATE MANAGER
control_mode_manager();
} // END IF slow_event_count..
}// END slow_event_handler
double EM(
const vector< pair< int, int > > & data, // [ ( coverage, majority ) ]
vector< pair< double, double > > & params // [ ( weight, rate ) ]
)
{
double * pij = new double[ data.size() * params.size() ];
double lg_L;
if ( params.size() == 1 ) {
int sum_cov = 0, sum_maj = 0;
for ( unsigned i = 0; i < data.size(); ++i ) {
sum_cov += data[ i ].first;
sum_maj += data[ i ].second;
}
params[ 0 ].first = 1.0;
if ( sum_cov == 0 )
params[ 0 ].second = 1.0;
else
params[ 0 ].second = double( sum_maj ) / double( sum_cov ); // or inverse of this?
}
else {
lg_L = lg_likelihood( pij, data, params );
for( int i = 0; i < 100; ++i ) {
double new_lg_L;
update_params( pij, data, params );
new_lg_L = lg_likelihood( pij, data, params );
if ( fabs( lg_L - new_lg_L ) < 1e-8 )
break;
lg_L = new_lg_L;
}
}
lg_L = lg_likelihood( pij, data, params, true );
delete [] pij;
return lg_L;
}
static bool update_settings(struct obs_x264 *obsx264, obs_data_t *settings)
{
char *preset = bstrdup(obs_data_get_string(settings, "preset"));
char *profile = bstrdup(obs_data_get_string(settings, "profile"));
char *tune = bstrdup(obs_data_get_string(settings, "tune"));
const char *opts = obs_data_get_string(settings, "x264opts");
char **paramlist;
bool success = true;
paramlist = strlist_split(opts, ' ', false);
blog(LOG_INFO, "---------------------------------");
if (!obsx264->context) {
override_base_params(obsx264, paramlist,
&preset, &profile, &tune);
if (preset && *preset) info("preset: %s", preset);
if (profile && *profile) info("profile: %s", profile);
if (tune && *tune) info("tune: %s", tune);
success = reset_x264_params(obsx264, preset, tune);
}
if (success) {
update_params(obsx264, settings, paramlist);
if (opts && *opts)
info("custom settings: %s", opts);
if (!obsx264->context)
apply_x264_profile(obsx264, profile);
}
obsx264->params.b_repeat_headers = false;
strlist_free(paramlist);
bfree(preset);
bfree(profile);
bfree(tune);
return success;
}
static void
battery_work_callback(struct work_struct *data)
{
static int failcount = 0;
static int failflag = 0;
if (!update_params()) {
if (!failflag) failcount++;
if (failcount == 10) {
printk(KERN_ERR "Cannot communicate with "
"battery controller.\n");
failflag = 1;
failcount = 0;
}
schedule_delayed_work(&battery_work, HZ);
} else {
if (failflag) {
failflag = 0;
printk(KERN_ERR "Battery OK now\n");
}
failcount = 0;
}
}
void new_game()
{
unsigned long count = 0;
BOOL is_going_down = TRUE;
unit_box boxes[MAX_TOTAL];
memset(boxes, 0, sizeof(boxes));
while(1)
{
update_params(boxes, box, row, column);
is_going_down = TRUE;
if(0 == generate(boxes, (INDEX)0, (INDEX) 0, (INDEX)0, &is_going_down))
{
printf("Total Possible Combination = %lu", count);
return;
}
count++;
if(debug)
{
printf("\nIter : %d", count);
}
//print_matrix(boxes);
}
}
/*******************************************************
* MAIN function, just setup some inits and loops
* "soft" real-time event handlers, defined hereafter
********************************************************/
int main(void)
{
// configuro l'oscillatore interno che mi fornisce Tcy
// Fosc = Fin (M/(N1*N2))
// FCY = Fosc/2
PLLFBD = 39; // M = 40
CLKDIVbits.PLLPOST=0; // N2 = 2
CLKDIVbits.PLLPRE=0; // N1 = 2
RCONbits.SWDTEN = 0; //disabilito il watchdog
DataEEInit();
//Init Peripheral Pin Selection (QEI and UART)
PPS_Init();
control_flags.first_scan = 1;
slow_ticks_limit = SLOW_RATE * (FCY_PWM / 1000) - 1 ;
medium_ticks_limit = MEDIUM_RATE * (FCY_PWM / 1000) - 1;
mposition1 = zero_pos1;//parto dalla posizione iniziale 90 90 90
mposition2 = zero_pos2;
mposition3 = zero_pos3;
/*mtheta1 = 0;
mtheta2 = 0;
mtheta3 = 0;
x_cart = 0;
y_cart = 0;
z_cart = 0;*/
coordinates_actual.x = 0;
coordinates_actual.y = 0;
coordinates_actual.z = 0;
coordinates_temp.x = 0;
coordinates_temp.y = 0;
coordinates_temp.z = 0;
angleJoints_actual.theta1 = 0;
angleJoints_actual.theta2 = 0;
angleJoints_actual.theta3 = 0;
angleJoints_temp.theta1 = 0;
angleJoints_temp.theta2 = 0;
angleJoints_temp.theta3 = 0;
update_params();
direction_flags_prev = direction_flags.word;
// UARTs init
// no need to set TRISx, they are "Module controlled"
UART1_Init();
// Setup control pins and PWM module,
// which is needed also to schedule "soft"
// real-time tasks w/PWM interrupt tick counts
DIR1 = direction_flags.motor1_dir;//0;
DIR2 = direction_flags.motor2_dir;//1;
DIR3 = direction_flags.motor3_dir;
//BRAKE1 = 0;
//BRAKE2 = 0;
DIR1_TRIS = OUTPUT;
DIR2_TRIS = OUTPUT;
DIR3_TRIS = OUTPUT;
//BRAKE1_TRIS = OUTPUT;
//BRAKE2_TRIS = OUTPUT;
CURRSENSE1_TRIS = INPUT;
CURRSENSE2_TRIS = INPUT;
CURRSENSE3_TRIS = INPUT;
PWM_Init();
// MUST SETUP ALSO ANALOG PINS AS INPUTS
AN0_TRIS = INPUT;
AN1_TRIS = INPUT;
AN2_TRIS = INPUT;
ADC_Init();
DMA0_Init();
// SETUP ENCODER INPUTS
// QEI inputs are "module controlled"
// -> no need to set TRISx
QEI1_Init();
QEI2_Init();
// Timers used to acquire Encoder 3
// corresponding PINS set as inputs
T1CK_TRIS = INPUT;
T4CK_TRIS = INPUT;
Timer1_Init();
Timer2_Init();
Timer4_Init();
// Timer5 used to schedule POSITION loops
Timer5_Init();
//Input capture
IC1_Init();
IC2_Init();
// TEST PIN
TEST_PIN_TRIS = OUTPUT;
TEST_PIN = FALSE;
while(1)//a ciclo infinito ripeto queste 2 routine
{
medium_event_handler();
slow_event_handler();
}
return 0; //code should never get here
}// END MAIN()
void
Syslink::task_main()
{
_bridge = new SyslinkBridge(this);
_bridge->init();
_memory = new SyslinkMemory(this);
_memory->init();
_battery.reset(&_battery_status);
// int ret;
/* Open serial port */
const char *dev = "/dev/ttyS2";
_fd = open_serial(dev);
if (_fd < 0) {
err(1, "can't open %s", dev);
return;
}
/* Set non-blocking */
/*
int flags = fcntl(_fd, F_GETFL, 0);
fcntl(_fd, F_SETFL, flags | O_NONBLOCK);
*/
px4_arch_configgpio(GPIO_NRF_TXEN);
px4_pollfd_struct_t fds[2];
fds[0].fd = _fd;
fds[0].events = POLLIN;
_params_sub = orb_subscribe(ORB_ID(parameter_update));
fds[1].fd = _params_sub;
fds[1].events = POLLIN;
int error_counter = 0;
char buf[64];
int nread;
syslink_parse_state state;
syslink_message_t msg;
syslink_parse_init(&state);
//setup initial parameters
update_params(true);
while (_task_running) {
int poll_ret = px4_poll(fds, 2, 500);
/* handle the poll result */
if (poll_ret == 0) {
/* timeout: this means none of our providers is giving us data */
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) {
/* use a counter to prevent flooding (and slowing us down) */
PX4_ERR("[syslink] ERROR return value from poll(): %d"
, poll_ret);
}
error_counter++;
} else {
if (fds[0].revents & POLLIN) {
if ((nread = read(_fd, buf, sizeof(buf))) < 0) {
continue;
}
for (int i = 0; i < nread; i++) {
if (syslink_parse_char(&state, buf[i], &msg)) {
handle_message(&msg);
}
}
}
if (fds[1].revents & POLLIN) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
update_params(false);
}
}
}
close(_fd);
}
static long init_record(epidRecord *pepid)
{
struct instio *pinstio;
asynStatus status;
asynUser *pasynUser;
asynInterface *pasynInterface;
epidFastPvt *pPvt;
char *tok_save;
char *p;
const char *drvUserName;
size_t drvUserSize;
asynDrvUser *pdrvUser;
void *drvUserPvt;
char temp[256];
void *registrarPvt;
pPvt = callocMustSucceed(1, sizeof(*pPvt), "devEpidFast::init_record");
pepid->dpvt = pPvt;
pPvt->KP = 1;
pPvt->lowLimit = 1.;
pPvt->highLimit =-1.;
pinstio = (struct instio*)&(pepid->inp.value);
/* Parse to get inputName, inputChannel, dataString, intervalString,
* outputName, outputChannel, outputString
* Copy to temp, since epicsStrtok_r overwrites it */
strcpy(temp, pinstio->string);
strcat(temp, " ");
strcat(temp, pepid->desc);
tok_save = NULL;
p = epicsStrtok_r(temp, ", ", &tok_save);
if (!p) {
errlogPrintf("devEpidFast::init_record %s, INP field=\"%s\" is missing inputName\n",
pepid->name, temp);
goto bad;
}
pPvt->inputName = epicsStrDup(p);
p = epicsStrtok_r(NULL, ", ", &tok_save);
if (!p) {
errlogPrintf("devEpidFast::init_record %s, INP field=\"%s\" is missing inputChannel\n",
pepid->name, temp);
goto bad;
}
pPvt->inputChannel = atoi(p);
p = epicsStrtok_r(NULL, ", ", &tok_save);
if (!p) {
errlogPrintf("devEpidFast::init_record %s, INP field=\"%s\" is missing inputDataString\n",
pepid->name, temp);
goto bad;
}
pPvt->inputDataString = epicsStrDup(p);
p = epicsStrtok_r(NULL, ", ", &tok_save);
if (!p) {
errlogPrintf("devEpidFast::init_record %s, INP field=\"%s\" is missing inputIntervalString\n",
pepid->name, temp);
goto bad;
}
pPvt->inputIntervalString = epicsStrDup(p);
p = epicsStrtok_r(NULL, ", ", &tok_save);
if (!p) {
errlogPrintf("devEpidFast::init_record %s, INP field=\"%s\" is missing outputName\n",
pepid->name, temp);
goto bad;
}
pPvt->outputName = epicsStrDup(p);
p = epicsStrtok_r(NULL, ", ", &tok_save);
if (!p) {
errlogPrintf("devEpidFast::init_record %s, INP field=\"%s\" is missing outputChannel\n",
pepid->name, temp);
goto bad;
}
pPvt->outputChannel = atoi(p);
p = epicsStrtok_r(NULL, ", ", &tok_save);
if (!p) {
errlogPrintf("devEpidFast::init_record %s, INP field=\"%s\" is missing outputDataString\n",
pepid->name, temp);
goto bad;
}
pPvt->outputDataString = epicsStrDup(p);
pPvt->mutexId = epicsMutexCreate();
/* Connect to input asyn driver */
pasynUser = pasynManager->createAsynUser(0, 0);
pPvt->pcallbackDataAsynUser = pasynUser;
status = pasynManager->connectDevice(pasynUser, pPvt->inputName,
pPvt->inputChannel);
if (status != asynSuccess) {
errlogPrintf("devEpidFast::init_record, %s error in connectDevice"
" to input %s\n",
pepid->name, pasynUser->errorMessage);
goto bad;
}
pasynInterface = pasynManager->findInterface(pasynUser,
asynFloat64Type, 1);
if (!pasynInterface) {
errlogPrintf("devEpidFast::init_record %s, cannot find "
"asynFloat64 interface %s\n",
pepid->name, pasynUser->errorMessage);
goto bad;
}
pPvt->pfloat64Input = (asynFloat64 *) pasynInterface->pinterface;
pPvt->float64InputPvt = pasynInterface->drvPvt;
pasynInterface = pasynManager->findInterface(pasynUser,
asynDrvUserType, 1);
if (!pasynInterface) {
errlogPrintf("devEpidFast::init_record %s, cannot find "
"asynDrvUser interface %s\n",
pepid->name, pasynUser->errorMessage);
goto bad;
}
pdrvUser = (asynDrvUser *)pasynInterface->pinterface;
drvUserPvt = pasynInterface->drvPvt;
status = pdrvUser->create(drvUserPvt, pPvt->pcallbackDataAsynUser,
pPvt->inputDataString, &drvUserName, &drvUserSize);
if (status) {
errlogPrintf("devEpidFast::init_record %s, asynDrvUser->create "
"failed for input data string %s %s\n",
pepid->name, pPvt->inputDataString, pPvt->pcallbackDataAsynUser->errorMessage);
goto bad;
}
status = pPvt->pfloat64Input->registerInterruptUser(pPvt->float64InputPvt,
pPvt->pcallbackDataAsynUser,
dataCallback, pPvt, ®istrarPvt);
if (status) {
errlogPrintf("devEpidFast::init_record %s, pfloat64Input->registerInterruptUser failed for dataCallback %s\n",
pepid->name, pPvt->pcallbackDataAsynUser->errorMessage);
goto bad;
}
pasynUser = pasynManager->createAsynUser(0, 0);
pPvt->pcallbackIntervalAsynUser = pasynUser;
status = pasynManager->connectDevice(pasynUser, pPvt->inputName, 0);
status = pdrvUser->create(drvUserPvt, pPvt->pcallbackIntervalAsynUser,
pPvt->inputIntervalString, &drvUserName, &drvUserSize);
if (status) {
errlogPrintf("devEpidFast::init_record %s, asynDrvUser->create "
"failed for interval string %s %s\n",
pepid->name, pPvt->inputIntervalString, pPvt->pcallbackIntervalAsynUser->errorMessage);
goto bad;
}
status = pPvt->pfloat64Input->registerInterruptUser(pPvt->float64InputPvt,
pPvt->pcallbackIntervalAsynUser,
intervalCallback, pPvt,
®istrarPvt);
if (status) {
errlogPrintf("devEpidFast::init_record %s, pfloat64Input->registerInterruptUser failed for intervalCallback %s\n",
pepid->name, pPvt->pcallbackIntervalAsynUser->errorMessage);
goto bad;
}
status = pPvt->pfloat64Input->read(pPvt->float64InputPvt,
pPvt->pcallbackIntervalAsynUser,
&pPvt->callbackInterval);
if (status) {
errlogPrintf("devEpidFast::init_record %s, pfloat64Input->read failed for callbackInterval %s\n",
pepid->name, pPvt->pcallbackIntervalAsynUser->errorMessage);
goto bad;
}
/* Connect to output asyn driver */
pasynUser = pasynManager->createAsynUser(0, 0);
pPvt->pfloat64OutputAsynUser = pasynUser;
status = pasynManager->connectDevice(pasynUser, pPvt->outputName,
pPvt->outputChannel);
if (status != asynSuccess) {
errlogPrintf("devEpidFast::init_record %s, error in connectDevice"
" to output %s\n",
pepid->name, pasynUser->errorMessage);
goto bad;
}
pasynInterface = pasynManager->findInterface(pasynUser,
asynFloat64Type, 1);
if (!pasynInterface) {
errlogPrintf("devEpidFast::init_record %s, cannot find "
"asynFloat64 interface %s\n",
pepid->name, pasynUser->errorMessage);
goto bad;
}
pPvt->pfloat64Output = (asynFloat64 *)pasynInterface->pinterface;
pPvt->float64OutputPvt = pasynInterface->drvPvt;
pasynInterface = pasynManager->findInterface(pasynUser,
asynDrvUserType, 1);
if (!pasynInterface) {
errlogPrintf("devEpidFast::init_record %s, cannot find "
"asynDrvUser interface %s\n",
pepid->name, pasynUser->errorMessage);
goto bad;
}
pdrvUser = (asynDrvUser *)pasynInterface->pinterface;
drvUserPvt = pasynInterface->drvPvt;
status = pdrvUser->create(drvUserPvt, pPvt->pfloat64OutputAsynUser,
pPvt->outputDataString, &drvUserName, &drvUserSize);
if (status) {
errlogPrintf("devEpidFast::init_record %s, asynDrvUser->create "
"failed for output data string %s status=%d, drvUserSize=%d %s\n",
pepid->name, pPvt->outputDataString, status, (int)drvUserSize, pPvt->pfloat64OutputAsynUser->errorMessage);
goto bad;
}
update_params(pepid);
return(0);
bad:
pepid->pact=1;
return(0);
}
void process_data_stream(void)
{
ui08 *beam_buffer;
ui08 *param_buffer;
ui08 *gate_params_pkt;
ui08 *current_gate_data_pkt, *prev_gate_data_pkt;
ui08 *tmp_gate_data_pkt;
int forever = TRUE;
int prev_end_of_tilt;
int params_initialized = FALSE;
int header_count = 0;
int summary_count = 0;
int beam_valid, prev_beam_valid;
int params_have_changed;
si32 gate_params_pkt_seq_no = 0;
si32 gate_data_pkt_seq_no = 0;
si32 nbytes_params_pkt;
si32 nbytes_data_pkt;
gate_data_radar_params_t *rparams;
/*
* allocate beam buffer
*/
nbytes_data_pkt = sizeof(gate_data_beam_header_t) +
(Glob->params.file_info.len * Glob->params.ngates_out);
beam_buffer = (ui08 *)umalloc((ui32)nbytes_data_pkt);
/*
* allocate the param buffer
*/
nbytes_params_pkt = sizeof(gate_data_radar_params_t) +
Glob->params.file_info.len * sizeof(gate_data_field_params_t);
param_buffer = (ui08 *)umalloc((ui32)nbytes_params_pkt);
/*
* allocate parameters packet
*/
gate_params_pkt = (ui08 *) umalloc ((ui32) nbytes_params_pkt);
rparams = (gate_data_radar_params_t *) gate_params_pkt;
/*
* allocate the space for the data packet, including space for
* storing the immediately previous one
*/
current_gate_data_pkt = (ui08 *) umalloc ((ui32) nbytes_data_pkt);
prev_gate_data_pkt = (ui08 *) umalloc ((ui32) nbytes_data_pkt);
/*
* enter loop
*/
while (forever)
{
/*
* read in a beam from the current file. Try twice since
* we get an invalid beam at the end of a volume but we still
* want to be able to detect the end of volume condition using
* the beams consecutively.
*/
beam_valid = read_disk_ufnexrad(beam_buffer, param_buffer);
if (!beam_valid)
beam_valid = read_disk_ufnexrad(beam_buffer, param_buffer);
/*
* for valid beams, print out as required
*/
if (beam_valid)
{
/*
* print header if requested
*/
if (Glob->params.header_print)
{
if (header_count == 0)
{
print_radar_params(param_buffer);
print_beam_buffer(beam_buffer);
}
header_count++;
if (header_count == Glob->params.header_interval)
header_count = 0;
}
/*
* print summary if requested
*/
if (Glob->params.summary_print)
{
if (summary_count == 0)
print_summary(beam_buffer, param_buffer);
summary_count++;
if (summary_count == Glob->params.summary_interval)
summary_count = 0;
}
} /* if(beam_valid) */
if (!params_initialized)
{
/*
* looking for the first valid beam for initialization
*/
if (beam_valid)
{
/*
* initialize
*/
params_have_changed =
update_params (param_buffer, gate_params_pkt,
nbytes_params_pkt);
printf("Initial radar parameters:\n");
print_radar_params(param_buffer);
/*
* send initial params packet
*/
if (transmit(gate_params_pkt,
nbytes_params_pkt,
(si32) GATE_PARAMS_PACKET_CODE,
gate_params_pkt_seq_no) == 0)
{
gate_params_pkt_seq_no++;
/*
* set up the first beam buffer
*/
memcpy((void *)current_gate_data_pkt,
(void *)beam_buffer,
(size_t)nbytes_data_pkt);
memcpy((void *)gate_params_pkt,
(void *)param_buffer,
(size_t)nbytes_params_pkt);
prev_beam_valid = TRUE;
params_initialized = TRUE;
}
} /* if (beam_valid) */
}
else
{
/*
* initialization has been done
*/
if (beam_valid)
{
/*
* beam valid
*/
if (prev_beam_valid)
{
/*
* set prev beam pointers to previously obtained beam
*/
tmp_gate_data_pkt = prev_gate_data_pkt;
prev_gate_data_pkt = current_gate_data_pkt;
current_gate_data_pkt = tmp_gate_data_pkt;
/*
* check if params have changed
*/
params_have_changed =
update_params (param_buffer, gate_params_pkt,
nbytes_params_pkt);
/*
* load in the new beam
*/
memcpy((void *)current_gate_data_pkt,
(void *)beam_buffer,
(size_t)nbytes_data_pkt);
memcpy((void *)gate_params_pkt,
(void *)param_buffer,
(size_t)nbytes_params_pkt);
/*
* set beam flags - if returns TRUE, tilt has changed
* so set flag
*/
prev_end_of_tilt = set_beam_flags(current_gate_data_pkt,
prev_gate_data_pkt);
/*
* write out prev beam
*/
BE_from_array_32((ui32 *) prev_gate_data_pkt,
(ui32) sizeof(gate_data_beam_header_t));
if (transmit(prev_gate_data_pkt,
nbytes_data_pkt,
(si32) GATE_DATA_PACKET_CODE,
gate_data_pkt_seq_no) == 0) {
gate_data_pkt_seq_no++;
}
/*
* if params have changed, write them out
*/
if (params_have_changed || prev_end_of_tilt)
{
printf("New radar parameters:\n");
print_radar_params(param_buffer);
if (transmit(gate_params_pkt,
nbytes_params_pkt,
(si32) GATE_PARAMS_PACKET_CODE,
gate_params_pkt_seq_no) == 0) {
gate_params_pkt_seq_no++;
}
}
}
else
{
/*
* prev beam not valid, so the outgoing beam will
* already have been written out. Therefore, just
* load in incoming beam
*/
memcpy((void *)current_gate_data_pkt,
(void *)beam_buffer,
(size_t)nbytes_data_pkt);
memcpy((void *)gate_params_pkt,
(void *)param_buffer,
(size_t)nbytes_params_pkt);
} /* if (prev_beam_valid) */
prev_beam_valid = TRUE;
}
else
{
/*
* beam not valid
*/
if (prev_beam_valid)
{
/*
* Read in the invalid beam, and set the
* flags on the previous beam, which was valid.
* Then write out prev beam
*/
tmp_gate_data_pkt = prev_gate_data_pkt;
prev_gate_data_pkt = current_gate_data_pkt;
current_gate_data_pkt = tmp_gate_data_pkt;
memcpy((void *)current_gate_data_pkt,
(void *)beam_buffer,
(size_t)nbytes_data_pkt);
memcpy((void *)gate_params_pkt,
(void *)param_buffer,
(size_t)nbytes_params_pkt);
/*
* set beam flags - if returns TRUE, tilt has changed
* so set flag
*/
prev_end_of_tilt = set_beam_flags(current_gate_data_pkt,
prev_gate_data_pkt);
BE_from_array_32((ui32 *) prev_gate_data_pkt,
(ui32) sizeof(gate_data_beam_header_t));
if (transmit(prev_gate_data_pkt,
nbytes_data_pkt,
(si32) GATE_DATA_PACKET_CODE,
gate_data_pkt_seq_no) == 0)
{
gate_data_pkt_seq_no++;
}
/*
* if prev beam was end of tilt, write out params
* because they may change for new tilt
*/
if (prev_end_of_tilt)
{
if (transmit(gate_params_pkt,
nbytes_params_pkt,
(si32) GATE_PARAMS_PACKET_CODE,
gate_params_pkt_seq_no) == 0)
{
gate_params_pkt_seq_no++;
}
}
} /* if (prev_beam_valid) */
prev_beam_valid = FALSE;
} /* if (beam_valid) */
} /* if (!params_initialized) */
} /* while */
}
static int
detect(struct i2c_adapter *adapter, int address, int kind)
{
int err = 0;
if (address == 9) {
/* LTC4100 */
if (!i2c_check_functionality(adapter,
I2C_FUNC_SMBUS_WORD_DATA)) {
goto exit;
}
memset(&client_ltc4100, 0, sizeof(client_ltc4100));
i2c_set_clientdata(&client_ltc4100, &client_ltc4100);
client_ltc4100.addr = address;
client_ltc4100.adapter = adapter;
client_ltc4100.driver = &ghi270_driver;
strlcpy(client_ltc4100.name, "ghi270_ltc4100", I2C_NAME_SIZE);
if (0x2 !=
(err = i2c_smbus_read_word_data(&client_ltc4100, 0x11))) {
if (err != -1) {
printk(KERN_ERR "Did not read expected\n");
}
}
if ((err = i2c_attach_client(&client_ltc4100))) {
goto exit;
}
INIT_DELAYED_WORK(&battery_work, battery_work_callback);
if (!update_params()) {
mdelay(100);
if (!update_params()) {
mdelay(100);
if (!update_params()) {
printk(KERN_ERR "Battery update fail\n");
}
}
}
# ifdef CONFIG_POWER_SUPPLY
power_supply_register(0, &battery_info);
# endif
ltc4100_attached = 1;
} else {
/* bq20z80 */
if (bq20z80_attached) {
printk(KERN_ERR "Already Attached!!\n");
return 0;
}
memset(&client_bq20z80, 0, sizeof(client_bq20z80));
i2c_set_clientdata(&client_bq20z80, &client_bq20z80);
client_bq20z80.addr = address;
client_bq20z80.adapter = adapter;
client_bq20z80.driver = &ghi270_driver;
strlcpy(client_bq20z80.name, "ghi270_bq20z80", I2C_NAME_SIZE);
/* This device may not be on the i2c bus at any given time
* because it is in the battery itself. */
if ((err = i2c_attach_client(&client_bq20z80))) {
goto exit;
}
bq20z80_attached = 1;
}
return 0;
exit:
return err;
}
void r300TranslateFragmentShader(r300ContextPtr r300,
struct r300_fragment_program *fp)
{
struct r300_fragment_program_external_state state;
build_state(r300, fp, &state);
if (_mesa_memcmp(&fp->state, &state, sizeof(state))) {
/* TODO: cache compiled programs */
fp->translated = GL_FALSE;
_mesa_memcpy(&fp->state, &state, sizeof(state));
}
if (!fp->translated) {
struct r300_fragment_program_compiler compiler;
compiler.r300 = r300;
compiler.fp = fp;
compiler.code = &fp->code;
compiler.program = _mesa_clone_program(r300->radeon.glCtx, &fp->mesa_program.Base);
if (RADEON_DEBUG & DEBUG_PIXEL) {
_mesa_printf("Fragment Program: Initial program:\n");
_mesa_print_program(compiler.program);
}
insert_WPOS_trailer(&compiler);
struct radeon_program_transformation transformations[] = {
{ &transform_TEX, &compiler },
{ &radeonTransformALU, 0 },
{ &radeonTransformTrigSimple, 0 }
};
radeonLocalTransform(
r300->radeon.glCtx,
compiler.program,
3, transformations);
if (RADEON_DEBUG & DEBUG_PIXEL) {
_mesa_printf("Fragment Program: After native rewrite:\n");
_mesa_print_program(compiler.program);
}
struct radeon_nqssadce_descr nqssadce = {
.Init = &nqssadce_init,
.IsNativeSwizzle = &r300FPIsNativeSwizzle,
.BuildSwizzle = &r300FPBuildSwizzle,
.RewriteDepthOut = GL_TRUE
};
radeonNqssaDce(r300->radeon.glCtx, compiler.program, &nqssadce);
if (RADEON_DEBUG & DEBUG_PIXEL) {
_mesa_printf("Compiler: after NqSSA-DCE:\n");
_mesa_print_program(compiler.program);
}
if (!r300FragmentProgramEmit(&compiler))
fp->error = GL_TRUE;
/* Subtle: Rescue any parameters that have been added during transformations */
_mesa_free_parameter_list(fp->mesa_program.Base.Parameters);
fp->mesa_program.Base.Parameters = compiler.program->Parameters;
compiler.program->Parameters = 0;
_mesa_reference_program(r300->radeon.glCtx, &compiler.program, NULL);
if (!fp->error)
fp->translated = GL_TRUE;
if (fp->error || (RADEON_DEBUG & DEBUG_PIXEL))
r300FragmentProgramDump(fp, &fp->code);
r300UpdateStateParameters(r300->radeon.glCtx, _NEW_PROGRAM);
}
update_params(r300, fp);
}
void
Gimbal::cycle()
{
if (!_initialized) {
/* get subscription handles */
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
/* get a publication handle on actuator output topic */
struct actuator_controls_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
zero_report.timestamp = hrt_absolute_time();
_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
if (_actuator_controls_2_topic == nullptr) {
warnx("advert err");
}
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
_initialized = true;
}
bool updated = false;
perf_begin(_sample_perf);
float roll = 0.0f;
float pitch = 0.0f;
float yaw = 0.0f;
float out_mount_mode = 0.0f;
/* Parameter update */
bool params_updated;
orb_check(_params_sub, ¶ms_updated);
if (params_updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); // XXX: is this actually necessary?
update_params();
}
/* Control mode update */
bool vehicle_control_mode_updated;
orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated);
if (vehicle_control_mode_updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
}
/* Check attitude */
struct vehicle_attitude_s att;
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
if (_attitude_compensation_roll) {
roll = 1.0f / M_PI_F * -att.roll;
updated = true;
}
if (_attitude_compensation_pitch) {
pitch = 1.0f / M_PI_F * -att.pitch;
updated = true;
}
if (_attitude_compensation_yaw) {
yaw = 1.0f / M_PI_F * att.yaw;
updated = true;
}
/* Check manual input */
bool manual_updated;
orb_check(_manual_control_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control);
/* only check manual input for mount mode when not in offboard and aux chan is configured */
if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) {
float aux = 0.0f;
switch (_parameters.aux_mnt_chn) {
case 1:
aux = _manual_control.aux1;
break;
case 2:
aux = _manual_control.aux2;
break;
case 3:
aux = _manual_control.aux3;
break;
}
if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) {
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
updated = true;
}
if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
updated = true;
}
}
}
/* Check command input */
struct vehicle_command_s cmd;
bool cmd_updated;
orb_check(_vehicle_command_sub, &cmd_updated);
if (cmd_updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
_control_cmd = cmd;
_control_cmd_set = true;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_config_cmd = cmd;
_config_cmd_set = true;
}
}
if (_config_cmd_set) {
_config_cmd_set = false;
_attitude_compensation_roll = (int)_config_cmd.param2 == 1;
_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;
/* only check commanded mount mode when in offboard */
if (_control_mode.flag_control_offboard_enabled &&
fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) {
_mount_mode = int(_config_cmd.param1 + 0.5f);
updated = true;
}
}
if (_control_cmd_set) {
unsigned mountMode = _control_cmd.param7;
DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode,
(double)_control_cmd.param1, (double)_control_cmd.param2);
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
updated = true;
}
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
roll += gimablDirectionEuler(0);
pitch += gimablDirectionEuler(1);
yaw += gimablDirectionEuler(2);
updated = true;
}
}
/* consider mount mode if parameter is set */
if (_parameters.use_mnt > 0) {
switch (_mount_mode) {
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
out_mount_mode = -1.0f;
roll = 0.0f;
pitch = 0.0f;
yaw = 0.0f;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
out_mount_mode = 1.0f;
break;
default:
out_mount_mode = -1.0f;
}
}
if (updated) {
struct actuator_controls_s controls;
// DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);
/* fill in the final control values */
controls.timestamp = hrt_absolute_time();
controls.control[0] = roll;
controls.control[1] = pitch;
controls.control[2] = yaw;
//controls.control[3] = ; // camera shutter
controls.control[4] = out_mount_mode;
/* publish it */
orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
perf_end(_sample_perf);
/* schedule a fresh cycle call when the measurement is done */
work_queue(LPWORK,
&_work,
(worker_t)&Gimbal::cycle_trampoline,
this,
USEC2TICK(GIMBAL_UPDATE_INTERVAL));
}
std::vector<float> Synthesizer::generate_frame()
{
// shift the signal left to get space for the new signal
sig = sig.shift(R);
for(int i = 0; i < R; i++)
{
sample_idx++;
if(update_params() >= 0 || finished)
{
finished = true;
break;
}
fpoint in = samples[sample_idx - last_phoneme_sample_idx];
// append the signal at the end
sig[L - R + i] = in;
}
// apply analysis window
std::valarray<complex> seg = sig * window, tmp(L);
fft(seg);
auto env = spectral_envelope(seg);
//for(auto a : env) std::cout << std::fixed << a << ",";
//std::cout << "\n\n";
const fpoint expect = 2 * M_PI / Ov;
// frequency between two bins
const fpoint bin_freq = fpoint(Fs) / L;
std::valarray<fpoint> tmp_mag(L / 2 + 1);
std::valarray<fpoint> tmp_frq(L / 2 + 1);
for(unsigned i = 0; i <= L / 2; i++)
{
// compute magnitude and phase
fpoint magn = abs(seg[i]) * 2;
fpoint phas = arg(seg[i]);
// compute phase difference
fpoint pdiff = phas - last_phase[i];
last_phase[i] = phas;
// subtract expected phase difference
pdiff -= i * expect;
// map delta phase into +/- Pi interval
pdiff -= 2 * M_PI * floor(pdiff / (2 * M_PI) + 0.5);
// get deviation from bin frequency from the +/- Pi interval
fpoint fdev = Ov * pdiff / (2 * M_PI);
// compute the k-th partials' true frequency
fpoint true_freq = i * bin_freq + fdev * bin_freq;
// store magnitude and true frequency in analysis arrays
tmp_mag[i] = magn;
tmp_frq[i] = true_freq;
}
// copy buffers
std::valarray<fpoint> seg_mag(L / 2 + 1);
std::valarray<fpoint> seg_frq(L / 2 + 1);
// pitch shifting
fpoint pitch_shift = 1.5;
for (unsigned i = 0; i <= L / 2; i++)
{
unsigned index = i * pitch_shift;
if (index <= L / 2)
{
seg_mag[index] += tmp_mag[i];// / env[i];
seg_frq[index] = tmp_frq[i] * pitch_shift;
}
}
// synthesis
for (unsigned i = 0; i <= L / 2; i++)
{
// get magnitude and true frequency from synthesis arrays
fpoint mag = seg_mag[i];
fpoint frq = seg_frq[i];
// subtract bin mid frequency
frq -= i * bin_freq;
// get bin deviation from freq deviation
frq /= bin_freq;
// take osamp into account
frq = 2 * M_PI * frq / Ov;
// add the overlap phase advance back in
frq += i * expect;
// accumulate delta phase to get bin phase
sum_phase[i] += frq;
fpoint phase = sum_phase[i];
// get real and imag part
seg[i] = std::polar(mag, phase);
seg[L - i - 1] = 0;
}
ifft(seg);
ova = ova.shift(R) + seg * window;
std::vector<float> out(R, 0);
for(unsigned i = 0; i < R; i++)
{
out[i] = (float)clamp(-1, 1, ova[i].real());
}
return out;
}
static bool update_settings(struct obs_qsv *obsqsv, obs_data_t *settings)
{
update_params(obsqsv, settings);
return true;
}
