static int
all_ins (PyObject *m)
{
int rc;
INS (selread);
INS (selwrite);
return 1;
}
static void
reset_and_dump(char *base, size_t size, size_t runs, reset_fn reset, const char *name)
{
assert(size % sizeof(struct cte) == 0);
struct cte *ctes = (struct cte*)base;
size_t num_caps = size/sizeof(struct cte);
for (size_t run = 0; run < runs; run++) {
// reset MDB
reset(base, size);
for (size_t i = 0; i < num_caps; i++) {
INS(&ctes[i]);
}
// dump entries
for (size_t i = 0; i < num_caps; i++) {
struct cte *cte = &ctes[i];
struct capability *curr = &cte->cap;
assert(curr->type == ObjType_RAM);
printf("%s/%zu:dump:%zu: 0x%08"PRIxGENPADDR"/%"PRIu8" %c%c%c\n",
name, num_caps, run, curr->u.ram.base, curr->u.ram.bits,
(HASCOP(cte) ? 'c' : '.'), (HASANC(cte) ? 'a' : '.'),
(HASDESC(cte) ? 'd' : '.'));
}
}
}
INS INSInitializer::initializeINS() const {
// Average up all accel and mag samples
Eigen::Vector3d y_a = mean(Eigen::Vector3d::Zero(), y_a_log.begin(), y_a_log.end());
Eigen::Vector3d y_m = mean(Eigen::Vector3d::Zero(), y_m_log.begin(), y_m_log.end());
// Use TRIAD to compute a rotation matrix
Eigen::Vector3d a = -config.g_nav.normalized();
Eigen::Vector3d a_hat = y_a.normalized();
Eigen::Vector3d m = a.cross(config.m_nav).normalized();
Eigen::Vector3d m_hat = a_hat.cross(y_m).normalized();
Eigen::Matrix3d R_imu2nav =
(Eigen::Matrix3d() << a, m, a.cross(m)).finished() *
(Eigen::Matrix3d() << a_hat, m_hat, a_hat.cross(m_hat)).finished().transpose();
// Sum up all the DVL readings
Eigen::Vector4d y_d = mean(Eigen::Vector4d::Zero(), y_d_log.begin(), y_d_log.end());
// Use least squares with the beam matrix to determine our velocity
Eigen::Vector3d v_imu_init =
(config.beam_mat.transpose()*config.beam_mat).lu().solve( // TODO fix guide
config.beam_mat.transpose() * y_d);
// Use the depth sensor to determine our z position
double p_nav_z = -mean(0, y_z_log.begin(), y_z_log.end())
- (R_imu2nav*config.r_imu2depth)[2];
// Initialize an INS
return INS(Eigen::Quaterniond(R_imu2nav),
R_imu2nav * v_imu_init,
Eigen::Vector3d(0, 0, p_nav_z),
config.g_nav);
}
void WaitGPS()
{
while(TRUE)
{
uint8 t=0;
LED2OFF();
INS();
t=INSFrameObj.Status&0x01;
if(t==0x01)
{
break;
}
DelayMs(3,100);
LED2ON();
DelayMs(3,100);
}
//LED2OFF();
}
Utility instruction routine. Creates and inserts an instruction
into the current block in one call.
=cut
*/
PARROT_WARN_UNUSED_RESULT
PARROT_CAN_RETURN_NULL
static Instruction *
insINS(PARROT_INTERP, ARGMOD(IMC_Unit *unit), ARGIN(Instruction *ins),
ARGIN(const char *name), ARGIN(SymReg **regs), int n)
{
ASSERT_ARGS(insINS)
/* INS can return NULL, but insert_ins() cannot take one */
Instruction * const tmp = INS(interp, unit, name, NULL, regs, n, 0, 0);
if (tmp)
insert_ins(unit, ins, tmp);
return tmp;
}
/*
=item C<SymReg* get_pasm_reg(PARROT_INTERP, const char *name)>
get or create the SymReg
=cut
*/
* Description: WHEN YOU FIGURE OUT WHAT THIS FILE DOES, PLEASE
* DESCRIBE IT HERE!
*
****************************************************************************/
#include "as.h"
static bool insErrFlag = FALSE; // to tell whether we had problems or not
#define INS( a, b, c, d, e ) { a, b, c, d, e, NULL }
ins_table AlphaTable[] = {
/* INS( name, opcode, funccode, template, method ), */
// Memory Format Instructions
INS( "lda", 0x08, 0x0000, IT_MEMORY_LDA, ENUM_NONE ),
INS( "ldah", 0x09, 0x0000, IT_MEMORY_ALL, ENUM_NONE ),
INS( "ldf", 0x20, 0x0000, IT_FP_MEMORY_ALL, ENUM_NONE ),
INS( "ldg", 0x21, 0x0000, IT_FP_MEMORY_ALL, ENUM_NONE ),
INS( "ldl", 0x28, 0x0000, IT_MEMORY_ALL, ENUM_NONE ),
INS( "ldl_l", 0x2A, 0x0000, IT_MEMORY_ALL, ENUM_NONE ),
INS( "ldq", 0x29, 0x0000, IT_MEMORY_ALL, ENUM_NONE ),
INS( "ldq_l", 0x2B, 0x0000, IT_MEMORY_ALL, ENUM_NONE ),
INS( "ldq_u", 0x0B, 0x0000, IT_MEMORY_ALL, ENUM_NONE ),
INS( "lds", 0x22, 0x0000, IT_FP_MEMORY_ALL, ENUM_NONE ),
INS( "ldt", 0x23, 0x0000, IT_FP_MEMORY_ALL, ENUM_NONE ),
INS( "stf", 0x24, 0x0000, IT_FP_MEMORY_ALL, ENUM_NONE ),
INS( "stg", 0x25, 0x0000, IT_FP_MEMORY_ALL, ENUM_NONE ),
INS( "stl", 0x2C, 0x0000, IT_MEMORY_ALL, ENUM_NONE ),
INS( "stl_c", 0x2E, 0x0000, IT_MEMORY_ALL, ENUM_NONE ),
INS( "stq", 0x2D, 0x0000, IT_MEMORY_ALL, ENUM_NONE ),
int main (void)
{
//Test
//Test();
//Init
InitDevice();
//Send Startup Message
FCMessageSend("vFlyOK");
//等待INS的GPS完成启动
//WaitGPS();
//改变运行状态并发送状态信息
RunState=DefaultRunState;
FCEventSend(RunState);
//初始化参数
InitPara();
FCMessageSend("InitParaOK");
//启动主计时器
TimerEnable(0);
while(1)
{
if(Do==DoWorkLoop)
{
LED2ON();
INS();
SwitchMode();
if(FPGAState==TRUE)
{
GetPWM();
}
//FCA();
//FCALimitPWM(void);
FCAGetNav();
FCAPWMOut();
SmoothPWM(40);
SetPWM();
//CollectMedianDataFun(40);
FCACollectPosYaw(5);
FCACollectMedianDataFun(40);
FCFrameProcess();
if(MainLoopRunCounter%DataSendDF==0)
{
DataReturn(0);
}
if(MainLoopRunCounter%DataRecDF==DataRecDF/2)
{
DataReturn(1);
}
MainLoopRunCounter++;
LED2OFF();
Do=DoNothing;
}
if(Do==DoFactoryLoop)
{
LED1ON();
GpioClrLOW(P1_25);//一律切换到手驾
INS();
FCFrameProcess();
if(DataToDownload==0)
{
if(MainLoopRunCounter%DataSendDF==0)
{
DataReturn(0);
}
/*
if(MainLoopRunCounter%DataRecDF==DataRecDF/2)
{
DataReturn(1);
}
*/
}
else
{
DataDownload();
}
MainLoopRunCounter++;
LED1OFF();
Do=DoNothing;
}
}
return 0;
}
* new release of Dune firmware that can be brought
* back to life successfully.
*/
#if 0
#if defined (CONFIG_BCM_VIDE_OFE)
#include <asm/brcmstb/brcm97635/bchp_ofe_armcr4_bridge.h>
#include <linux/libata.h>
#endif
#endif // 0
/*
* ART hardware sequencer
*/
art_ins_t art_pwr_down[] = {
INS( Delay, 10, 0),
INS( RdAnd, BCHP_CLK_PM_CTRL , 0xffffffff),
INS( WrOr, BCHP_CLK_PM_CTRL , BCHP_CLK_PM_CTRL_DIS_CPU0_108M_CLK_MASK),
INS( RdAnd, BCHP_CLK_PM_CTRL_2 , 0xffffffff),
INS( WrOr, BCHP_CLK_PM_CTRL_2 , BCHP_CLK_PM_CTRL_2_DIS_CPU0_CLK_MASK),
INS( RdAnd, BCHP_CLK_PM_CTRL_216 , 0xffffffff),
INS( WrOr, BCHP_CLK_PM_CTRL_216 , BCHP_CLK_PM_CTRL_216_DIS_CPU0_216_CLK_MASK),
INS( RdAnd, BCHP_VCXO_CTL_MISC_MIPS_CTRL , ~BCHP_VCXO_CTL_MISC_MIPS_CTRL_CLOCK_ENA_MASK),
INS( WrOr, BCHP_VCXO_CTL_MISC_MIPS_CTRL , 0),
INS( RdAnd, BCHP_VCXO_CTL_MISC_MIPS_CTRL , 0xffffffff),
INS( WrOr, BCHP_VCXO_CTL_MISC_MIPS_CTRL , BCHP_VCXO_CTL_MISC_MIPS_CTRL_POWERDOWN_MASK),
INS( Halt, 0, 0),
int diff (Alignment *A, int *ns, int **ls, int s1, int M,int s2, int N , int tb, int te, Constraint_list *CL, int **pos)
{
static int *CC;
static int *DD;
/* Forward cost-only vectors */
static int *RR;
static int *SS;
/* Reverse cost-only vectors */
int midi, midj, type; /* Midpoint, type, and cost */
int midc;
/*TREATMENT OF THE TERMINAL GAP PENALTIES*/
/*TG_MODE=0---> gop and gep*/
/*TG_MODE=1---> --- gep*/
/*TG_MODE=2---> --- ---*/
if ( !CC)
{
int L;
L=M+N+1;
CC=(int*)vcalloc (L, sizeof (int));
DD=(int*)vcalloc (L, sizeof (int));
RR=(int*)vcalloc (L, sizeof (int));
SS=(int*)vcalloc (L, sizeof (int));
}
if ( A==NULL)
{
vfree(CC);
vfree(DD);
vfree(RR);
vfree(SS);
CC=DD=RR=SS=NULL;
return 0;
}
{
int i, j;
int c, e, d, s,ma;
int t, g,h,m;
g=CL->gop*SCORE_K;
h=CL->gep*SCORE_K;
m=g+h;
if (N <= 0) {
if (M > 0) DEL(M);
return gap(M);
}
if (M <= 1)
{
if (M <= 0)
{ INS(N);
return gap(N);
}
if (tb > te) tb = te;
midc = (tb+h) + gap(N);
midj = 0;
for (j = 1; j <= N; j++)
{
c = gap(j-1) +(CL->get_dp_cost) (A, pos, sns[0], sls[0],s1, pos, sns[1], sls[1],j-1+s2,CL)+ gap(N-j);
if (c > midc)
{ midc = c;
midj = j;
}
}
if (midj == 0)
{
DEL(1) INS(N)
}
else
{ if (midj > 1) INS(midj-1);
REP;
if (midj < N) INS(N-midj);
}
return midc;
}
/* Divide: Find optimum midpoint (midi,midj) of cost midc */
midi = M/2; /* Forward phase: */
CC[0] = 0; /* Compute C(M/2,k) & D(M/2,k) for all k */
t = tb;
for (j = 1; j <= N; j++)
{ CC[j] = t = t+h;
DD[j] = t+g;
}
t = tb;
for (i = 1; i <= midi; i++)
{
s = CC[0];
CC[0] = c = t = t+h;
e = t+g;
for (j = 1; j <= N; j++)
{
if ((c = c + m) > (e = e + h)) e = c;
if ((c = CC[j] + m) > (d = DD[j] + h)) d = c;
ma=c = s + (CL->get_dp_cost) (A, pos, sns[0], sls[0],i-1+s1, pos, sns[1], sls[1],j-1+s2,CL);
if (e > c) c = e;
if (d > c) c = d;
s = CC[j];
CC[j] = c;
DD[j] = d;
}
}
DD[0] = CC[0];
RR[N] = 0; /* Reverse phase: */
t = te;
for (j = N-1; j >= 0; j--)
{ RR[j] = t = t+h;
SS[j] = t+g;
}
t = te;
for (i = M-1; i >= midi; i--)
{ s = RR[N];
RR[N] = c = t = t+h;
e = t+g;
for (j = N-1; j >= 0; j--)
{
if ((c = c + m) > (e = e + h)) e = c;
if ((c = RR[j] + m) > (d = SS[j] + h)) d = c;
ma=c = s + (CL->get_dp_cost) (A, pos, sns[0], sls[0],i+s1, pos, sns[1], sls[1],j+s2,CL);
if (e > c) c = e;
if (d > c) c = d;
s = RR[j];
RR[j] = c;
SS[j] = d;
}
}
SS[N] = RR[N];
midc = CC[0]+RR[0]; /* Find optimal midpoint */
midj = 0;
type = 1;
for (j = 0; j <= N; j++)
if ((c = CC[j] + RR[j]) >= midc)
if (c > midc || (CC[j] != DD[j] && RR[j] == SS[j]))
{
midc = c;
midj = j;
}
for (j = N; j >= 0; j--)
if ((c = DD[j] + SS[j] - g) > midc)
{ midc = c;
midj = j;
type = 2;
}
}
/* Conquer: recursively around midpoint */
if (type == 1)
{
diff (A,ns, ls, s1,midi, s2, midj, tb, CL->gop*SCORE_K, CL, pos);
diff (A,ns, ls, s1+midi,M-midi, s2+midj, N-midj, CL->gop*SCORE_K,te, CL, pos);
}
else
{
diff (A,ns, ls, s1,midi-1, s2, midj, tb,0, CL, pos);
DEL(2);
diff (A,ns, ls, s1+midi+1, M-midi-1,s2+midj, N-midj,0,te, CL, pos);
}
return midc;
}
#include "as.h"
static bool insErrFlag = FALSE; // to tell whether we had problems or not
#define OE IF_SETS_OVERFLOW
#define RC IF_SETS_CC
#define AA IF_SETS_ABSOLUTE
#define LK IF_SETS_LINK
#define INS( a, b, c, d, e, f ) { a, b, c, 0, d, e, f, NULL }
// many simpilfied mnemonics use the special field
#define INS2( a, b, c, d, e, f, g ) { a, b, c, d, e, f, g, NULL }
ins_table PPCTable[] = {
INS( "abs", 31, 360, IT_UN, OE | RC, 0 ),
INS( "add", 31, 266, IT_BIN, OE | RC, 0 ),
INS( "addc", 31, 10, IT_BIN, OE | RC, 0 ),
INS( "adde", 31, 138, IT_BIN, OE | RC, 0 ),
INS( "addi", 14, 0, IT_BIN_IMM, 0, 0 ),
INS( "addic", 12, 0, IT_BIN_IMM, 0, 0 ),
INS( "addic.", 13, 0, IT_BIN_IMM, 0, 0 ),
INS( "addis", 15, 0, IT_BIN_IMM, 0, 0 ),
INS( "addme", 31, 234, IT_UN, OE | RC, 0 ),
INS( "addze", 31, 202, IT_UN, OE | RC, 0 ),
INS( "and", 31, 28, IT_BIN, RC, 0 ),
INS( "andc", 31, 60, IT_BIN, RC, 0 ),
INS( "andi.", 28, 0, IT_BIN_IMM, 0, 0 ),
INS( "andis.", 29, 0, IT_BIN_IMM, 0, 0 ),
INS( "b", 18, 0, IT_BR, AA | LK, 0 ),
*
* Description: MIPS instruction opcodes.
*
****************************************************************************/
#include "as.h"
static bool insErrFlag = false; // to tell whether we had problems or not
#define INS( a, b, c, d, e, f ) { a, b, c, d, e, NULL, f }
ins_table MIPSTable[] = {
/* INS( name, opcode, fncode, template, method, ISA level ), */
// Memory Format Instructions
INS( "ldl", 0x1a, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA3 ),
INS( "ldr", 0x1b, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA3 ),
INS( "lb", 0x20, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "lh", 0x21, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "lwl", 0x22, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "lw", 0x23, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "lbu", 0x24, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "lhu", 0x25, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "lwr", 0x26, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "sb", 0x28, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "sh", 0x29, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "swl", 0x2a, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "sw", 0x2b, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
INS( "sdl", 0x2c, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA3 ),
INS( "sdr", 0x2d, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA3 ),
INS( "swr", 0x2e, 0x00, IT_MEMORY_ALL, ENUM_NONE, MIPS_ISA1 ),
Sophon_Result
sophon_ins_run (Sophon_VM *vm)
{
Sophon_Stack *enter_stack;
Sophon_Stack *top_stack;
Sophon_DeclFrame *var_env;
Sophon_Function *curr_func;
Sophon_Module *curr_module;
Sophon_U16 ibuf_size;
Sophon_U32 gc_level = sophon_gc_get_nb_count(vm);
Sophon_Result r;
Sophon_U16 argc;
Sophon_U16 id;
Sophon_U16 pos;
Sophon_U8 n;
Sophon_Result ret = SOPHON_NONE;
SOPHON_INFO(("run"));
#define TOP top_stack
#define VAR_ENV var_env
#define CURR_FUNC curr_func
#define CURR_MODULE curr_module
#define IBUF_SIZE ibuf_size
#define UPDATE()\
SOPHON_MACRO_BEGIN\
TOP = vm->stack;\
VAR_ENV = (Sophon_DeclFrame*)TOP->var_env;\
CURR_FUNC = TOP->func;\
CURR_MODULE = CURR_FUNC->module;\
IBUF_SIZE = CURR_FUNC->ibuf_size;\
SOPHON_MACRO_END
#define SP (TOP->sp)
#define IP (TOP->ip)
#define TP (TOP->tp)
#define INS(n) (CURR_FUNC->f.ibuf[IP+(n)])
#define STACK(n) (TOP->v[SP-(n)-1])
#define VAR(n) (VAR_ENV->v[n])
#define CONST(n) (sophon_module_get_const(CURR_MODULE, (n)))
#define FUNC(n) (sophon_module_get_func(CURR_MODULE, (n)))
SOPHON_ASSERT(vm);
enter_stack = vm->stack;
UPDATE();
again:
while (IP < IBUF_SIZE) {
switch (INS(0)) {
#define GET_PARAM(name) (name)
#define INS_PRE_OP_Op
#define INS_PRE_OP_Const id = (INS(1) << 8) | INS(2);
#define INS_PRE_OP_Call argc = INS(1);
#define INS_PRE_OP_Jump pos = (INS(1) << 8) | INS(2);
#define INS_PRE_OP_StackOp n = INS(1);
#define INS_POST_OP_Op IP++;
#define INS_POST_OP_Const IP += 3;
#define INS_POST_OP_Call IP += 2;
#define INS_POST_OP_Jump IP += 3;
#define INS_POST_OP_StackOp IP += 2;
#if defined(SOPHON_DEBUG) && defined(DUMP_INS)
#define INS_DEBUG()\
SOPHON_MACRO_BEGIN\
sophon_pr("%03d:%05d: ", SP, IP);\
sophon_ins_dump(vm, CURR_FUNC, &INS(0),\
IBUF_SIZE - IP);\
sophon_pr("\n");\
SOPHON_MACRO_END
#else
#define INS_DEBUG()
#endif
#define RUN_INS(name, model, stk)\
case I_##name:\
INS_DEBUG();\
INS_PRE_OP_##model;\
{I_##name##_run;}\
INS_POST_OP_##model;\
SP += stk;\
break;
FOR_EACH_INS(RUN_INS)
default:
SOPHON_ASSERT(0);
break;
}
SOPHON_ASSERT(SP >= 0);
}
exception:
if (TP != 0xFFFF) {
if ((TOP->pi_bottom != vm->pi_stack) && (vm->pi_stack->sp > TP))
return ret;
SP = TP;
TP = SOPHON_VALUE_GET_INT(STACK(-3));
IP = SOPHON_VALUE_GET_INT(STACK(-2));
TOP->lex_env = SOPHON_VALUE_GET_CPTR(STACK(-1));
if (ret < 0)
ret = SOPHON_NONE;
goto again;
}
if (TOP->pi_bottom != vm->pi_stack) {
return ret;
}
if (enter_stack != top_stack) {
Sophon_Value rv = TOP->retv;
sophon_stack_pop(vm);
UPDATE();
if (ret >= 0) {
ret = SOPHON_NONE;
STACK(0) = rv;
goto again;
} else {
goto exception;
}
}
return ret;
}
