/* 802.11/LO/GPHY/MeasuringGains */
static void lo_measure_gain_values(struct b43_wldev *dev,
s16 max_rx_gain, int use_trsw_rx)
{
struct b43_phy *phy = &dev->phy;
u16 tmp;
if (max_rx_gain < 0)
max_rx_gain = 0;
if (has_loopback_gain(phy)) {
int trsw_rx = 0;
int trsw_rx_gain;
if (use_trsw_rx) {
trsw_rx_gain = phy->trsw_rx_gain / 2;
if (max_rx_gain >= trsw_rx_gain) {
trsw_rx_gain = max_rx_gain - trsw_rx_gain;
trsw_rx = 0x20;
}
} else
trsw_rx_gain = max_rx_gain;
if (trsw_rx_gain < 9) {
phy->lna_lod_gain = 0;
} else {
phy->lna_lod_gain = 1;
trsw_rx_gain -= 8;
}
trsw_rx_gain = limit_value(trsw_rx_gain, 0, 0x2D);
phy->pga_gain = trsw_rx_gain / 3;
if (phy->pga_gain >= 5) {
phy->pga_gain -= 5;
phy->lna_gain = 2;
} else
phy->lna_gain = 0;
} else {
phy->lna_gain = 0;
phy->trsw_rx_gain = 0x20;
if (max_rx_gain >= 0x14) {
phy->lna_lod_gain = 1;
phy->pga_gain = 2;
} else if (max_rx_gain >= 0x12) {
phy->lna_lod_gain = 1;
phy->pga_gain = 1;
} else if (max_rx_gain >= 0xF) {
phy->lna_lod_gain = 1;
phy->pga_gain = 0;
} else {
phy->lna_lod_gain = 0;
phy->pga_gain = 0;
}
}
tmp = b43_radio_read16(dev, 0x7A);
if (phy->lna_lod_gain == 0)
tmp &= ~0x0008;
else
tmp |= 0x0008;
b43_radio_write16(dev, 0x7A, tmp);
}
void hoverPitchCntrl(void)
{
int16_t max_tilt_sine = sine((int8_t)(MAX_TILT*.7111));
if (flags._.pitch_feedback && flags._.GPS_steering)
{
//error along y axis between aircraft position and goal (origin point here) in cm
#ifdef TestGPSPositioning
hovering_pitch_order = RMAX;
tofinish_line_factor10 = 30;
#endif
if (control_position_hold)
{
target_pitch = compute_target_pitch(hovering_pitch_order, tofinish_line_factor10, max_tilt_sine);
}
else
{
target_pitch = 0;
pitch_error_integral = 0;
pitch_v_error_integral = 0;
}
//additional_int16_export4 = target_pitch;
uint16_t horizontal_air_speed = vector2_mag(IMUvelocityx._.W1 - estimatedWind[0],
IMUvelocityy._.W1 - estimatedWind[1]);
#ifdef TestGPSPositioning
horizontal_air_speed = 0;
#endif
//PI controller on pitch angle
pitch_v_target = compute_pi_block(-rmat[7], -target_pitch, hoverpitchToWPkp, hoverpitchToWPki, &pitch_error_integral,
(int16_t)(SERVO_HZ), limitintegralpitchToWP, control_position_hold);
//additional_int16_export3 = -rmat[7];
pitch_v_target = limit_value(pitch_v_target, -limittargetpitchV, limittargetpitchV);
pitch_hover_corr = compute_pi_block(horizontal_air_speed, pitch_v_target, hoverpitchToWPvkp, 0, &pitch_v_error_integral,
(int16_t)(SERVO_HZ), limitintegralpitchVToWP, control_position_hold);
//additional_int16_export8 = pitch_hover_corr;
}
else
{
pitch_hover_corr = 0;
}
pitch_control = pitch_hover_corr;
}
void GraphScene::updateArea() {
ValueRange limit_value(0,1.0);
for (int i=0;i < lines.size();i++) {
if (i==0) {
limit_value=lines.at(i)->getValueRange();
} else {
ValueRange tmp_value=lines.at(i)->getValueRange();
if (tmp_value.getMin() < limit_value.getMin()) limit_value.setMin(tmp_value.getMin());
if (tmp_value.getMax() > limit_value.getMax()) limit_value.setMax(tmp_value.getMax());
}
}
setSceneRect(control->getFullRange()->getMin(),limit_value.getMin(),control->getFullRange()->getLength(),limit_value.getLength());
//setSceneRect(control->getFullRange()->getMin(),-10,control->getFullRange()->getLength(),20);
qDebug("scene rect: %f %f %f %f\n",(double)control->getFullRange()->getMin(),(double)limit_value.getMin(),(double)control->getFullRange()->getLength(),(double)limit_value.getLength());
}
static void lo_measure_txctl_values(struct b43_wldev *dev)
{
struct b43_phy *phy = &dev->phy;
struct b43_txpower_lo_control *lo = phy->lo_control;
u16 reg, mask;
u16 trsw_rx, pga;
u16 radio_pctl_reg;
static const u8 tx_bias_values[] = {
0x09, 0x08, 0x0A, 0x01, 0x00,
0x02, 0x05, 0x04, 0x06,
};
static const u8 tx_magn_values[] = {
0x70, 0x40,
};
if (!has_loopback_gain(phy)) {
radio_pctl_reg = 6;
trsw_rx = 2;
pga = 0;
} else {
int lb_gain; /* Loopback gain (in dB) */
trsw_rx = 0;
lb_gain = phy->max_lb_gain / 2;
if (lb_gain > 10) {
radio_pctl_reg = 0;
pga = abs(10 - lb_gain) / 6;
pga = limit_value(pga, 0, 15);
} else {
int cmp_val;
int tmp;
pga = 0;
cmp_val = 0x24;
if ((phy->rev >= 2) &&
(phy->radio_ver == 0x2050) && (phy->radio_rev == 8))
cmp_val = 0x3C;
tmp = lb_gain;
if ((10 - lb_gain) < cmp_val)
tmp = (10 - lb_gain);
if (tmp < 0)
tmp += 6;
else
tmp += 3;
cmp_val /= 4;
tmp /= 4;
if (tmp >= cmp_val)
radio_pctl_reg = cmp_val;
else
radio_pctl_reg = tmp;
}
}
b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
& 0xFFF0) | radio_pctl_reg);
b43_phy_set_baseband_attenuation(dev, 2);
reg = lo_txctl_register_table(dev, &mask, NULL);
mask = ~mask;
b43_radio_write16(dev, reg, b43_radio_read16(dev, reg)
& mask);
if (has_tx_magnification(phy)) {
int i, j;
int feedthrough;
int min_feedth = 0xFFFF;
u8 tx_magn, tx_bias;
for (i = 0; i < ARRAY_SIZE(tx_magn_values); i++) {
tx_magn = tx_magn_values[i];
b43_radio_write16(dev, 0x52,
(b43_radio_read16(dev, 0x52)
& 0xFF0F) | tx_magn);
for (j = 0; j < ARRAY_SIZE(tx_bias_values); j++) {
tx_bias = tx_bias_values[j];
b43_radio_write16(dev, 0x52,
(b43_radio_read16(dev, 0x52)
& 0xFFF0) | tx_bias);
feedthrough =
lo_measure_feedthrough(dev, 0, pga,
trsw_rx);
if (feedthrough < min_feedth) {
lo->tx_bias = tx_bias;
lo->tx_magn = tx_magn;
min_feedth = feedthrough;
}
if (lo->tx_bias == 0)
break;
}
b43_radio_write16(dev, 0x52,
(b43_radio_read16(dev, 0x52)
& 0xFF00) | lo->tx_bias | lo->
tx_magn);
}
} else {
lo->tx_magn = 0;
lo->tx_bias = 0;
b43_radio_write16(dev, 0x52, b43_radio_read16(dev, 0x52)
& 0xFFF0); /* TX bias == 0 */
}
}
