zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Calibration parameters set successfully for input channels

QGC4.4
unknown 15 years ago
parent
bbb00c5bbc
commit
671b998a52
5 changed files with 174 additions and 24 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 84
      settings/ParameterList.xml
  2. 107
      src/comm/OpalLink.cc
  3. 2
      src/comm/OpalLink.h
  4. 3
      src/comm/ParameterList.cc
  5. 2
      src/ui/RadioCalibration/AbstractCalibrator.cc

84
settings/ParameterList.xml
Unescape View File

@ -46,15 +46,95 @@ @@ -46,15 +46,95 @@
QGCParamID="ELE_DOWN_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ElevatorInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="ELE_CENTER_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ElevatorInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="ELE_UP_IN"
/>
<!-- Settings for Throttle Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="THR_SET0_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="THR_SET1_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="THR_SET2_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint3"
QGCParamID="THR_SET3_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/ThrottleInput/"
SimulinkParameterName="Setpoint4"
QGCParamID="THR_SET4_IN"
/>
<!-- Settings for Rudder Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/RudderInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="RUD_LEFT_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/RudderInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="RUD_CENTER_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/RudderInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="RUD_RIGHT_IN"
/>
<!-- Settings for Gyro Mode/Gain Switch -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/GyroInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="GYRO_DEF_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/GyroInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="GYRO_TOG_IN"
/>
<!-- Settings for Pitch Servo -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint0"
QGCParamID="PIT_SET0_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint1"
QGCParamID="PIT_SET1_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint2"
QGCParamID="PIT_SET2_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint3"
QGCParamID="PIT_SET3_IN"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/PitchInput/"
SimulinkParameterName="Setpoint4"
QGCParamID="PIT_SET4_IN"
/>
</Block>
<!-- Parameters for the servo output block -->

107
src/comm/OpalLink.cc
Unescape View File

@ -147,20 +147,62 @@ void OpalLink::writeBytes(const char *bytes, qint64 length) @@ -147,20 +147,62 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
{
mavlink_radio_calibration_t radio;
mavlink_msg_radio_calibration_decode(&msg, &radio);
qDebug() << "RADIO CALIBRATION RECEIVED";
qDebug() << "AILERON: " << radio.aileron[0] << " " << radio.aileron[1] << " " << radio.aileron[2];
qDebug() << "ELEVATOR: " << radio.elevator[0] << " " << radio.elevator[1] << " " << radio.elevator[2];
qDebug() << "RUDDER: " << radio.rudder[0] << " " << radio.rudder[1] << " " << radio.rudder[2];
qDebug() << "GYRO: " << radio.gyro[0] << " " << radio.gyro[1];
qDebug() << "PITCH: " << radio.pitch[0] << radio.pitch[1] << radio.pitch[2] << radio.pitch[3] << radio.pitch[4];
qDebug() << "THROTTLE: " << radio.throttle[0] << radio.throttle[1] << radio.throttle[2] << radio.throttle[3] << radio.throttle[4];
// qDebug() << "RADIO CALIBRATION RECEIVED";
// qDebug() << "AILERON: " << radio.aileron[0] << " " << radio.aileron[1] << " " << radio.aileron[2];
// qDebug() << "ELEVATOR: " << radio.elevator[0] << " " << radio.elevator[1] << " " << radio.elevator[2];
// qDebug() << "RUDDER: " << radio.rudder[0] << " " << radio.rudder[1] << " " << radio.rudder[2];
// qDebug() << "GYRO: " << radio.gyro[0] << " " << radio.gyro[1];
// qDebug() << "PITCH: " << radio.pitch[0] << radio.pitch[1] << radio.pitch[2] << radio.pitch[3] << radio.pitch[4];
// qDebug() << "THROTTLE: " << radio.throttle[0] << radio.throttle[1] << radio.throttle[2] << radio.throttle[3] << radio.throttle[4];
/* AILERON SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN").setValue(radio.aileron[0]);
params->getParameter(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN").setValue(((radio.aileron[0]>900 /*in us?*/)?radio.aileron[0]/1000:radio.aileron[0]));
if (params->contains(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN").setValue(radio.aileron[1]);
params->getParameter(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN").setValue(((radio.aileron[1]>900 /*in us?*/)?radio.aileron[1]/1000:radio.aileron[1]));
if (params->contains(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN").setValue(radio.aileron[2]);
params->getParameter(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN").setValue(((radio.aileron[2]>900 /*in us?*/)?radio.aileron[2]/1000:radio.aileron[2]));
/* ELEVATOR SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN").setValue(((radio.elevator[0]>900 /*in us?*/)?radio.elevator[0]/1000:radio.elevator[0]));
if (params->contains(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN").setValue(((radio.elevator[1]>900 /*in us?*/)?radio.elevator[1]/1000:radio.elevator[1]));
if (params->contains(OpalRT::SERVO_INPUTS, "ELE_UP_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "ELE_UP_IN").setValue(((radio.elevator[2]>900 /*in us?*/)?radio.elevator[2]/1000:radio.elevator[2]));
/* THROTTLE SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET0_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET0_IN").setValue(((radio.throttle[0]>900 /*in us?*/)?radio.throttle[0]/1000:radio.throttle[0]));
if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET1_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET1_IN").setValue(((radio.throttle[1]>900 /*in us?*/)?radio.throttle[1]/1000:radio.throttle[1]));
if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET2_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET2_IN").setValue(((radio.throttle[2]>900 /*in us?*/)?radio.throttle[2]/1000:radio.throttle[2]));
if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET3_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET3_IN").setValue(((radio.throttle[3]>900 /*in us?*/)?radio.throttle[3]/1000:radio.throttle[3]));
if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET4_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET4_IN").setValue(((radio.throttle[4]>900 /*in us?*/)?radio.throttle[4]/1000:radio.throttle[4]));
/* RUDDER SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN").setValue(((radio.rudder[0]>900 /*in us?*/)?radio.rudder[0]/1000:radio.rudder[0]));
if (params->contains(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN").setValue(((radio.rudder[1]>900 /*in us?*/)?radio.rudder[1]/1000:radio.rudder[1]));
if (params->contains(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN").setValue(((radio.rudder[2]>900 /*in us?*/)?radio.rudder[2]/1000:radio.rudder[2]));
/* GYRO MODE/GAIN SWITCH */
if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN").setValue(((radio.gyro[0]>900 /*in us?*/)?radio.gyro[0]/1000:radio.gyro[0]));
if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN").setValue(((radio.gyro[1]>900 /*in us?*/)?radio.gyro[1]/1000:radio.gyro[1]));
/* PITCH SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET0_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET0_IN").setValue(((radio.pitch[0]>900 /*in us?*/)?radio.pitch[0]/1000:radio.pitch[0]));
if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET1_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET1_IN").setValue(((radio.pitch[1]>900 /*in us?*/)?radio.pitch[1]/1000:radio.pitch[1]));
if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET2_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET2_IN").setValue(((radio.pitch[2]>900 /*in us?*/)?radio.pitch[2]/1000:radio.pitch[2]));
if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET3_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET3_IN").setValue(((radio.pitch[3]>900 /*in us?*/)?radio.pitch[3]/1000:radio.pitch[3]));
if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET4_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET4_IN").setValue(((radio.pitch[4]>900 /*in us?*/)?radio.pitch[4]/1000:radio.pitch[4]));
}
break;
#endif
@ -291,17 +333,23 @@ void OpalLink::getSignals() @@ -291,17 +333,23 @@ void OpalLink::getSignals()
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_6]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_7]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_8]),
rescaleNorm(values[OpalRT::NORM_CHANNEL_1]),
rescaleNorm(values[OpalRT::NORM_CHANNEL_2]),
rescaleNorm(values[OpalRT::NORM_CHANNEL_3]),
rescaleNorm(values[OpalRT::NORM_CHANNEL_4]),
rescaleNorm(values[OpalRT::NORM_CHANNEL_5]),
rescaleNorm(values[OpalRT::NORM_CHANNEL_6]),
rescaleNorm(values[OpalRT::NORM_CHANNEL_7]),
rescaleNorm(values[OpalRT::NORM_CHANNEL_8]),
rescaleNorm(values[OpalRT::NORM_CHANNEL_1], OpalRT::NORM_CHANNEL_1),
rescaleNorm(values[OpalRT::NORM_CHANNEL_2], OpalRT::NORM_CHANNEL_2),
rescaleNorm(values[OpalRT::NORM_CHANNEL_3], OpalRT::NORM_CHANNEL_3),
rescaleNorm(values[OpalRT::NORM_CHANNEL_4], OpalRT::NORM_CHANNEL_4),
rescaleNorm(values[OpalRT::NORM_CHANNEL_5], OpalRT::NORM_CHANNEL_5),
rescaleNorm(values[OpalRT::NORM_CHANNEL_6], OpalRT::NORM_CHANNEL_6),
rescaleNorm(values[OpalRT::NORM_CHANNEL_7], OpalRT::NORM_CHANNEL_7),
rescaleNorm(values[OpalRT::NORM_CHANNEL_8], OpalRT::NORM_CHANNEL_8),
0 //rssi unused
);
receiveMessage(rc);
// qDebug() << __FILE__ << __LINE__ << "Aileron: " << values[OpalRT::NORM_CHANNEL_1];
// qDebug() << __FILE__ << __LINE__ << "Elevator: " << values[OpalRT::NORM_CHANNEL_2];
// qDebug() << __FILE__ << __LINE__ << "Rudder: " << values[OpalRT::NORM_CHANNEL_4];
// qDebug() << __FILE__ << __LINE__ << "Pitch: " << values[OpalRT::NORM_CHANNEL_6];
}
}
else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
@ -357,9 +405,26 @@ uint16_t OpalLink::duty2PulseMicros(double duty) @@ -357,9 +405,26 @@ uint16_t OpalLink::duty2PulseMicros(double duty)
return static_cast<uint16_t>(duty/70*1000000);
}
uint8_t OpalLink::rescaleNorm(double norm)
uint8_t OpalLink::rescaleNorm(double norm, int ch)
{
return static_cast<uint8_t>((norm+1)/2*255);
switch(ch)
{
case OpalRT::NORM_CHANNEL_1:
case OpalRT::NORM_CHANNEL_2:
case OpalRT::NORM_CHANNEL_4:
default:
// three setpoints
return static_cast<uint8_t>((norm+1)/2*255);
break;
case OpalRT::NORM_CHANNEL_5:
//two setpoints
case OpalRT::NORM_CHANNEL_3:
case OpalRT::NORM_CHANNEL_6:
return static_cast<uint8_t>(norm*255);
break;
}
}

2
src/comm/OpalLink.h
Unescape View File

@ -153,7 +153,7 @@ protected: @@ -153,7 +153,7 @@ protected:
unsigned short opalInstID;
uint16_t duty2PulseMicros(double duty);
uint8_t rescaleNorm(double norm);
uint8_t rescaleNorm(double norm, int ch);
bool sendRCValues;
};

3
src/comm/ParameterList.cc
Unescape View File

@ -48,6 +48,9 @@ ParameterList::ParameterList() @@ -48,6 +48,9 @@ ParameterList::ParameterList()
reqdServoParams->append("ELE_DOWN_IN");
reqdServoParams->append("ELE_CENTER_IN");
reqdServoParams->append("ELE_UP_IN");
reqdServoParams->append("RUD_LEFT_IN");
reqdServoParams->append("RUD_CENTER_IN");
reqdServoParams->append("RUD_RIGHT_IN");
QString filename(settingsDir.path() + "/ParameterList.xml");
if ((QFile::exists(filename)) && open(filename))

2
src/ui/RadioCalibration/AbstractCalibrator.cc
Unescape View File

@ -30,6 +30,7 @@ float AbstractCalibrator::logExtrema() @@ -30,6 +30,7 @@ float AbstractCalibrator::logExtrema()
if (log->value(i) < extrema)
extrema = log->value(i);
}
extrema -= 5; // add 5us to prevent integer overflow
}
else
{
@ -38,6 +39,7 @@ float AbstractCalibrator::logExtrema() @@ -38,6 +39,7 @@ float AbstractCalibrator::logExtrema()
if (log->value(i) > extrema)
extrema = log->value(i);
}
extrema += 5; // subtact 5us to prevent integer overflow
}
return extrema;

Write Preview
Loading…
Cancel
Save