//run our program
void Run() {
if(fp == NULL) {
printf("ERROR OPENING FILE %s!!\n", "stacktrace.txt");
}
//print our initial values
fprintf(fp,"Initial Values %-5d%-5d%-5d\n", 0,0,-1);
//run each instruction until we get to the halt condition
while(!halt) {
//get instruction
Fetch();
//write the instruction
WriteToOutput(1);
//ecevute the instruction
Execute();
//write the pointers
WriteToOutput(2);
//write the stack
WriteToOutput(3);
}
fclose(fp);
}
void SerialComm::SendDebugString(const String& string, MessageType type) const {
CobsPayload<2000> payload;
WriteProtocolHead(type, 0xFFFFFFFF, payload);
size_t str_len = string.length();
for (size_t i = 0; i < str_len; ++i)
payload.Append(string.charAt(i));
WriteToOutput(payload);
}
ostream& PtrDynArray<T>::TextWrite(ostream& Os_, Boolean* Ok_) const
{
size_t i, Max_;
for (i = 0, Max_ = RunLength(); i < Max_; ++i)
{
const T* Ptr_ = ((*this)[i]);
if (Ptr_)
WriteToOutput(Os_, *Ptr_);
Os_ <<" ";
}
return Os_;
}
void SerialComm::SendResponse(uint32_t mask, uint32_t response) const {
CobsPayload<12> payload;
WriteProtocolHead(MessageType::Response, mask, payload);
payload.Append(response);
WriteToOutput(payload);
}
void SerialComm::SendState(uint32_t timestamp_us, void (*extra_handler)(uint8_t*, size_t), uint32_t mask) const {
if (!mask)
mask = state_mask;
// No need to publish empty state messages
if (!mask)
return;
CobsPayloadGeneric payload;
WriteProtocolHead(SerialComm::MessageType::State, mask, payload);
if (mask & SerialComm::STATE_MICROS)
payload.Append(timestamp_us);
if (mask & SerialComm::STATE_STATUS)
payload.Append(state->status);
if (mask & SerialComm::STATE_V0)
payload.Append(state->V0_raw);
if (mask & SerialComm::STATE_I0)
payload.Append(state->I0_raw);
if (mask & SerialComm::STATE_I1)
payload.Append(state->I1_raw);
if (mask & SerialComm::STATE_ACCEL)
payload.Append(state->accel);
if (mask & SerialComm::STATE_GYRO)
payload.Append(state->gyro);
if (mask & SerialComm::STATE_MAG)
payload.Append(state->mag);
if (mask & SerialComm::STATE_TEMPERATURE)
payload.Append(state->temperature);
if (mask & SerialComm::STATE_PRESSURE)
payload.Append(state->pressure);
if (mask & SerialComm::STATE_RX_PPM) {
for (int i = 0; i < 6; ++i)
payload.Append(ppm[i]);
}
if (mask & SerialComm::STATE_AUX_CHAN_MASK)
payload.Append(state->AUX_chan_mask);
if (mask & SerialComm::STATE_COMMANDS)
payload.Append(state->command_throttle, state->command_pitch, state->command_roll, state->command_yaw);
if (mask & SerialComm::STATE_F_AND_T)
payload.Append(state->Fz, state->Tx, state->Ty, state->Tz);
if (mask & SerialComm::STATE_PID_FZ_MASTER)
WritePIDData(payload, control->thrust_pid.master());
if (mask & SerialComm::STATE_PID_TX_MASTER)
WritePIDData(payload, control->pitch_pid.master());
if (mask & SerialComm::STATE_PID_TY_MASTER)
WritePIDData(payload, control->roll_pid.master());
if (mask & SerialComm::STATE_PID_TZ_MASTER)
WritePIDData(payload, control->yaw_pid.master());
if (mask & SerialComm::STATE_PID_FZ_SLAVE)
WritePIDData(payload, control->thrust_pid.slave());
if (mask & SerialComm::STATE_PID_TX_SLAVE)
WritePIDData(payload, control->pitch_pid.slave());
if (mask & SerialComm::STATE_PID_TY_SLAVE)
WritePIDData(payload, control->roll_pid.slave());
if (mask & SerialComm::STATE_PID_TZ_SLAVE)
WritePIDData(payload, control->yaw_pid.slave());
if (mask & SerialComm::STATE_MOTOR_OUT)
payload.Append(state->MotorOut);
if (mask & SerialComm::STATE_KINE_ANGLE)
payload.Append(state->kinematicsAngle);
if (mask & SerialComm::STATE_KINE_RATE)
payload.Append(state->kinematicsRate);
if (mask & SerialComm::STATE_KINE_ALTITUDE)
payload.Append(state->kinematicsAltitude);
if (mask & SerialComm::STATE_LOOP_COUNT)
payload.Append(state->loopCount);
WriteToOutput(payload, extra_handler);
}
void SerialComm::SendConfiguration() const {
CobsPayloadGeneric payload;
WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_EEPROM_DATA, payload);
payload.Append(config->raw);
WriteToOutput(payload);
}
