// ---------------------------------------------------------------------------
//
// ------------
void bvDefScaleRef::wd_init(WindowRef wd){
_idx=1;
read();
ControlRef c;
if(_set==1){
c=get_control(kvDefScaleRefViewSignature,kvDefScaleRefTypeNameLegendID);
HideControl(c);
c=get_control(kvDefScaleRefViewSignature,kvDefScaleRefTypeNameID);
HideControl(c);
}
else{
c=get_control(kvDefScaleRefViewSignature,kvDefScaleRefTypeNameID);
char name[256];
if(_tp){
_tp->name(name);
}
else{
message_string(kMsgVirtual,name,1);
}
SetTextControlValue(c,name);
}
c=get_control(kvDefScaleRefViewSignature,kvDefScaleRefPopupID);
PopulatePopupControlWithScales(_gapp,c,_idx);
}
void msr(int i)
{
unsigned int n;
*((int *)(&moveToStatus)) = i;
n=(unsigned int) moveToStatus.rn;
if(Bad_Reg(n))
printf("UNPREDICTABLE instruciton\n");
else{
unsigned sysmTemp=moveToStatus.sysm;
sysmTemp=sysmTemp>>3;
sysmTemp&=0x01F;
if(sysmTemp==0){
if(!(moveToStatus.sysm & 0x04))
set_apsr(get_general_register(moveToStatus.rn)>>27);
}
else if(sysmTemp==1){
if(CurrentModeIsPrivileged()){
if((moveToStatus.sysm & 0x07)==0)
set_msp(get_general_register(moveToStatus.rn));
else if((moveToStatus.sysm & 0x07)==1)
set_psp(get_general_register(moveToStatus.rn));
}
}
else if(sysmTemp==2){
unsigned int temp=(moveToStatus.sysm & 0x07);
if(temp==0 && CurrentModeIsPrivileged()){
set_primask(get_general_register(moveToStatus.rn)&0x01);
}
else if(temp==1 && CurrentModeIsPrivileged()){
set_basepri(get_general_register(moveToStatus.rn)&0xFF);
}
else if(temp==2 && CurrentModeIsPrivileged()){
if((get_general_register(moveToStatus.rn)&0xFF)!=0 && ((get_general_register(moveToStatus.rn)&0xFF)!=0) &&
(((get_general_register(moveToStatus.rn)&0xFF)<get_basepri()) || get_basepri()==0))
set_basepri(get_general_register(moveToStatus.rn)&0xFF);
}
else if(temp==3 && CurrentModeIsPrivileged() && (ExecutionPriority() > -1)){
set_faultmask(get_general_register(moveToStatus.rn)&0x01);
}
else if(temp==4 && CurrentModeIsPrivileged()){
/* when `100`
if CurrentModeIsPrivileged() then
CONTROL<0> = R[n]<1:0>;
If Mode == Thread then CONTROL<1> = R[n]<1>;
*/// question
set_control((get_control()&0xFFFFFFFE)|(get_general_register(moveToStatus.rn)&0x01));
if(GetMode()==THREAD)
set_control((get_control()&0xFFFFFFFD)|(get_general_register(moveToStatus.rn)&0x02));
}
}
}
CommandResponsePacket send_command_and_receive_response(uvc::device & device, const CommandResponsePacket & command)
{
CommandResponsePacket c = command, r;
set_control(device, lr_xu, static_cast<int>(control::command_response), &c, sizeof(c));
get_control(device, lr_xu, static_cast<int>(control::command_response), &r, sizeof(r));
return r;
}
static int force_idle(struct i2c_algo_sgi_data *adap)
{
int i;
set_control(SGI_I2C_FORCE_IDLE);
for (i = 0; i < adap->xfer_timeout; i++) {
if ((get_control() & SGI_I2C_NOT_IDLE) == 0)
goto out;
udelay(1);
}
return -ETIMEDOUT;
out:
if (get_control() & SGI_I2C_BUS_ERR)
return -EIO;
return 0;
}
// ---------------------------------------------------------------------------
//
// ------------
bool bvDefScaleRef::wd_event(EventRef evt, WindowRef wd){
bool b=true;
UInt32 clss=GetEventClass(evt);
HICommand cmd;
ControlRef c;
bGenericUnit* u;
if(clss==kEventClassCommand){
GetEventParameter(evt,kEventParamDirectObject,typeHICommand,NULL,sizeof(HICommand),NULL,&cmd);
switch(cmd.commandID){
case kHICommandOK:
write();
break;
case kHICommandCancel:
break;
case kvDefScaleRefPopupCmd:
c=get_control(kvDefScaleRefViewSignature,kvDefScaleRefPopupID);
_idx=GetControl32BitValue(c);
if(_idx>2){
u=_gapp->scaleMgr()->get(_idx-2);
_scale=u->coef();
}
else{
_scale=0;
}
break;
default:
b=false;
break;
}
}
return(b);
}
int gpio_setdir(int port, int pin, int direction)
{
if (check_params(port, pin)) {
return -1;
}
const unsigned int *bitmask = get_bitmask(port);
register volatile unsigned int *r, *c;
volatile int position;
position = get_index(port, pin);
if ((position < 0) || (position >= GPIO_ADDRESS_MAX)) {
fprintf(stderr,"error: invalid position: %d\n", position);
return -1;
}
/** set GPIO on the pad control */
c = (void *)gpio.control + get_control(port, pin);
*c |= 0x07;
/** set register address */
r = (void*)gpio.address[position] + GPIO_OE;
switch(direction) {
case GPIO_DIR_OUTPUT:
*r &= ~(bitmask[pin]);
break;
case GPIO_DIR_INPUT:
*r |= (bitmask[pin]);
break;
default:
fprintf(stderr,"error: invalid direction %d\n", direction);
return -1;
}
return 0;
}
int trx_worker_t::_work_ACTIVE_impl()
{
// bind to the specified processor
_prs_id = PBIND_NONE;
TRY_TO_BIND(_prs_id,_is_bound);
w_rc_t e;
Request* ar = NULL;
// Check if signalled to stop
while (get_control() == WC_ACTIVE) {
// Reset the flags for the new loop
ar = NULL;
set_ws(WS_LOOP);
// Dequeue a request from the (main) input queue
// It will spin inside the queue or (after a while) wait on a cond var
ar = _pqueue->pop();
// Execute the particular request and deallocate it
if (ar) {
_serve_action(ar);
++_stats._served_input;
#ifndef CFG_FLUSHER
_env->_request_pool.destroy(ar);
#endif
}
}
return (0);
}
void ttabbar::set_report(treport* report, int width, int height)
{
VALIDATE(!report_ || !report_->content_grid_->children_vsize(), "Duplicate call!");
report_ = report;
if (!report) {
return;
}
start_ = 0;
segment_childs_ = 0;
VALIDATE(!segment_ || report_->unit_size_.x, "variable tabbar must not be segment!");
if (report->unit_size_.x) {
width = report->unit_size_.x;
height = report->unit_size_.y;
}
if (!width) {
std::string type = toggle_? "toggle_button": "button";
tresolution_definition_ptr ptr = get_control(type, definition_);
width = ptr->default_height;
height = ptr->default_height;
}
VALIDATE(width > 0 && height > 0, "Must valid width and height!");
if (!stuff_widget_) {
// previous arrow
previous_ = create_surface_button("previous", NULL);
previous_->set_fix_size(width, height);
previous_->set_surface(image::get_image("buttons/arrow_left.png"), width, height);
connect_signal_mouse_left_click(
*previous_
, boost::bind(
&ttabbar::click
, this
, true));
stuff_widget_ = create_spacer("sutff");
if (report->unit_size_.x) {
stuff_widget_->set_visible(twidget::INVISIBLE);
}
// next arrow
next_ = create_surface_button("next", NULL);
next_->set_fix_size(width, height);
next_->set_surface(image::get_image("buttons/arrow_right.png"), width, height);
connect_signal_mouse_left_click(
*next_
, boost::bind(
&ttabbar::click
, this
, false));
}
report->insert_child(*previous_);
report->insert_child(*stuff_widget_);
report->insert_child(*next_);
report_ = report;
report_->tabbar_ = this;
}
/*
* check_control - check the control record has sensible values for message, user and room numbers
*/
void check_control(void)
{
syslog(LOG_INFO, "Checking/re-building control record\n");
get_control();
// Find highest room number and message number.
CtdlForEachRoom(control_find_highest, NULL);
ForEachUser(control_find_user, NULL);
put_control();
}
/*
* CtdlGetCurrentMessageNumber() - Obtain the current highest message number in the system
* This provides a quick way to initialise a variable that might be used to indicate
* messages that should not be processed. EG. a new Sieve script will use this
* to record determine that messages older than this should not be processed.
*/
long CtdlGetCurrentMessageNumber(void)
{
long retval = 0L;
begin_critical_section(S_CONTROL);
get_control();
retval = CitControl.MMhighest;
end_critical_section(S_CONTROL);
return(retval);
}
static void load_config(const char * file)
{
int i;
FILE * f;
char * name;
const struct field * fld;
if (!strcmp(file, "-")) {
f = stdin;
} else {
f = fopen(file, "r");
if (f == NULL) {
warning("Could not open config file '%s'", file);
return;
}
}
while (igspaces(f) != EOF) {
fld = NULL;
name = get_string(f);
for (i = 0; i < sizeof(cfg_fields) / sizeof(*cfg_fields); i++) {
if (!strcmp(cfg_fields[i].name, name)) {
fld = cfg_fields + i;
break;
}
}
igspaces(f);
expect('=', f, file);
if (fld == NULL) {
warning("%s: Unknown field '%s'", file, name);
free(get_string(f));
} else {
switch (fld->type) {
case STRING:
// FIXME potential memory leak
*(char **)fld->ptr = get_string(f);
break;
case BOOLEAN:
*(int *)fld->ptr = get_boolean(f, file);
break;
case CONTROL:
*(int *)fld->ptr = get_control(f);
break;
case INTEGER:
*(int *)fld->ptr = get_integer(f);
break;
}
}
free(name);
}
if (f != stdin) fclose(f);
}
/*
* get_new_message_number() - Obtain a new, unique ID to be used for a message.
*/
long get_new_message_number(void)
{
long retval = 0L;
begin_critical_section(S_CONTROL);
get_control();
retval = ++CitControl.MMhighest;
put_control();
end_critical_section(S_CONTROL);
return(retval);
}
/*
* get_new_room_number() - Obtain a new, unique ID to be used for a room.
*/
long get_new_room_number(void)
{
long retval = 0L;
begin_critical_section(S_CONTROL);
get_control();
retval = ++CitControl.MMnextroom;
put_control();
end_critical_section(S_CONTROL);
return(retval);
}
unsigned
HelicopterMixer::mix(float *outputs, unsigned space)
{
/* Find index to use for curves */
float thrust_cmd = get_control(0, 3);
int idx = (thrust_cmd / 0.25f);
/* Make sure idx is in range */
if (idx < 0) {
idx = 0;
} else if (idx > HELI_CURVES_NR_POINTS - 2) {
/* We access idx + 1 below, so max legal index is (size - 2) */
idx = HELI_CURVES_NR_POINTS - 2;
}
/* Local throttle curve gradient and offset */
float tg = (_mixer_info.throttle_curve[idx + 1] - _mixer_info.throttle_curve[idx]) / 0.25f;
float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f);
float throttle = constrain(2.0f * (tg * thrust_cmd + to) - 1.0f, -1.0f, 1.0f);
/* Local pitch curve gradient and offset */
float pg = (_mixer_info.pitch_curve[idx + 1] - _mixer_info.pitch_curve[idx]) / 0.25f;
float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f);
float collective_pitch = constrain((pg * thrust_cmd + po), -0.5f, 0.5f);
float roll_cmd = get_control(0, 0);
float pitch_cmd = get_control(0, 1);
outputs[0] = throttle;
for (unsigned i = 0; i < _mixer_info.control_count; i++) {
outputs[i + 1] = collective_pitch
+ cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length
- sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length;
outputs[i + 1] *= _mixer_info.servos[i].scale;
outputs[i + 1] += _mixer_info.servos[i].offset;
outputs[i + 1] = constrain(outputs[i + 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output);
}
return _mixer_info.control_count + 1;
}
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
float roll = get_control(0, 0) * _roll_scale;
float pitch = get_control(0, 1) * _pitch_scale;
float yaw = get_control(0, 2) * _yaw_scale;
float thrust = get_control(0, 3);
float max = 0.0f;
float fixup_scale;
/* perform initial mix pass yielding un-bounded outputs */
for (unsigned i = 0; i < _rotor_count; i++) {
float tmp = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust;
if (tmp > max)
max = tmp;
outputs[i] = tmp;
}
/* scale values into the -1.0 - 1.0 range */
if (max > 1.0f) {
fixup_scale = 2.0f / max;
} else {
fixup_scale = 2.0f;
}
for (unsigned i = 0; i < _rotor_count; i++)
outputs[i] = -1.0f + (outputs[i] * fixup_scale);
/* ensure outputs are out of the deadband */
for (unsigned i = 0; i < _rotor_count; i++)
if (outputs[i] < _deadband)
outputs[i] = _deadband;
return 0;
}
static int wait_xfer_done(struct i2c_algo_sgi_data *adap)
{
int i;
for (i = 0; i < adap->xfer_timeout; i++) {
if ((get_control() & SGI_I2C_XFER_BUSY) == 0)
return 0;
udelay(1);
}
return -ETIMEDOUT;
}
uint32_t InputEventKey::get_scancode_with_modifiers() const {
uint32_t sc = scancode;
if (get_control())
sc |= KEY_MASK_CTRL;
if (get_alt())
sc |= KEY_MASK_ALT;
if (get_shift())
sc |= KEY_MASK_SHIFT;
if (get_metakey())
sc |= KEY_MASK_META;
return sc;
}
static int wait_ack(struct i2c_algo_sgi_data *adap)
{
int i;
if (wait_xfer_done(adap))
return -ETIMEDOUT;
for (i = 0; i < adap->ack_timeout; i++) {
if ((get_control() & SGI_I2C_NACK) == 0)
return 0;
udelay(1);
}
return -ETIMEDOUT;
}
/***********
* control frames have a higher priority then voice frames
* returns JB_OK if a frame is available and *data points to the packet
* returns JB_NOFRAME if it's no time to play voice and no control available
* returns JB_INTERP if interpolating is required
* returns JB_EMPTY if no voice frame is in the jitterbuffer (only during silence)
*/
int jb_get(jitterbuffer *jb, void **data, long now, long interpl)
{
int result;
jb_dbg("A");
if (jb == NULL) {
jb_err("no jitterbuffer in jb_get()\n");
return JB_NOJB;
}
result = get_control(jb, data);
if (result != JB_OK ) { //no control message available maybe there is voice...
result = get_voice(jb, data, now, interpl);
}
return result;
}
void WinSD::parse(PSECURITY_DESCRIPTOR sd) {
#ifdef ENABLE_LOGGER
WORD ctrl = get_control(sd);
LogInfo("Security descriptor:");
LogInfo(L"SDDL: %s", WinSD::as_sddl(sd).c_str());
LogInfo("Size: %Iu", size(sd));
try {
LogInfo(L"Owner: %s", Sid::get_name(get_owner(sd)).c_str());
} catch (...) {
}
try {
LogInfo(L"Group: %s", Sid::get_name(get_group(sd)).c_str());
} catch (...) {
}
LogInfo(L"Control: 0x%x (%s) [%s]", (int)ctrl, bits::BitMask<WORD>::to_str_bin(ctrl).c_str(), bits::BitMask<WORD>::to_str_num(ctrl).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_OWNER_DEFAULTED))
LogInfo(L"\tSE_OWNER_DEFAULTED (%s)", simstd::to_wstring(SE_OWNER_DEFAULTED).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_GROUP_DEFAULTED))
LogInfo(L"\tSE_GROUP_DEFAULTED (%s)", simstd::to_wstring(SE_GROUP_DEFAULTED).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_DACL_PRESENT))
LogInfo(L"\tSE_DACL_PRESENT (%s)", simstd::to_wstring(SE_DACL_PRESENT).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_DACL_DEFAULTED))
LogInfo(L"\tSE_DACL_DEFAULTED (%s)", simstd::to_wstring(SE_DACL_DEFAULTED).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_SACL_PRESENT))
LogInfo(L"\tSE_SACL_PRESENT (%s)", simstd::to_wstring(SE_SACL_PRESENT).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_SACL_DEFAULTED))
LogInfo(L"\tSE_SACL_DEFAULTED (%s)", simstd::to_wstring(SE_SACL_DEFAULTED).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_DACL_AUTO_INHERIT_REQ))
LogInfo(L"\tSE_DACL_AUTO_INHERIT_REQ (%s)", simstd::to_wstring(SE_DACL_AUTO_INHERIT_REQ).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_SACL_AUTO_INHERIT_REQ))
LogInfo(L"\tSE_SACL_AUTO_INHERIT_REQ (%s)", simstd::to_wstring(SE_SACL_AUTO_INHERIT_REQ).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_DACL_AUTO_INHERITED))
LogInfo(L"\tSE_DACL_AUTO_INHERITED (%s)", simstd::to_wstring(SE_DACL_AUTO_INHERITED).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_SACL_AUTO_INHERITED))
LogInfo(L"\tSE_SACL_AUTO_INHERITED (%s)", simstd::to_wstring(SE_SACL_AUTO_INHERITED).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_DACL_PROTECTED))
LogInfo(L"\tSE_DACL_PROTECTED (%s)", simstd::to_wstring(SE_DACL_PROTECTED).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_SACL_PROTECTED))
LogInfo(L"\tSE_SACL_PROTECTED (%s)", simstd::to_wstring(SE_SACL_PROTECTED).c_str());
if (bits::Flags::check(ctrl, (WORD)SE_SELF_RELATIVE))
LogInfo(L"\tSE_SELF_RELATIVE (%s)", simstd::to_wstring(SE_SELF_RELATIVE).c_str());
#else
(void)sd;
#endif
}
static int do_address(struct i2c_algo_sgi_data *adap, unsigned int addr,
int rd)
{
if (rd)
set_control(SGI_I2C_NOT_IDLE);
/* Check if bus is idle, eventually force it to do so */
if (get_control() & SGI_I2C_NOT_IDLE)
if (force_idle(adap))
return -EIO;
/* Write out the i2c chip address and specify operation */
set_control(SGI_I2C_HOLD_BUS | SGI_I2C_WRITE | SGI_I2C_NOT_IDLE);
if (rd)
addr |= 1;
write_data(addr);
if (wait_ack(adap))
return -EIO;
return 0;
}
void styled_widget::definition_load_configuration(const std::string& control_type)
{
assert(!config());
set_config(get_control(control_type, definition_));
if(get_canvas().size() != config()->state.size())
{
// TODO: Some widgets (toggle panel, toggle button) have a variable canvas count which is determined by its definition.
// I think we should remove the canvas_count from tcontrols constructor and always read it from the definition.
DBG_GUI_L << "Corrected canvas count to " << config()->state.size() << std::endl;
get_canvas() = std::vector<canvas>(config()->state.size());
}
for(size_t i = 0; i < get_canvas().size(); ++i) {
get_canvas(i) = config()->state[i].canvas_;
}
update_canvas();
}
void camera_v4l2<Tdata>::print_controls() {
cout << "__V4l2 camera controls___________________________________" << endl;
struct v4l2_queryctrl queryctrl;
struct v4l2_querymenu querymenu;
memset (&queryctrl, 0, sizeof (queryctrl));
for (queryctrl.id = V4L2_CID_BASE;
queryctrl.id < V4L2_CID_LASTP1;
queryctrl.id++) {
if (0 == ioctl (fd, VIDIOC_QUERYCTRL, &queryctrl)) {
if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
continue;
cout << queryctrl.name << " (" << queryctrl.id << "): "
<< get_control(queryctrl.id) << endl;
if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
enumerate_menu (fd, queryctrl, querymenu);
} else {
if (errno == EINVAL)
continue;
eblerror ("VIDIOC_QUERYCTRL");
}
}
for (queryctrl.id = V4L2_CID_PRIVATE_BASE;;
queryctrl.id++) {
if (0 == ioctl (fd, VIDIOC_QUERYCTRL, &queryctrl)) {
if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
continue;
printf ("Control %s\n", queryctrl.name);
if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
enumerate_menu (fd, queryctrl, querymenu);
} else {
if (errno == EINVAL)
break;
eblerror ("VIDIOC_QUERYCTRL");
}
}
cout << "_________________________________________________________" << endl;
}
String InputEventKey::as_text() const {
String kc = keycode_get_string(scancode);
if (kc == String())
return kc;
if (get_metakey()) {
kc = find_keycode_name(KEY_META) + ("+" + kc);
}
if (get_alt()) {
kc = find_keycode_name(KEY_ALT) + ("+" + kc);
}
if (get_shift()) {
kc = find_keycode_name(KEY_SHIFT) + ("+" + kc);
}
if (get_control()) {
kc = find_keycode_name(KEY_CONTROL) + ("+" + kc);
}
return kc;
}
gint get_next_group(FILE *fp, struct model_pak *model, gint *have_basis)
{
gchar *line, *keyword;
gint ret;
GSList *keywords = NULL, *list;
line = file_read_line(fp);
if (!line)
return(FALSE);
/* TODO not a valid keyword so for the moment skip but could store */
if (g_ascii_strncasecmp(line, " $", 2) != 0)
return(TRUE);
if (g_ascii_strncasecmp(line, " $data", 6) == 0)
{
ret = get_data(fp, model, *have_basis);
}
else if (g_ascii_strncasecmp(line, " $basis", 7) == 0)
{
*have_basis = TRUE;
keywords = get_gamess_keywords(fp, line+7, &ret);
for (list=keywords ; list ; list=g_slist_next(list))
{
keyword = (gchar *) list->data;
ret = get_basis(keyword, model);
}
}
else if (g_ascii_strncasecmp(line, " $contrl", 8) == 0)
{
keywords = get_gamess_keywords(fp, line+8, &ret);
for (list=keywords; list ; list=g_slist_next(list))
{
keyword = (gchar *) list->data;
ret = get_control(keyword, model);
}
}
else if (g_ascii_strncasecmp(line, " $system", 8) == 0)
{
keywords = get_gamess_keywords(fp, line+8, &ret);
for (list=keywords; list ; list=g_slist_next(list))
{
keyword = (gchar *) list->data;
ret = get_system(keyword, model);
}
}
else if (g_ascii_strncasecmp(line, " $statpt", 8) == 0)
{
keywords = get_gamess_keywords(fp, line+8, &ret);
for (list=keywords; list ; list=g_slist_next(list))
{
keyword = (gchar *) list->data;
ret = get_statpt(keyword, model);
}
}
else if (g_ascii_strncasecmp(line, " $dft", 5) == 0)
{
model->gamess.dft = TRUE;
/* TODO - process functional */
}
else
{
/* TODO - Unknown keyword, just pass through */
}
free_slist(keywords);
g_free(line);
return(TRUE);
}
void mrs(int i)
{
/*P463
*/
unsigned int n,result;
*((int *)(&moveToReg)) = i;
n=(unsigned int) moveToReg.rd;
if(Bad_Reg(n))
printf("UNPREDICTABLE instruciton\n");
else{
unsigned sysmTemp=moveToReg.sysm;
result=0;
sysmTemp=sysmTemp>>3;
sysmTemp&=0x01F;
if(sysmTemp==0){
if(moveToReg.sysm&0x1 && CurrentModeIsPrivileged()){ //question
result=get_ipsr();
result &= 0x000000FF;
}
if(moveToReg.sysm&0x02){
//result &= 0xF8FF03FF;
}
if(moveToReg.sysm&0x04){
unsigned int temp=0;
temp=get_apsr();
temp=temp<<27;
temp &= 0xF8000000;
result &= 0x07FFFFFF;
result = result | temp;
}
}
else if(sysmTemp==1){
if(CurrentModeIsPrivileged()){
unsigned temp=moveToReg.sysm;
temp &= 0x07;
switch(temp){
case 0:
result=get_msp();
break;
case 1:
result=get_psp();
break;
}
}
}
else if(sysmTemp==2){
unsigned temp=(moveToReg.sysm& 0x07);
switch(temp){
case 0:
if(CurrentModeIsPrivileged()){
result=get_primask();
result &= 0x01;
}
else
result &= 0x0;
break;
case 1:
if(CurrentModeIsPrivileged()){
result =get_basepri();
result &= 0xFF;
}
else
result &= 0x0;
break;
case 2:
if(CurrentModeIsPrivileged()){
result =get_basepri();
result &= 0xFF;
}
else
result &= 0x0;
break;
case 3:
if(CurrentModeIsPrivileged()){
result=get_faultmask();
result &= 0x01;
}
else
result &=0x0;
break;
case 4:
result=get_control();
result &= 0x03;
break;
}
}
}
set_general_register(moveToReg.rd,result);
}
int saa7146_video_do_ioctl(struct inode *inode, struct file *file, unsigned int cmd, void *arg)
{
struct saa7146_fh *fh = file->private_data;
struct saa7146_dev *dev = fh->dev;
struct saa7146_vv *vv = dev->vv_data;
int err = 0, result = 0, ee = 0;
struct saa7146_use_ops *ops;
struct videobuf_queue *q;
/* check if extension handles the command */
for(ee = 0; dev->ext_vv_data->ioctls[ee].flags != 0; ee++) {
if( cmd == dev->ext_vv_data->ioctls[ee].cmd )
break;
}
if( 0 != (dev->ext_vv_data->ioctls[ee].flags & SAA7146_EXCLUSIVE) ) {
DEB_D(("extension handles ioctl exclusive.\n"));
result = dev->ext_vv_data->ioctl(fh, cmd, arg);
return result;
}
if( 0 != (dev->ext_vv_data->ioctls[ee].flags & SAA7146_BEFORE) ) {
DEB_D(("extension handles ioctl before.\n"));
result = dev->ext_vv_data->ioctl(fh, cmd, arg);
if( -EAGAIN != result ) {
return result;
}
}
/* fixme: add handle "after" case (is it still needed?) */
switch (fh->type) {
case V4L2_BUF_TYPE_VIDEO_CAPTURE: {
ops = &saa7146_video_uops;
q = &fh->video_q;
break;
}
case V4L2_BUF_TYPE_VBI_CAPTURE: {
ops = &saa7146_vbi_uops;
q = &fh->vbi_q;
break;
}
default:
BUG();
return 0;
}
switch (cmd) {
case VIDIOC_QUERYCAP:
{
struct v4l2_capability *cap = arg;
memset(cap,0,sizeof(*cap));
DEB_EE(("VIDIOC_QUERYCAP\n"));
strcpy(cap->driver, "saa7146 v4l2");
strlcpy(cap->card, dev->ext->name, sizeof(cap->card));
sprintf(cap->bus_info,"PCI:%s", pci_name(dev->pci));
cap->version = SAA7146_VERSION_CODE;
cap->capabilities =
V4L2_CAP_VIDEO_CAPTURE |
V4L2_CAP_VIDEO_OVERLAY |
V4L2_CAP_READWRITE |
V4L2_CAP_STREAMING;
cap->capabilities |= dev->ext_vv_data->capabilities;
return 0;
}
case VIDIOC_G_FBUF:
{
struct v4l2_framebuffer *fb = arg;
DEB_EE(("VIDIOC_G_FBUF\n"));
*fb = vv->ov_fb;
fb->capability = V4L2_FBUF_CAP_LIST_CLIPPING;
return 0;
}
case VIDIOC_S_FBUF:
{
struct v4l2_framebuffer *fb = arg;
struct saa7146_format *fmt;
DEB_EE(("VIDIOC_S_FBUF\n"));
if(!capable(CAP_SYS_ADMIN) &&
!capable(CAP_SYS_RAWIO))
return -EPERM;
/* check args */
fmt = format_by_fourcc(dev,fb->fmt.pixelformat);
if (NULL == fmt) {
return -EINVAL;
}
/* planar formats are not allowed for overlay video, clipping and video dma would clash */
if (0 != (fmt->flags & FORMAT_IS_PLANAR)) {
DEB_S(("planar pixelformat '%4.4s' not allowed for overlay\n",(char *)&fmt->pixelformat));
}
/* check if overlay is running */
if (IS_OVERLAY_ACTIVE(fh) != 0) {
if (vv->video_fh != fh) {
DEB_D(("refusing to change framebuffer informations while overlay is active in another open.\n"));
return -EBUSY;
}
}
down(&dev->lock);
/* ok, accept it */
vv->ov_fb = *fb;
vv->ov_fmt = fmt;
if (0 == vv->ov_fb.fmt.bytesperline)
vv->ov_fb.fmt.bytesperline =
vv->ov_fb.fmt.width*fmt->depth/8;
up(&dev->lock);
return 0;
}
case VIDIOC_ENUM_FMT:
{
struct v4l2_fmtdesc *f = arg;
int index;
switch (f->type) {
case V4L2_BUF_TYPE_VIDEO_CAPTURE:
case V4L2_BUF_TYPE_VIDEO_OVERLAY: {
index = f->index;
if (index < 0 || index >= NUM_FORMATS) {
return -EINVAL;
}
memset(f,0,sizeof(*f));
f->index = index;
strlcpy(f->description,formats[index].name,sizeof(f->description));
f->pixelformat = formats[index].pixelformat;
break;
}
default:
return -EINVAL;
}
DEB_EE(("VIDIOC_ENUM_FMT: type:%d, index:%d\n",f->type,f->index));
return 0;
}
case VIDIOC_QUERYCTRL:
{
const struct v4l2_queryctrl *ctrl;
struct v4l2_queryctrl *c = arg;
if ((c->id < V4L2_CID_BASE ||
c->id >= V4L2_CID_LASTP1) &&
(c->id < V4L2_CID_PRIVATE_BASE ||
c->id >= V4L2_CID_PRIVATE_LASTP1))
return -EINVAL;
ctrl = ctrl_by_id(c->id);
if( NULL == ctrl ) {
return -EINVAL;
/*
c->flags = V4L2_CTRL_FLAG_DISABLED;
return 0;
*/
}
DEB_EE(("VIDIOC_QUERYCTRL: id:%d\n",c->id));
*c = *ctrl;
return 0;
}
case VIDIOC_G_CTRL: {
DEB_EE(("VIDIOC_G_CTRL\n"));
return get_control(fh,arg);
}
case VIDIOC_S_CTRL:
{
DEB_EE(("VIDIOC_S_CTRL\n"));
err = set_control(fh,arg);
return err;
}
case VIDIOC_G_PARM:
{
struct v4l2_streamparm *parm = arg;
if( parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ) {
return -EINVAL;
}
memset(&parm->parm.capture,0,sizeof(struct v4l2_captureparm));
parm->parm.capture.readbuffers = 1;
// fixme: only for PAL!
parm->parm.capture.timeperframe.numerator = 1;
parm->parm.capture.timeperframe.denominator = 25;
return 0;
}
case VIDIOC_G_FMT:
{
struct v4l2_format *f = arg;
DEB_EE(("VIDIOC_G_FMT\n"));
return g_fmt(fh,f);
}
case VIDIOC_S_FMT:
{
struct v4l2_format *f = arg;
DEB_EE(("VIDIOC_S_FMT\n"));
return s_fmt(fh,f);
}
case VIDIOC_TRY_FMT:
{
struct v4l2_format *f = arg;
DEB_EE(("VIDIOC_TRY_FMT\n"));
return try_fmt(fh,f);
}
case VIDIOC_G_STD:
{
v4l2_std_id *id = arg;
DEB_EE(("VIDIOC_G_STD\n"));
*id = vv->standard->id;
return 0;
}
/* the saa7146 supfhrts (used in conjunction with the saa7111a for example)
PAL / NTSC / SECAM. if your hardware does not (or does more)
-- override this function in your extension */
case VIDIOC_ENUMSTD:
{
struct v4l2_standard *e = arg;
if (e->index < 0 )
return -EINVAL;
if( e->index < dev->ext_vv_data->num_stds ) {
DEB_EE(("VIDIOC_ENUMSTD: index:%d\n",e->index));
v4l2_video_std_construct(e, dev->ext_vv_data->stds[e->index].id, dev->ext_vv_data->stds[e->index].name);
return 0;
}
return -EINVAL;
}
case VIDIOC_S_STD:
{
v4l2_std_id *id = arg;
int found = 0;
int i, err;
DEB_EE(("VIDIOC_S_STD\n"));
if ((vv->video_status & STATUS_CAPTURE) == STATUS_CAPTURE) {
DEB_D(("cannot change video standard while streaming capture is active\n"));
return -EBUSY;
}
if ((vv->video_status & STATUS_OVERLAY) != 0) {
vv->ov_suspend = vv->video_fh;
err = saa7146_stop_preview(vv->video_fh); /* side effect: video_status is now 0, video_fh is NULL */
if (0 != err) {
DEB_D(("suspending video failed. aborting\n"));
return err;
}
}
down(&dev->lock);
for(i = 0; i < dev->ext_vv_data->num_stds; i++)
if (*id & dev->ext_vv_data->stds[i].id)
break;
if (i != dev->ext_vv_data->num_stds) {
vv->standard = &dev->ext_vv_data->stds[i];
if( NULL != dev->ext_vv_data->std_callback )
dev->ext_vv_data->std_callback(dev, vv->standard);
found = 1;
}
up(&dev->lock);
if (vv->ov_suspend != NULL) {
saa7146_start_preview(vv->ov_suspend);
vv->ov_suspend = NULL;
}
if( 0 == found ) {
DEB_EE(("VIDIOC_S_STD: standard not found.\n"));
return -EINVAL;
}
DEB_EE(("VIDIOC_S_STD: set to standard to '%s'\n",vv->standard->name));
return 0;
}
case VIDIOC_OVERLAY:
{
int on = *(int *)arg;
int err = 0;
DEB_D(("VIDIOC_OVERLAY on:%d\n",on));
if (on != 0) {
err = saa7146_start_preview(fh);
} else {
err = saa7146_stop_preview(fh);
}
return err;
}
case VIDIOC_REQBUFS: {
struct v4l2_requestbuffers *req = arg;
DEB_D(("VIDIOC_REQBUFS, type:%d\n",req->type));
return videobuf_reqbufs(q,req);
}
case VIDIOC_QUERYBUF: {
struct v4l2_buffer *buf = arg;
DEB_D(("VIDIOC_QUERYBUF, type:%d, offset:%d\n",buf->type,buf->m.offset));
return videobuf_querybuf(q,buf);
}
case VIDIOC_QBUF: {
struct v4l2_buffer *buf = arg;
int ret = 0;
ret = videobuf_qbuf(q,buf);
DEB_D(("VIDIOC_QBUF: ret:%d, index:%d\n",ret,buf->index));
return ret;
}
case VIDIOC_DQBUF: {
struct v4l2_buffer *buf = arg;
int ret = 0;
ret = videobuf_dqbuf(q,buf,file->f_flags & O_NONBLOCK);
DEB_D(("VIDIOC_DQBUF: ret:%d, index:%d\n",ret,buf->index));
return ret;
}
case VIDIOC_STREAMON: {
int *type = arg;
DEB_D(("VIDIOC_STREAMON, type:%d\n",*type));
err = video_begin(fh);
if( 0 != err) {
return err;
}
err = videobuf_streamon(q);
return err;
}
case VIDIOC_STREAMOFF: {
int *type = arg;
DEB_D(("VIDIOC_STREAMOFF, type:%d\n",*type));
/* ugly: we need to copy some checks from video_end(),
because videobuf_streamoff() relies on the capture running.
check and fix this */
if ((vv->video_status & STATUS_CAPTURE) != STATUS_CAPTURE) {
DEB_S(("not capturing.\n"));
return 0;
}
if (vv->video_fh != fh) {
DEB_S(("capturing, but in another open.\n"));
return -EBUSY;
}
err = videobuf_streamoff(q);
if (0 != err) {
DEB_D(("warning: videobuf_streamoff() failed.\n"));
video_end(fh, file);
} else {
err = video_end(fh, file);
}
return err;
}
case VIDIOCGMBUF:
{
struct video_mbuf *mbuf = arg;
struct videobuf_queue *q;
int i;
/* fixme: number of capture buffers and sizes for v4l apps */
int gbuffers = 2;
int gbufsize = 768*576*4;
DEB_D(("VIDIOCGMBUF \n"));
q = &fh->video_q;
down(&q->lock);
err = videobuf_mmap_setup(q,gbuffers,gbufsize,
V4L2_MEMORY_MMAP);
if (err < 0) {
up(&q->lock);
return err;
}
memset(mbuf,0,sizeof(*mbuf));
mbuf->frames = gbuffers;
mbuf->size = gbuffers * gbufsize;
for (i = 0; i < gbuffers; i++)
mbuf->offsets[i] = i * gbufsize;
up(&q->lock);
return 0;
}
default:
return v4l_compat_translate_ioctl(inode,file,cmd,arg,
saa7146_video_do_ioctl);
}
return 0;
}
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
/* Summary of mixing strategy:
1) mix roll, pitch and thrust without yaw.
2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation ->
increase or decrease total thrust (boost). The total increase or decrease of thrust is limited
(max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch.
In case there is violation at the lower and upper bound then try to shift such that violation is equal
on both sides.
3) mix in yaw and scale if it leads to limit violation.
4) scale all outputs to range [idle_speed,1]
*/
float roll = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
float pitch = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
float yaw = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
float thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f);
float min_out = 1.0f;
float max_out = 0.0f;
// clean out class variable used to capture saturation
_saturation_status.value = 0;
// thrust boost parameters
float thrust_increase_factor = 1.5f;
float thrust_decrease_factor = 0.6f;
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
float out = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
thrust;
out *= _rotors[i].out_scale;
/* calculate min and max output values */
if (out < min_out) {
min_out = out;
}
if (out > max_out) {
max_out = out;
}
outputs[i] = out;
}
float boost = 0.0f; // value added to demanded thrust (can also be negative)
float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch
if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
float max_thrust_diff = thrust * thrust_increase_factor - thrust;
if (max_thrust_diff >= -min_out) {
boost = -min_out;
} else {
boost = max_thrust_diff;
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
}
} else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
if (max_thrust_diff >= max_out - 1.0f) {
boost = -(max_out - 1.0f);
} else {
boost = -max_thrust_diff;
roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
}
} else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
float max_thrust_diff = thrust * thrust_increase_factor - thrust;
boost = math::constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff);
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
} else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) {
float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
boost = math::constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f);
roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
} else if (min_out < 0.0f && max_out > 1.0f) {
boost = math::constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust,
thrust_increase_factor * thrust - thrust);
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
}
// capture saturation
if (min_out < 0.0f) {
_saturation_status.flags.motor_neg = true;
}
if (max_out > 1.0f) {
_saturation_status.flags.motor_pos = true;
}
// Thrust reduction is used to reduce the collective thrust if we hit
// the upper throttle limit
float thrust_reduction = 0.0f;
// mix again but now with thrust boost, scale roll/pitch and also add yaw
for (unsigned i = 0; i < _rotor_count; i++) {
float out = (roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust + boost;
out *= _rotors[i].out_scale;
// scale yaw if it violates limits. inform about yaw limit reached
if (out < 0.0f) {
if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
yaw = 0.0f;
} else {
yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
}
} else if (out > 1.0f) {
// allow to reduce thrust to get some yaw response
float prop_reduction = fminf(0.15f, out - 1.0f);
// keep the maximum requested reduction
thrust_reduction = fmaxf(thrust_reduction, prop_reduction);
if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
yaw = 0.0f;
} else {
yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + (thrust - thrust_reduction) + boost)) / _rotors[i].yaw_scale;
}
}
}
// Apply collective thrust reduction, the maximum for one prop
thrust -= thrust_reduction;
// add yaw and scale outputs to range idle_speed...1
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = (roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust + boost;
/*
implement simple model for static relationship between applied motor pwm and motor thrust
model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2
this model assumes normalized input / output in the range [0,1] so this is the right place
to do it as at this stage the outputs are in that range.
*/
if (_thrust_factor > 0.0f) {
outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) *
(1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] /
_thrust_factor));
}
outputs[i] = math::constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
}
/* slew rate limiting and saturation checking */
for (unsigned i = 0; i < _rotor_count; i++) {
bool clipping_high = false;
bool clipping_low = false;
// check for saturation against static limits
if (outputs[i] > 0.99f) {
clipping_high = true;
} else if (outputs[i] < _idle_speed + 0.01f) {
clipping_low = true;
}
// check for saturation against slew rate limits
if (_delta_out_max > 0.0f) {
float delta_out = outputs[i] - _outputs_prev[i];
if (delta_out > _delta_out_max) {
outputs[i] = _outputs_prev[i] + _delta_out_max;
clipping_high = true;
} else if (delta_out < -_delta_out_max) {
outputs[i] = _outputs_prev[i] - _delta_out_max;
clipping_low = true;
}
}
_outputs_prev[i] = outputs[i];
// update the saturation status report
update_saturation_status(i, clipping_high, clipping_low);
}
// this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen
_delta_out_max = 0.0f;
return _rotor_count;
}
tresolution_definition_ptr get_control(const std::string& control_type,
const std::string& definition)
{
const tgui_definition::tcontrol_definition_map::const_iterator
#ifdef GUI2_EXPERIMENTAL_LISTBOX
control_definition
= (control_type == "list")
? current_gui->second.control_definition.find("listbox")
: current_gui->second.control_definition.find(
control_type);
#else
control_definition
= current_gui->second.control_definition.find(control_type);
#endif
std::map<std::string, tcontrol_definition_ptr>::const_iterator control;
if(control_definition == current_gui->second.control_definition.end()) {
goto fallback;
}
control = control_definition->second.find(definition);
if(control == control_definition->second.end()) {
fallback:
bool found_fallback = false;
if(current_gui != default_gui) {
#ifdef GUI2_EXPERIMENTAL_LISTBOX
auto default_control_definition = (control_type == "list")
? default_gui->second.control_definition.find("listbox")
: default_gui->second.control_definition.find(control_type);
#else
auto default_control_definition = default_gui->second.control_definition.find(control_type);
#endif
VALIDATE(control_definition != current_gui->second.control_definition.end(), formatter() << "Type '" << control_type << "' is unknown.");
control = default_control_definition->second.find(definition);
found_fallback = control != default_control_definition->second.end();
}
if(!found_fallback) {
if(definition != "default") {
LOG_GUI_G << "Control: type '" << control_type << "' definition '"
<< definition << "' not found, falling back to 'default'.\n";
return get_control(control_type, "default");
}
FAIL(formatter() << "default definition not found for control " << control_type);
}
}
for(std::vector<tresolution_definition_ptr>::const_iterator itor
= (*control->second).resolutions.begin(),
end = (*control->second).resolutions.end();
itor != end;
++itor) {
if(settings::screen_width <= (**itor).window_width
&& settings::screen_height <= (**itor).window_height) {
return *itor;
} else if(itor == end - 1) {
return *itor;
}
}
FAIL(formatter() << "Control: type '" << control_type << "' definition '" << definition << "' has no resolutions.\n");
}
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
float roll = get_control(0, 0) * _roll_scale;
//lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
float pitch = get_control(0, 1) * _pitch_scale;
float yaw = get_control(0, 2) * _yaw_scale;
float thrust = get_control(0, 3);
//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
float max = 0.0f;
float fixup_scale;
/* use an output factor to prevent too strong control signals at low throttle */
float min_thrust = 0.05f;
float max_thrust = 1.0f;
float startpoint_full_control = 0.40f;
float output_factor;
/* keep roll, pitch and yaw control to 0 below min thrust */
if (thrust <= min_thrust) {
output_factor = 0.0f;
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
} else if (thrust < startpoint_full_control && thrust > min_thrust) {
output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
/* and then stay at full control */
} else {
output_factor = max_thrust;
}
roll *= output_factor;
pitch *= output_factor;
yaw *= output_factor;
/* perform initial mix pass yielding un-bounded outputs */
for (unsigned i = 0; i < _rotor_count; i++) {
float tmp = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust;
if (tmp > max)
max = tmp;
outputs[i] = tmp;
}
/* scale values into the -1.0 - 1.0 range */
if (max > 1.0f) {
fixup_scale = 2.0f / max;
} else {
fixup_scale = 2.0f;
}
for (unsigned i = 0; i < _rotor_count; i++)
outputs[i] = -1.0f + (outputs[i] * fixup_scale);
/* ensure outputs are out of the deadband */
for (unsigned i = 0; i < _rotor_count; i++)
if (outputs[i] < _deadband)
outputs[i] = _deadband;
return _rotor_count;
}
