void install_irq()
{
//reprogram the PICs
init_PIC();
//install the appropriate ISRs into the IDT
idt_set_gate(32, (unsigned)irq0, 0x08, 0x8E);
idt_set_gate(33, (unsigned)irq1, 0x08, 0x8E);
idt_set_gate(34, (unsigned)irq2, 0x08, 0x8E);
//turn on interupts!
__asm__ __volatile__("sti");
}
/* this is called from assembler and passed control in long mode 64 bit
* interrupts disabled.
* At this stage the first 32MB have been mapped with 2MB pages.
*/
extern "C" void
kinit()
{
extern char __bss_end[];
struct KernelInitArgs kernelInitArgs;
MemoryMgrPrimitiveKern *memory = &kernelInitArgs.memory;
uval vp = 0; /* master processor */
uval vaddr;
/* on this machine like all x86 machines nowaydays the boot
* image is loaded at 1MB. This is hard coded here. */
extern code start_real;
codeAddress kernPhysStartAddress = &start_real;
extern code kernVirtStart;
early_printk("kernPhysStartAddress 0x%lx \n",
(unsigned long)kernPhysStartAddress);
/* We ignore memory below the 1meg boundary. PhysSize is the
* size of memory above the boundary.
*/
uval physSize = BOOT_MINIMUM_REAL_MEMORY -0x100000;
uval physStart = 0x0;
uval physEnd = physStart + 0x100000 + physSize;
early_printk("BOOT_MINIMUM_REAL_MEMORY 0x%lx, physStart 0x%lx,"
" physEnd 0x%lx, physSize 0x%lx \n",
BOOT_MINIMUM_REAL_MEMORY, physStart, physEnd, physSize);
/*
* We want to map all of physical memory into a V->R region. We choose a
* base for the V->R region (virtBase) that makes the kernel land correctly
* at its link origin, &kernVirtStart. This link origin must wind up
* mapped to the physical location at which the kernel was loaded
* (kernPhysStartAddress).
*/
uval virtBase = (uval) (&kernVirtStart - kernPhysStartAddress);
early_printk("&kernVirtStart 0x%lx virtBase 0x%lx \n",
(unsigned long long)&kernVirtStart,
(unsigned long long)virtBase);
/*
* Memory from __end_bss
* to the end of physical memory is available for allocation.
* Correct first for the 2MB page mapping the kernel.
*/
early_printk("__bss_end is 0x%lx physEnd is 0x%lx \n", __bss_end , physEnd);
uval allocStart = ALIGN_UP(__bss_end, SEGMENT_SIZE);
uval allocEnd = virtBase + physEnd;
early_printk("allocStart is 0x%lx allocEnd is 0x%lx \n",
allocStart, allocEnd);
memory->init(physStart, physEnd, virtBase, allocStart, allocEnd);
/*
* Remove mappings between allocStart and
* BOOT_MINIMUM_REAL_MEMORY to allow 4KB page mapping for
* that range. No need to tlb invalidate, unless they are
* touched (debugging). Actually we need to keep the first
* 2MB mapping above allocStart so that we can initialize the
* first 2 (or 3 if we need a PDP page as well) 4KB pages
* which are PDE and PTE pages for the V->R mapping before
* they are themselves mapped as 4KB pages.
*/
early_printk("top page real address is 0x%lx \n", (uval)&level4_pgt);
uval level1_pgt_virt = memory->virtFromPhys((uval)&level4_pgt);
early_printk("top page real address is 0x%lx \n", (uval)level4_pgt & ~0xfff);
early_printk("top page virtual address is 0x%lx \n", (uval )level1_pgt_virt);
for (vaddr = allocStart + SEGMENT_SIZE; vaddr < allocEnd; vaddr += SEGMENT_SIZE) {
#ifndef NDEBUG
// early_printk("removing pde, pml4 at virtual address 0x%lx \n", EARLY_VADDR_TO_L1_PTE_P(level1_pgt_virt, vaddr, memory));
TOUCH(EARLY_VADDR_TO_L1_PTE_P(level1_pgt_virt, vaddr, memory));
// early_printk("removing pde, pdp at virtual address 0x%lx \n", EARLY_VADDR_TO_L2_PTE_P(level1_pgt_virt, vaddr, memory));
TOUCH(EARLY_VADDR_TO_L2_PTE_P(level1_pgt_virt, vaddr, memory));
// early_printk("removing pde at virtual address 0x%lx \n", EARLY_VADDR_TO_L3_PTE_P(level1_pgt_virt, vaddr, memory));
TOUCH(EARLY_VADDR_TO_L3_PTE_P(level1_pgt_virt, vaddr, memory));
#endif /* #ifndef NDEBUG */
EARLY_VADDR_TO_L3_PTE_P(level1_pgt_virt, vaddr, memory)->P = 0;
EARLY_VADDR_TO_L3_PTE_P(level1_pgt_virt, vaddr, memory)->PS = 0;
EARLY_VADDR_TO_L3_PTE_P(level1_pgt_virt, vaddr, memory)->G = 0;
EARLY_VADDR_TO_L3_PTE_P(level1_pgt_virt, vaddr, memory)->Frame = 0;
__flush_tlb_one(vaddr);
}
/*
* Because of the 2MB page mapping for the kernel no
* unused space can be recuperated at a 4KB page granularity.
* We may want to map the fringe bss with 4KB page(s)
* or alternatively make free for (pinned only) 4KB allocation
* the unused 4KB pages unused in the 2MB pages at this point. XXX dangerous
*/
early_printk("Calling InitKernelMappings\n");
InitKernelMappings(0, memory);
// kernelInitArgs.onSim = onSim; not there anymore but where is it set XXX
kernelInitArgs.vp = 0;
kernelInitArgs.barrierP = 0;
#define LOOP_NUMBER 0x000fffff // iteration counter for delay
init_PIC(LOOP_NUMBER);
early_printk("Calling InitIdt\n");
InitIdt(); // initialize int handlers
early_printk("Calling enableHardwareInterrupts\n");
enableHardwareInterrupts();
early_printk("Calling thinwireInit\n");
thinwireInit(memory);
/* no thinwire console XXX taken from mips64 but check */
early_printk("Calling LocalConsole and switching to tty \n");
LocalConsole::Init(vp, memory, CONSOLE_CHANNEL, 1, 0 );
err_printf("Calling KernelInit.C\n");
/* Remove the V=R initial mapping only used for jumping to
* the final mapping, i.e the first 2MB. XXX todo should not
* do it until VGABASE has been relocated currently mapped
* V==R XXX cannot use early_printk() from now on. */
L3_PTE *p;
p = EARLY_VADDR_TO_L3_PTE_P(level1_pgt_virt,(uval)0x100000,memory);
p->P = 0;
p->PS = 0;
p->G = 0;
p->Frame = 0;
__flush_tlb_one(0x100000);
KernelInit(kernelInitArgs);
/* NOTREACHED */
}
void main(){
int increm ;
int i;
//Initialisation du PIC et chenillard
init_PIC() ;
flashLEDs();
//Initialisation des moteurs de la station sur appui du BP init :
LED1 = 1 ;
while(!Init) ;
initialisation_station() ;
LED1 = 0 ;
//Acquisiton des coordonnées de la station :
LED6 = 1;
while(1) {
//Attente de réception d'une trame
while (position_drone_flag == 0);
//Vérification de la validité de la trame
if ((position_drone.gps_x!=0)&&(position_drone.gps_y!=0)) break ; // on sort de la boucle que si les coordonnées reçues ont un sens
}
longitude_station = position_drone.gps_x ;
latitude_station = position_drone.gps_y ;
altitude_station = position_drone.altitude_baro ;
//libère le flag
position_drone_flag = 0;
Enable_Azimut = 1 ;
Enable_Profondeur = 1 ;
LED6 = 0;
// Début de la routinede suivi GPS :
LED2 = 1 ;
while(1) {
LED3 = 1-LED3 ;
// acquisition des coordonnées du drone :
//acquisition_GPS();
//Attente de réception d'une trame
while (position_drone_flag == 0);
longitude_GPS = position_drone.gps_x ;
latitude_GPS = position_drone.gps_y ;
altitude_GPS = position_drone.altitude_baro ;
//libère le flag
position_drone_flag = 0;
// calcul des différences de coordonnées drone / GPS :
coord_x = (long) (longitude_GPS - longitude_station) ; // 1° = 10 000
coord_y = (long) (latitude_GPS - latitude_station) ;
coord_z = altitude_GPS - altitude_station ; // en mètres
///////////////////////// SUIVI AZIMUT //////////////////////////////
// conversion des delta en angle du drone par rapport au Nord ]-pi;+pi°] (positif dans le sens horaire) :
angle_drone_azimut = angle_azimut_du_drone(coord_x,coord_y) ;
// détermination de l'angle courant du moteur par rapport au Nord
angle_moteur_azimut = position_moteur_azimut*2.0*pi/488.0 ;
// calcul du delta angle moteur / drone
// et attribution du sens de rotation des moteurs :
if( fabs(angle_moteur_azimut-angle_drone_azimut) > pi ) {
delta_angle_azimut = 2*pi - fabs(angle_moteur_azimut-angle_drone_azimut) ;
if (angle_moteur_azimut-angle_drone_azimut>0) sens_rotation_azimut = SENS_HOR ;
else sens_rotation_azimut = SENS_TRIGO ;
}
else {
delta_angle_azimut = fabs(angle_moteur_azimut-angle_drone_azimut) ;
if (angle_moteur_azimut-angle_drone_azimut>0) sens_rotation_azimut = SENS_TRIGO ;
else sens_rotation_azimut = SENS_HOR ;
}
// calcul du nombre de pas nécessaires pour atteindre l'angle souhaité :
nb_pas_azimut = (int) (0.5+(delta_angle_azimut*488.0/(2.0*pi))) ;
// déplacement du moteur azimut :
tracking_azimut(4) ;
///////////////////////// SUIVI PROFONDEUR //////////////////////////////
// conversion des delta distances (en degrés) en valeurs exploitables (en mètres)
// 111194.92... = Rayon_Terre * 2pi / 360
x = (int) (11.11949266*coord_x) ;
y = (int) (11.11949266*coord_y) ;
// calcul de l'angle du drone par rapport à l'horizontale ]-pi;+pi°] (positif vers le ciel) :
angle_drone_profondeur = angle_profondeur_du_drone(x,y,coord_z) ;
// détermination de l'angle courant du moteur par rapport à l'horizon :
angle_moteur_profondeur = position_moteur_profondeur*2.0*pi/246.0 ;
// calcul du delta angle moteur / drone
// et attribution du sens de rotation des moteurs :
///////// TODO : VERIFIER QUE TOUTES LES CONDITIONS D'ANGLES APPARAISSENT
delta_angle_profondeur = fabs(angle_moteur_profondeur-angle_drone_profondeur) ;
if ( angle_moteur_profondeur < angle_drone_profondeur ) sens_rotation_profondeur = SENS_HOR ;
else sens_rotation_profondeur = SENS_TRIGO ;
// calcul du nombre de pas nécessaires pour atteindre l'angle souhaité :
nb_pas_profondeur = (int) (0.5+(delta_angle_profondeur*246.0/(2.0*pi))) ;
// déplacement du moteur azimut :
tracking_profondeur(20) ;
} // Fin de la routine de tracking => on recommence
// Cette partie du code ne devrait jamais s'exécuter :
while(1) {
LED4 = 1 ;
Delay10KTCYx(100) ;
LED4 = 0 ;
LED5 = 1 ;
Delay10KTCYx(100) ;
LED5 = 0 ;
}
}
