| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // ====================================================================== | ||
| 2 | // \title SequenceFileLoader.cpp | ||
| 3 | // \author bocchino | ||
| 4 | // \brief cpp file for SequenceFileLoader component implementation class | ||
| 5 | // | ||
| 6 | // \copyright | ||
| 7 | // Copyright (C) 2009-2016 California Institute of Technology. | ||
| 8 | // ALL RIGHTS RESERVED. United States Government Sponsorship | ||
| 9 | // acknowledged. | ||
| 10 | // | ||
| 11 | // ====================================================================== | ||
| 12 | |||
| 13 | #include <Fw/Types/Assert.hpp> | ||
| 14 | #include <Ref/SignalGen/SignalGen.hpp> | ||
| 15 | #include <cmath> | ||
| 16 | #include <cstdlib> | ||
| 17 | |||
| 18 | // TKC - don't know why it's undefined in VxWorks | ||
| 19 | #ifdef TGT_OS_TYPE_VXWORKS | ||
| 20 | #define M_PI (22.0 / 7.0) | ||
| 21 | #endif | ||
| 22 | |||
| 23 | namespace Ref { | ||
| 24 | |||
| 25 | // ---------------------------------------------------------------------- | ||
| 26 | // Construction, initialization, and destruction | ||
| 27 | // ---------------------------------------------------------------------- | ||
| 28 | |||
| 29 | 5 | SignalGen ::SignalGen(const char* name) | |
| 30 | : SignalGenComponentBase(name), | ||
| 31 | 5 | sampleFrequency(25), | |
| 32 | 5 | signalFrequency(1), | |
| 33 | 5 | signalAmplitude(0.0f), | |
| 34 | 5 | signalPhase(0.0f), | |
| 35 | 5 | ticks(0), | |
| 36 | 5 | sigType(SignalType::SINE), | |
| 37 |
1/1✓ Branch 1 taken 5 times.
|
5 | sigHistory(), |
| 38 |
1/1✓ Branch 1 taken 5 times.
|
5 | sigPairHistory(), |
| 39 | 5 | running(false), | |
| 40 | 5 | skipOne(false), | |
| 41 | 5 | m_dpInProgress(false), | |
| 42 | 5 | m_numDps(0), | |
| 43 | 5 | m_currDp(0), | |
| 44 | 5 | m_dpBytes(0), | |
| 45 |
2/2✓ Branch 2 taken 5 times.
✓ Branch 5 taken 5 times.
|
10 | m_dpPriority(0) {} |
| 46 | |||
| 47 | 10 | SignalGen ::~SignalGen() {} | |
| 48 | |||
| 49 | // ---------------------------------------------------------------------- | ||
| 50 | // Handler implementations | ||
| 51 | // ---------------------------------------------------------------------- | ||
| 52 | |||
| 53 | 1 | F32 SignalGen::generateSample(U32 ticks) { | |
| 54 | 1 | F32 val = 0.0f; | |
| 55 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (this->skipOne) { |
| 56 | ✗ | return val; | |
| 57 | } | ||
| 58 | // Samples per period | ||
| 59 | 1 | F32 samplesPerPeriod = static_cast<F32>(this->sampleFrequency) / static_cast<F32>(this->signalFrequency); | |
| 60 | 1 | U32 halfSamplesPerPeriod = samplesPerPeriod / 2; | |
| 61 | // Guard against divide/modulo-by-zero for degenerate frequency settings | ||
| 62 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (halfSamplesPerPeriod == 0) { |
| 63 | ✗ | return val; | |
| 64 | } | ||
| 65 | /* Signals courtesy of the open source Aquila DSP Library */ | ||
| 66 |
1/5✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
1 | switch (this->sigType.e) { |
| 67 | ✗ | case SignalType::TRIANGLE: { | |
| 68 | ✗ | F32 m = this->signalAmplitude / static_cast<F32>(halfSamplesPerPeriod); | |
| 69 | ✗ | val = m * static_cast<F32>(ticks % halfSamplesPerPeriod); | |
| 70 | ✗ | break; | |
| 71 | } | ||
| 72 | ✗ | case SignalType::SINE: { | |
| 73 | ✗ | F32 normalizedFrequency = 1.0f / samplesPerPeriod; | |
| 74 | ✗ | val = this->signalAmplitude * std::sin((2.0 * M_PI * normalizedFrequency * static_cast<F32>(ticks)) + | |
| 75 | ✗ | (this->signalPhase * 2.0 * M_PI)); | |
| 76 | ✗ | break; | |
| 77 | } | ||
| 78 | 1 | case SignalType::SQUARE: { | |
| 79 | 2 | val = this->signalAmplitude * | |
| 80 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | ((ticks % static_cast<U32>(samplesPerPeriod) < halfSamplesPerPeriod) ? 1.0f : -1.0f); |
| 81 | 1 | break; | |
| 82 | } | ||
| 83 | ✗ | case SignalType::NOISE: { | |
| 84 | ✗ | val = this->signalAmplitude * (std::rand() / static_cast<double>(RAND_MAX)); | |
| 85 | ✗ | break; | |
| 86 | } | ||
| 87 | ✗ | default: | |
| 88 | ✗ | FW_ASSERT(0); // Should never happen | |
| 89 | } | ||
| 90 | 1 | return val; | |
| 91 | } | ||
| 92 | |||
| 93 | 765 | void SignalGen::schedIn_handler(FwIndexType portNum, /*!< The port number*/ | |
| 94 | U32 context /*!< The call order*/ | ||
| 95 | ) { | ||
| 96 | 765 | F32 value = 0.0f; | |
| 97 | // This is a queued component, so it must intentionally run the dispatch of commands and queue processing on this | ||
| 98 | // synchronous scheduled call | ||
| 99 |
1/1✓ Branch 1 taken 765 times.
|
765 | this->doDispatch(); |
| 100 | |||
| 101 | // This short-circuits when the signal generator is not running | ||
| 102 |
2/2✓ Branch 0 taken 764 times.
✓ Branch 1 taken 1 times.
|
765 | if (not this->running) { |
| 103 | 764 | return; | |
| 104 | } | ||
| 105 | // Allows for skipping a single reading of the signal | ||
| 106 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (not this->skipOne) { |
| 107 |
1/1✓ Branch 1 taken 1 times.
|
1 | value = this->generateSample(this->ticks); |
| 108 | } | ||
| 109 | 1 | this->skipOne = false; | |
| 110 | |||
| 111 | // Build our new types | ||
| 112 |
1/1✓ Branch 1 taken 1 times.
|
1 | SignalPair pair = SignalPair(this->ticks, value); |
| 113 | |||
| 114 | // Shift and assign our array types | ||
| 115 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
|
4 | for (U32 i = 1; i < this->sigHistory.SIZE; i++) { |
| 116 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 4 taken 3 times.
|
3 | this->sigHistory[i - 1] = this->sigHistory[i]; |
| 117 |
3/3✓ Branch 1 taken 3 times.
✓ Branch 4 taken 3 times.
✓ Branch 7 taken 3 times.
|
3 | this->sigPairHistory[i - 1] = this->sigPairHistory[i]; |
| 118 | } | ||
| 119 |
1/1✓ Branch 1 taken 1 times.
|
1 | this->sigHistory[this->sigHistory.SIZE - 1] = value; |
| 120 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1 times.
|
1 | this->sigPairHistory[this->sigPairHistory.SIZE - 1] = pair; |
| 121 | |||
| 122 | // Composite structure | ||
| 123 |
1/1✓ Branch 2 taken 1 times.
|
1 | SignalInfo sigInfo(this->sigType, this->sigHistory, this->sigPairHistory); |
| 124 | |||
| 125 | // Write all signals | ||
| 126 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1 times.
|
1 | this->tlmWrite_Type(this->sigType); |
| 127 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1 times.
|
1 | this->tlmWrite_Output(value); |
| 128 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1 times.
|
1 | this->tlmWrite_PairOutput(pair); |
| 129 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1 times.
|
1 | this->tlmWrite_History(this->sigHistory); |
| 130 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1 times.
|
1 | this->tlmWrite_PairHistory(this->sigPairHistory); |
| 131 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1 times.
|
1 | this->tlmWrite_Info(sigInfo); |
| 132 | |||
| 133 | // if a Data product is being generated, store a record | ||
| 134 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (this->m_dpInProgress) { |
| 135 | ✗ | Fw::SerializeStatus stat = this->m_dpContainer.serializeRecord_DataRecord(sigInfo); | |
| 136 | ✗ | this->m_currDp++; | |
| 137 | ✗ | this->m_dpBytes += SignalInfo::SERIALIZED_SIZE; | |
| 138 | // check for full data product | ||
| 139 | ✗ | if (Fw::SerializeStatus::FW_SERIALIZE_NO_ROOM_LEFT == stat) { | |
| 140 | ✗ | this->log_WARNING_LO_DpRecordFull(this->m_currDp, this->m_dpBytes); | |
| 141 | ✗ | this->cleanupAndSendDp(); | |
| 142 | ✗ | } else if (this->m_currDp == this->m_numDps) { // if we reached the target number of DPs | |
| 143 | ✗ | this->log_ACTIVITY_LO_DpComplete(this->m_numDps, this->m_dpBytes); | |
| 144 | ✗ | this->cleanupAndSendDp(); | |
| 145 | } | ||
| 146 | |||
| 147 | ✗ | this->tlmWrite_DpBytes(this->m_dpBytes); | |
| 148 | ✗ | this->tlmWrite_DpRecords(this->m_currDp); | |
| 149 | } | ||
| 150 | |||
| 151 | 1 | this->ticks += 1; | |
| 152 | 1 | } | |
| 153 | |||
| 154 | 1 | void SignalGen::Settings_cmdHandler(FwOpcodeType opCode, /*!< The opcode*/ | |
| 155 | U32 cmdSeq, /*!< The command sequence number*/ | ||
| 156 | U32 Frequency, | ||
| 157 | F32 Amplitude, | ||
| 158 | F32 Phase, | ||
| 159 | const Ref::SignalType& SigType) { | ||
| 160 | // Reject frequencies that would produce a divide- or modulo-by-zero in | ||
| 161 | // generateSample: a zero frequency, or one high enough that | ||
| 162 | // halfSamplesPerPeriod truncates to zero | ||
| 163 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if ((Frequency == 0) || |
| 164 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | (static_cast<U32>((static_cast<F32>(this->sampleFrequency) / static_cast<F32>(Frequency)) / 2) == 0)) { |
| 165 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR); | |
| 166 | ✗ | return; | |
| 167 | } | ||
| 168 | 1 | this->signalFrequency = Frequency; | |
| 169 | 1 | this->signalAmplitude = Amplitude; | |
| 170 | 1 | this->signalPhase = Phase; | |
| 171 | 1 | this->sigType = SigType; | |
| 172 | |||
| 173 | // When the settings change, reset the history values | ||
| 174 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (U32 i = 0; i < SignalSet::SIZE; i++) { |
| 175 | 4 | this->sigHistory[i] = 0.0f; | |
| 176 | } | ||
| 177 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
5 | for (U32 i = 0; i < SignalPairSet::SIZE; i++) { |
| 178 | 4 | this->sigPairHistory[i].set_time(0.0f); | |
| 179 | 4 | this->sigPairHistory[i].set_value(0.0f); | |
| 180 | } | ||
| 181 | 1 | this->log_ACTIVITY_LO_SettingsChanged(this->signalFrequency, this->signalAmplitude, this->signalPhase, | |
| 182 | 1 | this->sigType); | |
| 183 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1 times.
|
1 | this->tlmWrite_Type(SigType); |
| 184 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 4 taken 1 times.
|
1 | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); |
| 185 | } | ||
| 186 | |||
| 187 | 2 | void SignalGen::Toggle_cmdHandler(FwOpcodeType opCode, /*!< The opcode*/ | |
| 188 | U32 cmdSeq /*!< The command sequence number*/ | ||
| 189 | ) { | ||
| 190 | 2 | this->running = !this->running; | |
| 191 | 2 | this->ticks = 0; | |
| 192 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 4 taken 2 times.
|
2 | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); |
| 193 | 2 | } | |
| 194 | |||
| 195 | ✗ | void SignalGen::Skip_cmdHandler(FwOpcodeType opCode, /*!< The opcode*/ | |
| 196 | U32 cmdSeq /*!< The command sequence number*/ | ||
| 197 | ) { | ||
| 198 | ✗ | this->skipOne = true; | |
| 199 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); | |
| 200 | ✗ | } | |
| 201 | |||
| 202 | ✗ | void SignalGen::Dp_cmdHandler(FwOpcodeType opCode, | |
| 203 | U32 cmdSeq, | ||
| 204 | const Ref::SignalGen_DpReqType& reqType, | ||
| 205 | U32 records, | ||
| 206 | U32 priority) { | ||
| 207 | // at least one record | ||
| 208 | ✗ | if (0 == records) { | |
| 209 | ✗ | this->log_WARNING_HI_InSufficientDpRecords(); | |
| 210 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR); | |
| 211 | ✗ | return; | |
| 212 | } | ||
| 213 | |||
| 214 | // make sure DPs are available | ||
| 215 | ✗ | if (not this->isConnected_productGetOut_OutputPort(0)) { | |
| 216 | ✗ | this->log_WARNING_HI_DpsNotConnected(); | |
| 217 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR); | |
| 218 | ✗ | return; | |
| 219 | } | ||
| 220 | |||
| 221 | // get DP buffer. Use sync or async request depending on | ||
| 222 | // requested type | ||
| 223 | ✗ | FwSizeType dpSize = records * SIZE_OF_DataRecord_RECORD; | |
| 224 | ✗ | this->m_numDps = records; | |
| 225 | ✗ | this->m_currDp = 0; | |
| 226 | ✗ | this->m_dpPriority = static_cast<FwDpPriorityType>(priority); | |
| 227 | ✗ | this->log_ACTIVITY_LO_DpMemRequested(dpSize); | |
| 228 | ✗ | if (Ref::SignalGen_DpReqType::IMMEDIATE == reqType) { | |
| 229 | ✗ | Fw::Success stat = this->dpGet_DataContainer(dpSize, this->m_dpContainer); | |
| 230 | // make sure we got the memory we wanted | ||
| 231 | ✗ | if (Fw::Success::FAILURE == stat) { | |
| 232 | ✗ | this->log_WARNING_HI_DpMemoryFail(); | |
| 233 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR); | |
| 234 | } else { | ||
| 235 | ✗ | this->m_dpInProgress = true; | |
| 236 | ✗ | this->log_ACTIVITY_LO_DpStarted(records); | |
| 237 | ✗ | this->log_ACTIVITY_LO_DpMemReceived(this->m_dpContainer.getBuffer().getSize()); | |
| 238 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); | |
| 239 | // override priority with requested priority | ||
| 240 | ✗ | this->m_dpContainer.setPriority(this->m_dpPriority); | |
| 241 | } | ||
| 242 | ✗ | } else if (Ref::SignalGen_DpReqType::ASYNC == reqType) { | |
| 243 | ✗ | this->dpRequest_DataContainer(dpSize); | |
| 244 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); | |
| 245 | } else { | ||
| 246 | // should never get here | ||
| 247 | ✗ | FW_ASSERT(0, reqType.e); | |
| 248 | } | ||
| 249 | } | ||
| 250 | |||
| 251 | ✗ | void SignalGen::cleanupAndSendDp() { | |
| 252 | ✗ | this->dpSend(this->m_dpContainer); | |
| 253 | ✗ | this->m_dpInProgress = false; | |
| 254 | ✗ | this->m_dpBytes = 0; | |
| 255 | ✗ | this->m_numDps = 0; | |
| 256 | ✗ | this->m_currDp = 0; | |
| 257 | ✗ | } | |
| 258 | |||
| 259 | // ---------------------------------------------------------------------- | ||
| 260 | // Handler implementations for data products | ||
| 261 | // ---------------------------------------------------------------------- | ||
| 262 | |||
| 263 | ✗ | void SignalGen ::dpRecv_DataContainer_handler(DpContainer& container, Fw::Success::T status) { | |
| 264 | // Make sure we got the buffer we wanted or quit | ||
| 265 | ✗ | if (Fw::Success::SUCCESS == status) { | |
| 266 | ✗ | this->m_dpContainer = container; | |
| 267 | ✗ | this->m_dpInProgress = true; | |
| 268 | // set previously requested priority | ||
| 269 | ✗ | this->m_dpContainer.setPriority(this->m_dpPriority); | |
| 270 | ✗ | this->log_ACTIVITY_LO_DpStarted(this->m_numDps); | |
| 271 | } else { | ||
| 272 | ✗ | this->log_WARNING_HI_DpMemoryFail(); | |
| 273 | // cleanup | ||
| 274 | ✗ | this->m_dpInProgress = false; | |
| 275 | ✗ | this->m_dpBytes = 0; | |
| 276 | ✗ | this->m_numDps = 0; | |
| 277 | ✗ | this->m_currDp = 0; | |
| 278 | } | ||
| 279 | ✗ | } | |
| 280 | |||
| 281 | } // namespace Ref | ||
| 282 |