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(); }