static int
isp1301_set_peripheral(struct otg_transceiver *otg, struct usb_gadget *gadget)
{
struct isp1301 *isp = container_of(otg, struct isp1301, otg);
if (!otg || isp != the_transceiver)
return -ENODEV;
if (!gadget) {
OTG_IRQ_EN_REG = 0;
if (!isp->otg.default_a)
enable_vbus_draw(isp, 0);
usb_gadget_vbus_disconnect(isp->otg.gadget);
isp->otg.gadget = 0;
power_down(isp);
return 0;
}
#ifdef CONFIG_USB_OTG
isp->otg.gadget = gadget;
dev_dbg(&isp->client.dev, "registered gadget\n");
/* gadget driver may be suspended until vbus_connect () */
if (isp->otg.host)
return isp1301_otg_enable(isp);
return 0;
#elif !defined(CONFIG_USB_OHCI_HCD) && !defined(CONFIG_USB_OHCI_HCD_MODULE)
isp->otg.gadget = gadget;
// FIXME update its refcount
OTG_CTRL_REG = (OTG_CTRL_REG & OTG_CTRL_MASK
& ~(OTG_XCEIV_OUTPUTS|OTG_CTRL_BITS))
| OTG_ID;
power_up(isp);
isp->otg.state = OTG_STATE_B_IDLE;
if (machine_is_omap_h2())
isp1301_set_bits(isp, ISP1301_MODE_CONTROL_1, MC1_DAT_SE0);
isp1301_set_bits(isp, ISP1301_INTERRUPT_RISING,
INTR_SESS_VLD);
isp1301_set_bits(isp, ISP1301_INTERRUPT_FALLING,
INTR_VBUS_VLD);
dev_info(&isp->client.dev, "B-Peripheral sessions ok\n");
dump_regs(isp, __func__);
/* If this has a Mini-AB connector, this mode is highly
* nonstandard ... but can be handy for testing, so long
* as you don't plug a Mini-A cable into the jack.
*/
if (isp1301_get_u8(isp, ISP1301_INTERRUPT_SOURCE) & INTR_VBUS_VLD)
b_peripheral(isp);
return 0;
#else
dev_dbg(&isp->client.dev, "peripheral sessions not allowed\n");
return -EINVAL;
#endif
}
double power(double n, int p)
{
double pow = 1;
int i;
int neg = 0;
if (n == 0) {
if (p == 0) {
printf("0 raised to the power of 0 is undefined\n");
printf("(returning a value of 1)\n");
pow = 1;
}
else
pow = 1;
}
else {
if (p < 0) {
neg = 1;
p *= -1;
}
pow = power_up(n, p);
// for (i = 1; i <= p; i++)
// pow *= n;
if (neg)
pow = 1 / pow;
}
return pow;
}
static int ov5693_s_power(struct v4l2_subdev *sd, int on)
{
struct ov5693_device *dev = to_ov5693_sensor(sd);
struct i2c_client *client = v4l2_get_subdevdata(sd);
int ret = 0;
mutex_lock(&dev->input_lock);
if (on == 0) {
if (dev->vcm_driver && dev->vcm_driver->power_down)
ret = dev->vcm_driver->power_down(sd);
if (ret)
dev_err(&client->dev, "vcm power-down failed.\n");
ret = power_down(sd);
} else {
ret = power_up(sd);
if (ret)
goto done;
ret = ov5693_init(sd);
if (ret)
goto done;
if (dev->vcm_driver && dev->vcm_driver->power_up)
ret = dev->vcm_driver->power_up(sd);
if (ret)
dev_err(&client->dev, "vcm power-up failed.\n");
}
done:
mutex_unlock(&dev->input_lock);
return ret;
}
static int ov2680_s_config(struct v4l2_subdev *sd,
int irq, void *platform_data)
{
struct ov2680_device *dev = to_ov2680_sensor(sd);
struct i2c_client *client = v4l2_get_subdevdata(sd);
int ret = 0;
if (!platform_data)
return -ENODEV;
dev->platform_data =
(struct camera_sensor_platform_data *)platform_data;
mutex_lock(&dev->input_lock);
/* power off the module, then power on it in future
* as first power on by board may not fulfill the
* power on sequqence needed by the module
*/
ret = power_down(sd);
if (ret) {
dev_err(&client->dev, "ov2680 power-off err.\n");
goto fail_power_off;
}
ret = power_up(sd);
if (ret) {
dev_err(&client->dev, "ov2680 power-up err.\n");
goto fail_power_on;
}
ret = dev->platform_data->csi_cfg(sd, 1);
if (ret)
goto fail_csi_cfg;
/* config & detect sensor */
ret = ov2680_detect(client);
if (ret) {
dev_err(&client->dev, "ov2680_detect err s_config.\n");
goto fail_csi_cfg;
}
/* turn off sensor, after probed */
ret = power_down(sd);
if (ret) {
dev_err(&client->dev, "ov2680 power-off err.\n");
goto fail_csi_cfg;
}
mutex_unlock(&dev->input_lock);
return 0;
fail_csi_cfg:
dev->platform_data->csi_cfg(sd, 0);
fail_power_on:
power_down(sd);
dev_err(&client->dev, "sensor power-gating failed\n");
fail_power_off:
mutex_unlock(&dev->input_lock);
return ret;
}
max287x<max287x_regs_t>::max287x(write_fn func) :
_can_sync(false),
_config_for_sync(false),
_write_all_regs(true),
_write(func),
_delay_after_write(true)
{
power_up();
}
double power_up (double n, double p)
{
if (p > 0)
n *= power_up(n, (p - 1));
else
n = 1;
return n;
}
void
read_chip(void)
{
int r;
power_up();
r = chips[chipindex].read_func(buffersize, pagesize);
file_save();
power_down();
}
/* add or disable the host device+driver */
static int
isp1301_set_host(struct otg_transceiver *otg, struct usb_bus *host)
{
struct isp1301 *isp = container_of(otg, struct isp1301, otg);
if (!otg || isp != the_transceiver)
return -ENODEV;
if (!host) {
omap_writew(0, OTG_IRQ_EN);
power_down(isp);
isp->otg.host = NULL;
return 0;
}
#ifdef CONFIG_USB_OTG
isp->otg.host = host;
dev_dbg(&isp->client->dev, "registered host\n");
host_suspend(isp);
if (isp->otg.gadget)
return isp1301_otg_enable(isp);
return 0;
#elif !defined(CONFIG_USB_GADGET_OMAP)
// FIXME update its refcount
isp->otg.host = host;
power_up(isp);
if (machine_is_omap_h2())
isp1301_set_bits(isp, ISP1301_MODE_CONTROL_1, MC1_DAT_SE0);
dev_info(&isp->client->dev, "A-Host sessions ok\n");
isp1301_set_bits(isp, ISP1301_INTERRUPT_RISING,
INTR_ID_GND);
isp1301_set_bits(isp, ISP1301_INTERRUPT_FALLING,
INTR_ID_GND);
/* If this has a Mini-AB connector, this mode is highly
* nonstandard ... but can be handy for testing, especially with
* the Mini-A end of an OTG cable. (Or something nonstandard
* like MiniB-to-StandardB, maybe built with a gender mender.)
*/
isp1301_set_bits(isp, ISP1301_OTG_CONTROL_1, OTG1_VBUS_DRV);
dump_regs(isp, __func__);
return 0;
#else
dev_dbg(&isp->client->dev, "host sessions not allowed\n");
return -EINVAL;
#endif
}
static int ov2680_s_power(struct v4l2_subdev *sd, int on)
{
int ret;
if (on == 0){
ret = power_down(sd);
} else {
ret = power_up(sd);
if (!ret)
return ov2680_init(sd);
}
return ret;
}
void power_down() //dodać zapis do eepromu
{
eeprom_write_byte((uint8_t*)0, (uint8_t)VOL); // Save volume to eeprom
eeprom_write_byte((uint8_t*)8, (uint8_t)id_tab[0][0]);
eeprom_write_byte((uint8_t*)16, (uint8_t)id_tab[0][1]);
eeprom_write_byte((uint8_t*)24, (uint8_t)id_tab[0][2]);
eeprom_write_byte((uint8_t*)32, (uint8_t)id_tab[1][0]);
eeprom_write_byte((uint8_t*)40, (uint8_t)id_tab[1][1]);
eeprom_write_byte((uint8_t*)48, (uint8_t)id_tab[1][2]);
eeprom_write_byte((uint8_t*)56, (uint8_t)id_tab[2][0]);
eeprom_write_byte((uint8_t*)64, (uint8_t)id_tab[2][1]);
eeprom_write_byte((uint8_t*)72, (uint8_t)id_tab[2][2]);
eeprom_write_byte((uint8_t*)80, (uint8_t)id_tab[3][0]);
eeprom_write_byte((uint8_t*)88, (uint8_t)id_tab[3][1]);
eeprom_write_byte((uint8_t*)96, (uint8_t)id_tab[3][2]);
cli();
PCICR &= ~(1 << PCIE2) & ~(1 << PCIE0); // disable PCINT
EIMSK |= (1 << INT1); // Enebale INT1 external interrupt on low state
power_flag = 0;
led_enable(0);
while (!(B1_PIN & (1 << B1)));
SMCR |= (1 << SM1) |(1 << SE);
sleep_enable();
sei();
sleep_cpu();
power_up();
_delay_ms(50);
if (((B1_PIN & (1 << B1)) ) !=0 )
{
power_flag = 0;
}
else
{
led_enable(1);
_delay_ms(5);
power_flag = 1;
SPK_FREQ = 2;
cli();
play_speaker(50);
sei();
}
}
static int ov5693_s_config(struct v4l2_subdev *sd,
int irq, void *platform_data)
{
struct ov5693_device *dev = to_ov5693_sensor(sd);
struct i2c_client *client = v4l2_get_subdevdata(sd);
int ret = 0;
if (platform_data == NULL)
return -ENODEV;
mutex_lock(&dev->input_lock);
dev->platform_data = platform_data;
ret = power_up(sd);
if (ret) {
dev_err(&client->dev, "ov5693 power-up err.\n");
goto fail_power_on;
}
ret = dev->platform_data->csi_cfg(sd, 1);
if (ret)
goto fail_csi_cfg;
/* config & detect sensor */
ret = ov5693_detect(client);
if (ret) {
dev_err(&client->dev, "ov5693_detect err s_config.\n");
goto fail_csi_cfg;
}
/* turn off sensor, after probed */
ret = power_down(sd);
if (ret) {
dev_err(&client->dev, "ov5693 power-off err.\n");
goto fail_csi_cfg;
}
mutex_unlock(&dev->input_lock);
return 0;
fail_csi_cfg:
dev->platform_data->csi_cfg(sd, 0);
fail_power_on:
power_down(sd);
dev_err(&client->dev, "sensor power-gating failed\n");
mutex_unlock(&dev->input_lock);
return ret;
}
void
erase_chip(void)
{
int r,
b;
power_up();
r = chips[chipindex].erase_func(buffersize, pagesize);
b = generic_blank_check(buffersize, pagesize);
power_down();
if (r < 0 || b < 0)
msg_error();
}
static int isp1301_otg_enable(struct isp1301 *isp)
{
power_up(isp);
otg_init(isp);
/* NOTE: since we don't change this, this provides
* a few more interrupts than are strictly needed.
*/
isp1301_set_bits(isp, ISP1301_INTERRUPT_RISING,
INTR_VBUS_VLD | INTR_SESS_VLD | INTR_ID_GND);
isp1301_set_bits(isp, ISP1301_INTERRUPT_FALLING,
INTR_VBUS_VLD | INTR_SESS_VLD | INTR_ID_GND);
dev_info(&isp->client->dev, "ready for dual-role USB ...\n");
return 0;
}
void
program_chip(void)
{
int r,
b,
v;
file_open();
power_up();
chips[chipindex].erase_func(buffersize, pagesize);
b = generic_blank_check(buffersize, pagesize);
r = chips[chipindex].burn_func(buffersize, pagesize);
v = generic_verify(buffersize, pagesize);
power_down();
if (r < 0 || b < 0 || v < 0)
msg_error();
}
static int hm5040_s_power(struct v4l2_subdev *sd, int on)
{
struct hm5040_device *dev = to_hm5040_sensor(sd);
int ret;
pr_info("%s: on %d\n", __func__, on);
if (on == 0) {
if (dev->vcm_driver && dev->vcm_driver->power_down)
ret = dev->vcm_driver->power_down(sd);
return power_down(sd);
} else {
if (dev->vcm_driver && dev->vcm_driver->power_up)
ret = dev->vcm_driver->power_up(sd);
ret = power_up(sd);
if (!ret)
return hm5040_init(sd);
}
return ret;
}
/**
* \brief Configuration function for the MPU9250 sensor.
*
* \param type Activate, enable or disable the sensor. See below
* \param enable
*
* When type == SENSORS_HW_INIT we turn on the hardware
* When type == SENSORS_ACTIVE and enable==1 we enable the sensor
* When type == SENSORS_ACTIVE and enable==0 we disable the sensor
*/
static int
configure(int type, int enable)
{
switch(type) {
case SENSORS_HW_INIT:
ti_lib_rom_ioc_pin_type_gpio_input(BOARD_IOID_MPU_INT);
ti_lib_ioc_io_port_pull_set(BOARD_IOID_MPU_INT, IOC_IOPULL_DOWN);
ti_lib_ioc_io_hyst_set(BOARD_IOID_MPU_INT, IOC_HYST_ENABLE);
ti_lib_rom_ioc_pin_type_gpio_output(BOARD_IOID_MPU_POWER);
ti_lib_ioc_io_drv_strength_set(BOARD_IOID_MPU_POWER, IOC_CURRENT_4MA,
IOC_STRENGTH_MAX);
ti_lib_gpio_clear_dio(BOARD_IOID_MPU_POWER);
elements = MPU_9250_SENSOR_TYPE_NONE;
break;
case SENSORS_ACTIVE:
if(((enable & MPU_9250_SENSOR_TYPE_ACC) != 0) ||
((enable & MPU_9250_SENSOR_TYPE_GYRO) != 0)) {
PRINTF("MPU: Enabling\n");
elements = enable & MPU_9250_SENSOR_TYPE_ALL;
power_up();
state = SENSOR_STATE_BOOTING;
} else {
PRINTF("MPU: Disabling\n");
if(HWREG(GPIO_BASE + GPIO_O_DOUT31_0) & BOARD_MPU_POWER) {
/* Then check our state */
elements = MPU_9250_SENSOR_TYPE_NONE;
ctimer_stop(&startup_timer);
sensor_sleep();
while(ti_lib_i2c_master_busy(I2C0_BASE));
state = SENSOR_STATE_DISABLED;
ti_lib_gpio_clear_dio(BOARD_IOID_MPU_POWER);
}
}
break;
default:
break;
}
return state;
}
int main(int argc, char *argv[])
{
int which;
int time;
if ( argc <2 )
{
printf("Usage: %s <KickerNumber> <Duration>\n",argv[0]);
printf(" 0xF000 0xF000 -> Master OFF (default)\n");
printf(" 0xFFFF 0xFFFF -> Master ON\n");
return(3);
}
initKicker();
if (argc==3)
{
which = atoi(argv[1]);
time = atoi(argv[2]);
kick(which,time);
}
if (argc==2)
{
printf("Argv = %s \n",argv[1]);
if (strcmp(argv[1],"on")==0) power_up();
if (strcmp(argv[1],"off")==0) power_down();
}
deinitKicker();
return 0;
}
int
probe_chip (void)
{
int i;
power_up ();
for (i = 0; chips[i].name != NULL; i++)
{
chips[i].probe_func();
if ((chips[i].id1 == id1) && (chips[i].id2 == id2))
{
chipindex = i;
chip_selected ();
power_down ();
return 1;
};
};
chipindex = i - 1;
power_down ();
return -1;
}
int switch_boot_mode(void)
{
// unsigned long hold_time = 50000, polling_time = 10000, tmp;
unsigned long upgrade_step;
int ret=0;
act8942_init(&act8942_pdata);
//act8942_dump();
upgrade_step = simple_strtoul (getenv ("upgrade_step"), NULL, 16);
printf("upgrade_step = %d\n", upgrade_step);
saradc_enable();
#ifdef ENABLE_FONT_RESOURCE
RegisterFont(DEFAULT_FONT);
#endif
ret=isVolAKeyPress();
if(ret==1)
aml_autoscript();
powerkey_hold(0);
#ifdef CONFIG_AML_TINY_USBTOOL
usb_boot(1);
#endif
if(upgrade_step == 2)
{
switch(reboot_mode)
{
case AMLOGIC_NORMAL_BOOT:
{
ret=keypress_to_upgrade();
if(ret==0)
return 0;
printf("AMLOGIC_NORMAL_BOOT...\n");
power_up();
logo_display();
return 1;
}
case AMLOGIC_FACTORY_RESET_REBOOT:
{
printf("AMLOGIC_FACTORY_RESET_REBOOT...\n");
power_up();
logo_display();
run_command ("nand read ${recovery_name} ${loadaddr} 0 ${recovery_size}", 0);
run_command ("bootm", 0);
break;
}
case AMLOGIC_UPDATE_REBOOT:
{
printf("AMLOGIC_UPDATE_REBOOT...\n");
power_up();
logo_display();
run_command ("set upgrade_step 0", 0);
run_command ("save", 0);
upgrade_step = 0;
break;
}
default:
{
printf("AMLOGIC_CHARGING_REBOOT...\n");
if(is_ac_connected)
{
power_up();
#ifdef CONFIG_BATTERY_CHARGING
//battery_charging();
#endif
logo_display();
}
else
{
powerkey_hold(0);
#ifdef CONFIG_BATTERY_CHARGING
if(get_powerkey_hold_count())
{
logo_display();
if(get_battery_percentage() < 10)
{
power_low_display();
sdelay(2);
power_down();
printf("Low Power!!!\nPower Down!\n");
hang();
}
#else
if(powerkey_hold(1000))
{
#endif
logo_display();
power_up();
printf("Power Up!\n");
}
else
{
power_down();
printf("Power Down!\n");
hang();
}
}
break;
}
}
}
else
{
power_up();
printf("Upgrade step %d...\n", upgrade_step);
}
if(upgrade_step == 0)
{
#ifdef CONFIG_AML_TINY_USBTOOL
usb_boot(1);
#endif
display_messge("upgrade step 1! Don't Power Off!");
if(upgrade_bootloader())
{
run_command ("set upgrade_step 1", 0);
run_command ("save", 0);
run_command ("reset", 0);
hang();
}
else
{
printf("### ERROR: u-boot write failed!!!\n");
return -1;
}
}
else if(upgrade_step == 1)
{
display_messge("upgrade step 2! Don't Power Off!");
run_command ("defenv", 0);
run_command ("set upgrade_step 2", 0);
run_command ("save", 0);
into_recovery();
}
//added by Elvis for added fool idle
/*get_key();
get_key();
while(hold_time > 0)
{
udelay(polling_time);
tmp = get_key();
printf("get_key(): %d\n", tmp);
if(!tmp) break;
hold_time -= polling_time;
}
if(hold_time > 0)
{
printf("Normal Start...\n");
return 1;
}
else
{
display_messge("upgrading... please wait");
if(upgrade_bootloader())
{
run_command ("set upgrade_step 1", 0);
run_command ("save", 0);
run_command ("reset", 0);
hang();
}
run_command ("set upgrade_step 2", 0);
run_command ("save", 0);
into_recovery();
}
*/
ret=keypress_to_upgrade();
return ret;
//return 0;
}
int main(int argc, char **argv)
{
printf("ur5 Server Start\n");
// JointVector tcp_home= {0.0, -191.0, 600.0, 0.0, -2.2211, -2.2211};
JointVector joint_home_rad={0.0000, -1.5708, 0.0000, -1.5708, 0.0000, 0.0000};
JointVector joint_home={0.0000, -90, 0.0000, -90, 0.0000, 0.0000};
JointVector init_two ={-6.5018, -95.3469, -29.8123, -194.7758, -59.0822, -62.3032};
JointVector init_right_side = {0.309199, -1.240009, 0.323497, -1.039543, 0.541075, 0.486453};
JointVector initialP_rad = {-0.330294, -1.881878, -0.290919, -2.243194, -0.660115, -0.704284};
JointVector viereck[4] = {{43.24, -140.88, -30.05, -12.83, 129.28, -0.27},
{130.67, -140.87, -30.05, -12.75, 44.08, -0.27},
{130.67, -187.90, 6.46, -1.22, 43.35, -0.27},
{43.24, -187.90, -30.05, 3.87, 100.32, -0.27}
};
JointVector PosOne= {36.12, -112.88, -8.78, -24.53, 88.39, 16.64};
int i,h,n=0;
for(i=0; i<4;i++){
for(h=0;h<6;h++){
viereck[i][h]= deg_to_rad(viereck[i][h]);
}
}
for(i=0;i<MSG_BUFFER_SIZE;i++) msg_buffer[i].text = msg_text_buffers[i];
// JointVector initialP={-30.4347, -132.2621, -40.8718, -149.8236, -59.1151, -62.2781};
// int i;
// printf("joint_home ={");
// for(i=0; i<6; i++){
// joint_home_rad[i]= deg_to_rad(joint_home[i]);
// joint_home[i] = rad_to_deg(joint_home_rad[i]);
// printf("%3.4f%s",joint_home_rad[i], i<5 ? ", " : "};");
//// }
// MovePTPPacket move_ptp_packet;
for(i=0; i<6; i++){
PosOne[i]= deg_to_rad(PosOne[i]);}
// move_ptp(joint_home_rad, PosOne , &move_ptp_packet);
// return 0;
struct connection_data server;
fd_set masterfds, readfds;
struct timeval timeout,send_time_start, send_time_end;
int rc;
pthread_t tcp_server_thread;
MovePTPPacket move_ptp_packet;
server.initialize_direction = -1.0;
read_args(argc, argv, &server);
initialize_direction = server.initialize_direction;
printf("initialize direction: %f", initialize_direction);
rc = pthread_create(&tcp_server_thread, NULL, &start_tcp_server, &server);
if(rc < 0){
quit_program=true;
}
int connection_timeout_count=0;
pva_packet.header.protocol_id=PVA_PACKET_ID;
p_packet.header.protocol_id=PPACKET_ID;
//--------------------------- Init Robot -------------------------------------------------------
configuration_load();
open_interface();
power_up();
set_wrench();
if(!initialize(0)){
puts("could not initialize robot");
exit(EXIT_FAILURE);
}
settle();
setup_sigint();
setup_sigpipe();
memcpy(&last_pva_packet, &pva_packet,sizeof(PVAPacket));
for(i=0;i<6;i++) last_pva_packet.acceleration[i] = 0.0;
for(i=0;i<6;i++) last_pva_packet.velocity[i] = 0.0;
bool timeout_flag=false;
char *buff[sizeof(PVAPacket)];
//------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------
// for(pva_packet.header.cycle_number =0; quit_program == false; pva_packet.header.cycle_number++) {
// robotinterface_read_state_blocking();
// pthread_mutex_lock(&connect_mutex);
// if(connection_timeout_count > TIMEOUT_TOLERANCE){
// connection_timeout_count=0;
// connected=false;
// }
// //--------------------------- Stuff roboter does -------------------------------------------------------
// robotinterface_get_actual(servo_packet.position, servo_packet.velocity);
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //---------------------------- send -------------------------------------------------------------------
// if(connected){
// gettimeofday(&send_time_start,0);
// if(send(client_fd, (char*) &pva_packet , sizeof(pva_packet),0) < 1){
// connected=false;
// }
// }
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //---------------------------- recieve -----------------------------------------------------------------
// timeout_flag=false;
// if(connected){
// FD_ZERO(&masterfds);
// FD_SET(client_fd, &masterfds);
// memcpy(&readfds, &masterfds, sizeof(fd_set));
// gettimeofday(&send_time_end,0);
// timeval_diff(&timeout, &send_time_start, &send_time_end);
// timeout.tv_usec = 6000 - timeout.tv_usec;
// if (select(client_fd +1, &readfds, NULL, NULL, &timeout) < 0) {
// pthread_mutex_lock(&connect_mutex);
// connected=false;
// pthread_mutex_unlock(&connect_mutex);
// }
// if(FD_ISSET(client_fd, &readfds)){
// n=recv(client_fd, buff, sizeof(buff), 0);
// if (n < 1 || n != sizeof(PVAPacket)){
// pthread_mutex_lock(&connect_mutex);
// connected=false;
// pthread_mutex_unlock(&connect_mutex);
// }
// }else{
// connection_timeout_count++;
// if(connection_timeout_count< TIMEOUT_TOLERANCE)
// timeout_flag=true;
// }
// if(!timeout_flag && connected){
// memcpy(&pva_packet, buff, n);
// if(connection_timeout_count) connection_timeout_count--;
// }
// }
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //---------------------------- do stuff with data ------------------------------------------------------
// // TODO check for errors-----
// //------------------------------------------------------------------------------------------------------
// if(!timeout_flag && connected){
// // save pva to last if something went wrong;
// memcpy(&last_pva_packet, &pva_packet, sizeof(PVAPacket);
// }
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //--------------------------- Stuff roboter does -------------------------------------------------------
// if(connected){
// if(!timeout){
// robotinterface_command_position_velocity_acceleration(pva_packet.position,
// pva_packet.velocity,
// pva_packet.acceleration);
// }else{ //timeout
// if(connection_timeout_count<3){
// connected=false;
// robotinterface_command_velocity(zero_vector);
// }
// // cause of timeout we take the last pva_packet
// robotinterface_command_position_velocity_acceleration(last_pva_packet.position,
// last_pva_packet.velocity,
// last_pva_packet.acceleration);
// }
// }else{
// robotinterface_command_velocity(zero_vector);
// }
// pthread_mutex_unlock(&connect_mutex);
// robotinterface_send();
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// }
// // ---- quit programm
printf("shutdown robot...\n");
//---- shutdown robot ---------
// move to home ----
move_to_position(viereck[0]);
// move_to_position(joint_home_rad);
// move_to_position(PosOne);
// ---
puts("- Speeding down");
int j;
for (j = 0; j < 10; j++)
{
robotinterface_read_state_blocking();
robotinterface_command_velocity(zero_vector);
robotinterface_send();
}
puts("close robotinterface");
robotinterface_close();
puts("Done!");
printf("shutdown program\n");
return 0;
}
/**
* \ingroup sd_raw
* Initializes memory card communication.
*
* \returns 0 on failure, 1 on success.
*/
uint8_t sd_raw_init()
{
/* Configure Chip-Select Pin*/
configure_pin_ss();
#if defined(AVRNETIO) && defined(AVRNETIO_ADDON)
/* enable inputs for reading card status */
DDRD |= (1<<PD7);
PORTD |= (1<<PD7);
DDRD |= (1<<PD5);
PORTD |= (1<<PD5);
configure_pin_available();
// configure_pin_locked();
#elif defined(AVRNETIO) && !defined(AVRNETIO_ADDON)
/* enable inputs for reading card status */
configure_pin_available();
configure_pin_locked();
configure_pin_available_pullup();
configure_pin_locked_pullup();
#endif
/* unselect MMC-Card */
select_card();
unselect_card();
/* SPI-Bus Init */
SPI_init( spi_bus_num );
#if defined(AVRNETIO) && !defined(AVRNETIO_ADDON)
/* Power up the MMC-Interface */
configure_power_up();
power_up();
#endif
/* initialization procedure */
sd_raw_card_type = 0;
if(!sd_raw_available())
{
return 0;
}
/* card needs 74 cycles minimum to start up */
for(uint8_t i = 0; i < 100; ++i)
{
/* wait 8 clock cycles */
sd_raw_rec_byte();
}
/* address card */
select_card();
/* reset card */
uint8_t response;
for(uint16_t i = 0; ; ++i)
{
response = sd_raw_send_command(CMD_GO_IDLE_STATE, 0);
if(response == (1 << R1_IDLE_STATE))
break;
if(i == 0xfff)
{
unselect_card();
return 0;
}
}
#if SD_RAW_SDHC
/* check for version of SD card specification */
response = sd_raw_send_command(CMD_SEND_IF_COND, 0x100 /* 2.7V - 3.6V */ | 0xaa /* test pattern */);
if((response & (1 << R1_ILL_COMMAND)) == 0)
{
sd_raw_rec_byte();
sd_raw_rec_byte();
if((sd_raw_rec_byte() & 0x01) == 0)
{
unselect_card();
return 0; /* card operation voltage range doesn't match */
}
if(sd_raw_rec_byte() != 0xaa)
{
unselect_card();
return 0; /* wrong test pattern */
}
/* card conforms to SD 2 card specification */
sd_raw_card_type |= (1 << SD_RAW_SPEC_2);
}
else
#endif
{
/* determine SD/MMC card type */
sd_raw_send_command(CMD_APP, 0);
response = sd_raw_send_command(CMD_SD_SEND_OP_COND, 0);
if((response & (1 << R1_ILL_COMMAND)) == 0)
{
/* card conforms to SD 1 card specification */
sd_raw_card_type |= (1 << SD_RAW_SPEC_1);
}
else
{
/* MMC card */
}
}
/* wait for card to get ready */
for(uint16_t i = 0; ; ++i)
{
if(sd_raw_card_type & ((1 << SD_RAW_SPEC_1) | (1 << SD_RAW_SPEC_2)))
{
uint32_t arg = 0;
#if SD_RAW_SDHC
if(sd_raw_card_type & (1 << SD_RAW_SPEC_2))
arg = 0x40000000;
#endif
sd_raw_send_command(CMD_APP, 0);
response = sd_raw_send_command(CMD_SD_SEND_OP_COND, arg);
}
else
{
response = sd_raw_send_command(CMD_SEND_OP_COND, 0);
}
if((response & (1 << R1_IDLE_STATE)) == 0)
break;
if(i == 0x7fff)
{
unselect_card();
return 0;
}
}
#if SD_RAW_SDHC
if(sd_raw_card_type & (1 << SD_RAW_SPEC_2))
{
if(sd_raw_send_command(CMD_READ_OCR, 0))
{
unselect_card();
return 0;
}
if(sd_raw_rec_byte() & 0x40)
sd_raw_card_type |= (1 << SD_RAW_SPEC_SDHC);
sd_raw_rec_byte();
sd_raw_rec_byte();
sd_raw_rec_byte();
}
#endif
/* set block size to 512 bytes */
if(sd_raw_send_command(CMD_SET_BLOCKLEN, 512))
{
unselect_card();
return 0;
}
/* deaddress card */
unselect_card();
// /* switch to highest SPI frequency possible */
// SPCR &= ~((1 << SPR1) | (1 << SPR0)); /* Clock Frequency: f_OSC / 4 */
// SPSR |= (1 << SPI2X); /* Doubled Clock Frequency: f_OSC / 2 */
#if !SD_RAW_SAVE_RAM
/* the first block is likely to be accessed first, so precache it here */
raw_block_address = (offset_t) -1;
#if SD_RAW_WRITE_BUFFERING
raw_block_written = 1;
#endif
if(!sd_raw_read(0, raw_block, sizeof(raw_block)))
{
unselect_card();
return 0;
}
#endif
return 1;
}
nsapi_error_t PPPCellularInterface::connect()
{
nsapi_error_t retcode;
bool success;
bool did_init = false;
const char *apn_config = NULL;
if (dev_info.ppp_connection_up) {
return NSAPI_ERROR_IS_CONNECTED;
}
do {
retry_init:
/* setup AT parser */
setup_at_parser();
if (!initialized) {
/* If we have hangup (eg DCD) detection, we don't want it active
* as long as we are using ATCmdParser.
* As soon as we get into data mode, we will turn it back on. */
enable_hup(false);
if (!power_up()) {
return NSAPI_ERROR_DEVICE_ERROR;
}
retcode = initialize_sim_card();
if (retcode != NSAPI_ERROR_OK) {
return retcode;
}
success = nwk_registration(PACKET_SWITCHED) //perform network registration
&& get_CCID(_at)//get integrated circuit ID of the SIM
&& get_IMSI(_at)//get international mobile subscriber information
&& get_IMEI(_at)//get international mobile equipment identifier
&& get_MEID(_at)//its same as IMEI
&& set_CMGF(_at)//set message format for SMS
&& set_CNMI(_at);//set new SMS indication
if (!success) {
return NSAPI_ERROR_NO_CONNECTION;
}
#if MBED_CONF_PPP_CELL_IFACE_APN_LOOKUP
if (!apn_config) {
apn_config = apnconfig(dev_info.imsi);
}
#endif
/* Check if user want skip SIM pin checking on boot up */
if (set_sim_pin_check_request) {
retcode = do_sim_pin_check(_at, _pin);
if (retcode != NSAPI_ERROR_OK) {
return retcode;
}
/* set this request to false, as it is unnecessary to repeat in case of retry */
set_sim_pin_check_request = false;
}
/* check if the user requested a sim pin change */
if (change_pin) {
retcode = do_change_sim_pin(_at, _pin, _new_pin);
if (retcode != NSAPI_ERROR_OK) {
return retcode;
}
/* set this request to false, as it is unnecessary to repeat in case of retry */
change_pin = false;
}
#if MBED_CONF_PPP_CELL_IFACE_APN_LOOKUP
if (apn_config) {
_apn = _APN_GET(apn_config);
_uname = _APN_GET(apn_config);
_pwd = _APN_GET(apn_config);
tr_info("Looked up APN %s.", _apn);
}
#endif
//sets up APN and IP protocol for external PDP context
retcode = setup_context_and_credentials();
if (retcode != NSAPI_ERROR_OK) {
return retcode;
}
if (!success) {
shutdown_at_parser();
return NSAPI_ERROR_NO_CONNECTION;
}
initialized = true;
did_init = true;
} else {
/* If we were already initialized, we expect to receive NO_CARRIER response
* from the modem as we were kicked out of Data mode */
_at->recv("NO CARRIER");
success = _at->send("AT") && _at->recv("OK");
}
/* Attempt to enter data mode */
success = set_atd(_at); //enter into Data mode with the modem
if (!success) {
power_down();
initialized = false;
/* if we were previously initialized , i.e., not in this particular attempt,
* we want to re-initialize */
if (!did_init) {
goto retry_init;
}
/* shutdown AT parser before notifying application of the failure */
shutdown_at_parser();
return NSAPI_ERROR_NO_CONNECTION;
}
/* This is the success case.
* Save RAM, discard AT Parser as we have entered Data mode. */
shutdown_at_parser();
/* We now want hangup (e.g., DCD) detection if available */
enable_hup(true);
/* Initialize PPP
* mbed_ppp_init() is a blocking call, it will block until
* connected, or timeout after 30 seconds*/
retcode = nsapi_ppp_connect(_fh, _connection_status_cb, _uname, _pwd, _stack);
if (retcode == NSAPI_ERROR_OK) {
dev_info.ppp_connection_up = true;
}
}while(!dev_info.ppp_connection_up && apn_config && *apn_config);
return retcode;
}
int switch_boot_mode(void)
{
unsigned long hold_time = 50000, polling_time = 10000, tmp;
unsigned long upgrade_step;
clrbits_le32(P_PREG_PAD_GPIO2_EN_N, (1<<25)); // VCC5V_EN GPIOD_9
setbits_le32(P_PREG_PAD_GPIO2_O, (1<<25));
get_osd_size();
#ifdef CONFIG_PMU_ACT8942
act8942_init(&act8942_pdata);
//act8942_dump();
#endif
upgrade_step = simple_strtoul (getenv ("upgrade_step"), NULL, 16);
printf("upgrade_step = %d\n", upgrade_step);
saradc_enable();
#ifdef ENABLE_FONT_RESOURCE
RegisterFont(DEFAULT_FONT);
#endif
aml_autoscript();
powerkey_hold(0);
u32 reboot_mode_current = reboot_mode;//cvt
reboot_mode_clear();//cvt
#ifdef CONFIG_AML_TINY_USBTOOL //cvt
extern int usb_boot(int clk_cfg, int time_out);
if((reboot_mode_current == MESON_USB_BURNER_REBOOT) || get_burner_key())
{
usb_boot(1, 200000);
}
#endif
if(upgrade_step == 2)
{
switch(reboot_mode_current)
{
case AMLOGIC_NORMAL_BOOT:
{
printf("AMLOGIC_NORMAL_BOOT...\n");
power_up();
logo_display();
return 1;
}
case AMLOGIC_FACTORY_RESET_REBOOT:
{
printf("AMLOGIC_FACTORY_RESET_REBOOT...\n");
power_up();
logo_display();
run_command ("nand read ${recovery_name} ${loadaddr} 0 ${recovery_size}", 0);
run_command ("bootm", 0);
break;
}
case AMLOGIC_UPDATE_REBOOT:
{
printf("AMLOGIC_UPDATE_REBOOT...\n");
power_up();
logo_display();
run_command ("set upgrade_step 0", 0);
run_command ("save", 0);
upgrade_step = 0;
break;
}
default:
{
printf("AMLOGIC_CHARGING_REBOOT...\n");
if(is_ac_connected)
{
power_up();
#ifdef CONFIG_BATTERY_CHARGING
battery_charging();
#endif
logo_display();
}
else
{
powerkey_hold(0);
#ifdef CONFIG_BATTERY_CHARGING
if(get_powerkey_hold_count())
{
logo_display();
if(get_battery_percentage() < 10)
{
power_low_display();
sdelay(2);
power_down();
printf("Low Power!!!\nPower Down!\n");
hang();
}
logo_display();
power_up();
printf("Power Up!\n");
}
#else
if(powerkey_hold(1000))
{
logo_display();
power_up();
printf("Power Up!\n");
}
#endif
else
{
power_down();
printf("Power Down!\n");
hang();
}
}
break;
}
}
}
else
{
power_up();
printf("Upgrade step %d...\n", upgrade_step);
}
if(upgrade_step == 0)
{
display_messge("upgrade step 1! Don't Power Off!");
if(upgrade_bootloader())
{
run_command ("set upgrade_step 1", 0);
run_command ("save", 0);
run_command ("reset", 0);
hang();
}
else
{
printf("### ERROR: u-boot write failed!!!\n");
return -1;
}
}
else if(upgrade_step == 1)
{
display_messge("upgrade step 2! Don't Power Off!");
run_command ("set upgrade_step 2", 0);
run_command ("save", 0);
into_recovery();
}
//added by Elvis for added fool idle
get_key();
get_key();
while(hold_time > 0)
{
udelay(polling_time);
tmp = get_key();
printf("get_key(): %d\n", tmp);
if(!tmp) break;
hold_time -= polling_time;
}
if(hold_time > 0)
{
printf("Normal Start...\n");
char *recovery_command;
recovery_command = getenv("recovery_command");
if (strcmp(recovery_command, "--usb_burning") == 0){
run_command ("set recovery_command", 0);
}
run_command ("save", 0);
return 1;
}
else
{
display_messge("upgrading... please wait");
if(upgrade_bootloader())
{
run_command ("set upgrade_step 1", 0);
run_command ("save", 0);
run_command ("reset", 0);
hang();
}
run_command ("set upgrade_step 2", 0);
run_command ("save", 0);
into_recovery();
}
return 0;
}
