void hdmirx_hw_reset(void)
{
hdmirx_print("%s %d\n", __func__, rx.port);
//WRITE_CBUS_REG(RESET0_REGISTER, 0x8); //reset HDMIRX module
//mdelay(10);
//clk_init();
hdmirx_wr_top(HDMIRX_TOP_INTR_MASKN, 0); //disable top interrupt gate
hdmirx_wr_top( HDMIRX_TOP_SW_RESET, 0x3f);
mdelay(1);
control_reset(0);
hdmirx_wr_top( HDMIRX_TOP_PORT_SEL, (1<<rx.port)); //EDID port select
hdmirx_interrupts_cfg(false); //disable dwc interrupt
if(hdcp_enable){
hdmi_rx_ctrl_hdcp_config(&rx.hdcp);
} else {
hdmirx_wr_bits_dwc( RA_HDCP_CTRL, HDCP_ENABLE, 0);
}
/*phy config*/
//hdmirx_phy_restart();
//hdmi_rx_phy_fast_switching(1);
phy_init(rx.port, 0); //port, dcm
/**/
/* control config */
control_init(rx.port);
audio_init();
packet_init();
hdmirx_audio_fifo_rst();
hdmirx_packet_fifo_rst();
/**/
control_reset(1);
/*enable irq */
hdmirx_wr_top(HDMIRX_TOP_INTR_STAT_CLR, ~0);
hdmirx_wr_top(HDMIRX_TOP_INTR_MASKN, 0x00001fff);
hdmirx_interrupts_hpd(true);
/**/
#ifndef USE_GPIO_FOR_HPD
hdmi_rx_ctrl_hpd(true);
hdmirx_wr_top( HDMIRX_TOP_HPD_PWR5V, (1<<5)|(1<<4)); //invert HDP output
#endif
/* wait at least 4 video frames (at 24Hz) : 167ms for the mode detection
recover the video mode */
mdelay(200);
/* Check If HDCP engine is in Idle state, if not wait for authentication time.
200ms is enough if no Ri errors */
if (hdmirx_rd_dwc(0xe0) != 0)
{
mdelay(200);
}
}
void motor_set_target_angle(int16 pAngle) {
// Reseting the control to avoid inertia with the integral part
control_reset();
if (pAngle > MAX_ANGLE) {
mot.targetAngle = MAX_ANGLE;
} else if (pAngle < 0) {
mot.targetAngle = 0;
} else {
mot.targetAngle = pAngle;
}
mot.targetAngle = motor_check_limit_angles(pAngle);
}
static void usb_reset(void)
{
/* Disable TIM7. */
tim_disable_counter(TIM7);
/* Disable DMA. */
dma_disable(DMA_TIM7_UP);
/* Clear queue. */
head = tail = 0;
/* Set endpoint type and address. */
usbdevfs_setup_endpoint(0, USBDEVFS_CONTROL, 0);
usbdevfs_setup_endpoint(1, USBDEVFS_ISOCHRONOUS, AS_ENUM);
/* Set default address. */
usbdevfs_set_device_address(0);
/* Reset control transfer state. */
control_reset();
}
uint8_t findPosition(uint8_t type)
{
disableSpinning();
if(type == REDSTART)
{
asserv_set_vitesse_low(&asserv);
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);//dist centre = 12.2
while(!trajectory_is_ended(&traj));
trajectory_goto_arel(&traj, END, -90.0);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
//asserv_set_distance()
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(20.0), fxx_from_double(0.0));
wait_ms(100);
printf("pos x : %ld pos y %ld \n",pos.x,pos.y);
printf("pos sum x : %lf pos y %lf \n",pos.sum_x,pos.sum_y);
asserv_stop(&asserv);//attention bug bizare avant
asserv_set_vitesse_fast(&asserv);
}
else if(type == YELLOWSTART)// team == YELLOW
{
asserv_set_vitesse_low(&asserv);
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);//dist centre = 12.2
while(!trajectory_is_ended(&traj));
trajectory_goto_arel(&traj, END, 90.0);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(280.0), fxx_from_double(0.0));
asserv_stop(&asserv);
asserv_set_vitesse_fast(&asserv);
}
else if(type == REDPAINT)
{
// asserv_set_vitesse_low(&asserv);
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos, &eps));
// printf("start pos: %d eps %lf \n",position_get_coor_eps(&pos, &eps),eps);
// set_goalCoor(G_LENGTH * 1 + 7);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv() && !actionIsFailed());
// if(actionIsFailed())return DONE;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_a(&traj, END, 90);
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
enableSpinning();
//if(actionIsFailed())return DONE;
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
//while(!trajectory_is_ended(&traj));
// if(actionIsFailed())return DONE;
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(160.0), fxx_from_double(90.0));
asserv_stop(&asserv);
asserv_set_vitesse_normal(&asserv);
}
else if(type == YELLOWPAINT)
{
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos, &eps));
// printf("start pos: %d eps %lf \n",position_get_coor_eps(&pos, &eps),eps);
// set_goalCoor(G_LENGTH * 1 + 8);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv() && !actionIsFailed());
// if(actionIsFailed())return DONE;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_a(&traj, END, -90);
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
enableSpinning();
//if(actionIsFailed())return DONE;
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
//if(actionIsFailed())return DONE;
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(160.0), fxx_from_double(-90.0));
asserv_stop(&asserv);
asserv_set_vitesse_normal(&asserv);
}
//printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
quadramp_reset(&asserv);
control_reset(&asserv);
pid_reset(&asserv);
diff_reset(&asserv);
printf("pos x : %ld pos y %ld \n",pos.x,pos.y);
printf("pos sum x : %lf pos y %lf \n",pos.sum_x,pos.sum_y);
sei();
enableSpinning();
return DONE;
}
void control_stop(void)
{
enable &= ~(1<<ENABLE_CONTROLLER);
control_reset();
}
void motor_set_target_current(int pCurrent) {
// Reseting the control to avoid inertia with the integral part
control_reset();
mot.targetCurrent = pCurrent;
}
void motor_set_target_angle_multi_turn_mode(int16 pAngle) {
// Reseting the control to avoid inertia with the integral part
control_reset();
mot.targetAngle = pAngle;
}
void control_update(short flight_mode)
{
// make a quick exit if we are disabled
if ( !autopilot_active ) {
return;
}
// reset the autopilot if requested
if ( autopilot_reinit ) {
control_reset();
autopilot_reinit = false;
}
// optional: use channel #6 to change the autopilot target value
// double min_value = -35.0;
// double max_value = 35.0;
// double tgt_value = (max_value - min_value) *
// ((double)servo_in.chn[5] / 65535.0) + min_value;
// ap_target->setFloatValue( tgt_value );
// update the autopilot stages
ap.update( 0.04 ); // dt = 1/25
/* printf("%.2f %.2f\n", aileron_out_node->getFloatValue(),
elevator_out_node->getFloatValue()); */
static SGPropertyNode *vert_speed_fps
= fgGetNode("/velocities/vertical-speed-fps", true);
static SGPropertyNode *true_alt
= fgGetNode("/position/altitude-ft", true);
/* printf("%.1f %.2f %.2f\n",
true_alt->getFloatValue(),
vert_speed_fps->getFloatValue(),
elevator_out_node->getFloatValue()); */
// initialize the servo command array to central values so we don't
// inherit junk
for ( int i = 0; i < 8; ++i ) {
servo_out.chn[i] = 32768;
}
if ( elevon_mix->getBoolValue() ) {
// elevon mixing mode
//aileron
servo_out.chn[0] = 32768
+ (int16_t)(aileron_out_node->getFloatValue() * 32768)
+ (int16_t)(elevator_out_node->getFloatValue() * 32768);
//elevator
servo_out.chn[1] = 32768
+ (int16_t)(aileron_out_node->getFloatValue() * 32768)
- (int16_t)(elevator_out_node->getFloatValue() * 32768);
} else {
// conventional airframe mode
//aileron
servo_out.chn[0] = 32768
+ (int16_t)(aileron_out_node->getFloatValue() * 32768);
//elevator
servo_out.chn[1] = 32768
+ (int16_t)(elevator_out_node->getFloatValue() * 32768);
}
//throttle
servo_out.chn[2] = 32768
+ (uint16_t)(throttle_out_node->getFloatValue() * 32768);
// rudder
servo_out.chn[3] = 32768
+ (int16_t)(rudder_out_node->getFloatValue() * 32768);
// time stamp the packet for logging
servo_out.time = get_Time();
// send commanded servo positions to the MNAV
send_short_servo_cmd();
}
