mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) const {
mavlink_waypoint_t mavCmd;
mavCmd.seq = getSeq();
mavCmd.command = getCommand();
mavCmd.frame = getFrame();
mavCmd.param1 = getParam1();
mavCmd.param2 = getParam2();
mavCmd.param3 = getParam3();
mavCmd.param4 = getParam4();
mavCmd.x = getX();
mavCmd.y = getY();
mavCmd.z = getZ();
mavCmd.autocontinue = getAutocontinue();
mavCmd.current = (getSeq() == current);
mavCmd.target_component = mavlink_system.compid;
mavCmd.target_system = mavlink_system.sysid;
return mavCmd;
}
AP_MavlinkCommand::AP_MavlinkCommand(const mavlink_waypoint_t & cmd) :
_data(k_commands + cmd.seq), _seq(cmd.seq) {
setCommand(MAV_CMD(cmd.command));
setAutocontinue(cmd.autocontinue);
setFrame(MAV_FRAME(cmd.frame));
setParam1(cmd.param1);
setParam2(cmd.param2);
setParam3(cmd.param3);
setParam4(cmd.param4);
setX(cmd.x);
setY(cmd.y);
setZ(cmd.z);
save();
// reload home if sent, home must be a global waypoint
if ( (cmd.seq == 0) && (cmd.frame == MAV_FRAME_GLOBAL)) home.load();
Serial.println("============================================================");
Serial.println("storing new command from mavlink_waypoint_t");
Serial.print("key: ");
Serial.println(_data.key(),DEC);
Serial.print("number: ");
Serial.println(cmd.seq,DEC);
Serial.print("command: ");
Serial.println(getCommand());
Serial.print("autocontinue: ");
Serial.println(getAutocontinue(),DEC);
Serial.print("frame: ");
Serial.println(getFrame(),DEC);
Serial.print("1000*param1: ");
Serial.println(int(1000*getParam1()),DEC);
Serial.print("1000*param2: ");
Serial.println(int(1000*getParam2()),DEC);
Serial.print("1000*param3: ");
Serial.println(int(1000*getParam3()),DEC);
Serial.print("1000*param4: ");
Serial.println(int(1000*getParam4()),DEC);
Serial.print("1000*x0: ");
Serial.println(int(1000*cmd.x),DEC);
Serial.print("1000*y0: ");
Serial.println(int(1000*cmd.y),DEC);
Serial.print("1000*z0: ");
Serial.println(int(1000*cmd.z),DEC);
Serial.print("1000*x: ");
Serial.println(int(1000*getX()),DEC);
Serial.print("1000*y: ");
Serial.println(int(1000*getY()),DEC);
Serial.print("1000*z: ");
Serial.println(int(1000*getZ()),DEC);
load();
Serial.print("1000*x1: ");
Serial.println(int(1000*getX()),DEC);
Serial.print("1000*y1: ");
Serial.println(int(1000*getY()),DEC);
Serial.print("1000*z1: ");
Serial.println(int(1000*getZ()),DEC);
Serial.println("============================================================");
Serial.flush();
}
void interpretStep(struct pbrain_pcb *pcb)
{
int base = pcbBaseAddress(pcb);
memcpy(pcb->IR, pcb->memory[0][base + pcb->PC], 6); //load the current instruction into the IR
int opcode = getOpcode(pcb->IR); //get the opcode of the current instruction
if (opcode < 0 || opcode > 99) {
cpuHalt(pcb);
}
switch (opcode) {
case 0:
interpret00(pcb);
break;
case 1:
interpret01(pcb);
break;
case 2:
interpret02(pcb);
break;
case 3:
interpret03(pcb);
break;
case 4:
interpret04(pcb);
break;
case 5:
interpret05(pcb);
break;
case 6:
interpret06(pcb);
break;
case 7:
interpret07(pcb);
break;
case 8:
interpret08(pcb);
break;
case 9:
interpret09(pcb);
break;
case 10:
interpret10(pcb);
break;
case 11:
interpret11(pcb);
break;
case 12:
interpret12(pcb);
break;
case 13:
interpret13(pcb);
break;
case 14:
interpret14(pcb);
break;
case 15:
interpret15(pcb);
break;
case 16:
interpret16(pcb);
break;
case 17:
interpret17(pcb);
break;
case 18:
interpret18(pcb);
break;
case 19:
interpret19(pcb);
break;
case 20:
if (getMemInt(pcb) == pcb->AC) {
pcb->PSW[0] = 'T';
debug("AC [%d] == %d (via pointer); PSW = T\n", pcb->AC, getMemInt(pcb));
} else {
pcb->PSW[0] = 'F';
debug("AC [%d] != %d (via pointer); PSW = F\n", pcb->AC, getMemInt(pcb));
}
pcb->PC++;
break;
case 21:
if (pcb->AC < getMemInt(pcb)) {
pcb->PSW[0] = 'T';
debug("AC [%d] < %d (via pointer); PSW = T\n", pcb->AC, getMemInt(pcb));
} else {
pcb->PSW[0] = 'F';
debug("AC [%d] !< %d (via pointer); PSW = F\n", pcb->AC, getMemInt(pcb));
}
pcb->PC++;
break;
case 22:
if (pcb->AC > getMemInt(pcb)) {
pcb->PSW[0] = 'T';
debug("AC [%d] > %d (via pointer); PSW = T\n", pcb->AC, getMemInt(pcb));
} else {
pcb->PSW[0] = 'F';
debug("AC [%d] !> %d (via pointer); PSW = F\n", pcb->AC, getMemInt(pcb));
}
pcb->PC++;
break;
case 23:
if (pcb->AC > getCombinedParams(pcb->IR)) {
pcb->PSW[0] = 'T';
debug("AC [%d] > %d; PSW = T\n", pcb->AC, getCombinedParams(pcb->IR));
} else {
pcb->PSW[0] = 'F';
debug("AC [%d] !> %d; PSW = F\n", pcb->AC, getCombinedParams(pcb->IR));
}
pcb->PC++;
break;
case 24:
if (pcb->AC == getCombinedParams(pcb->IR)) {
pcb->PSW[0] = 'T';
debug("AC [%d] == %d; PSW = T\n", pcb->AC, getCombinedParams(pcb->IR));
} else {
pcb->PSW[0] = 'F';
debug("AC [%d] != %d; PSW = F\n", pcb->AC, getCombinedParams(pcb->IR));
}
pcb->PC++;
break;
case 25:
if (pcb->AC < getCombinedParams(pcb->IR)) {
pcb->PSW[0] = 'T';
debug("AC [%d] < %d; PSW = T\n", pcb->AC, getCombinedParams(pcb->IR));
} else {
pcb->PSW[0] = 'F';
debug("AC [%d] !< %d; PSW = F\n", pcb->AC, getCombinedParams(pcb->IR));
}
pcb->PC++;
break;
case 26:
if (pcb->PSW[0] == 'T') {
debug("PSW == T, jumping to ");
pcb->PC = getParam1(pcb->IR);
debug("%d\n", pcb->PC);
} else {
pcb->PC++;
debug("PSW != T, continuing\n");
}
break;
case 27:
if (pcb->PSW[0] == 'F') {
debug("PSW == F, jumping to ");
pcb->PC = getParam1(pcb->IR);
debug("%d\n", pcb->PC);
} else {
debug("PSW != F; continuing\n");
pcb->PC++;
}
break;
case 28:
debug("Jumping unconditionally to ");
pcb->PC = getParam1(pcb->IR);
debug("%d\n", pcb->PC);
break;
case 31:
interpret31(pcb);
break;
case 32:
interpret32(pcb);
break;
case 99:
pcb->halted = true;
break;
}
}
