void coopth_detach(void)
{
struct coopth_thrdata_t *thdata;
assert(_coopth_is_in_thread());
thdata = co_get_data(co_current());
ensure_single(thdata);
if (!thdata->attached)
return;
switch_state(COOPTH_DETACH);
}
/* for some time coopth_leave() was implemented on top of coopth_detach().
* This appeared not the best implementation. In particular, the commit
* 551371689 was needed to make leaving operation atomic, but this is
* not needed for detached threads at all. While the detached threads
* has a separate entry point (via coopth_run()), the left thread must
* not have a separate entry point. So it appeared better to return the
* special type "left" threads.
* Additionally the leave operation now calls the post handler immediately. */
void coopth_leave(void)
{
struct coopth_thrdata_t *thdata;
if (!_coopth_is_in_thread_nowarn())
return;
thdata = co_get_data(co_current());
ensure_single(thdata);
if (thdata->left)
return;
switch_state(COOPTH_LEAVE);
}
int main(void)
{
setlocale(LC_CTYPE, "");
const char *config_file = "avm-motion-trigger.conf";
struct config c = get_config(config_file);
char *session_id = session_start(c.avm.hostname, c.avm.username,
c.avm.password);
size_t max_ains = 32;
char* ains[max_ains];
int found = switches_list(c.avm.hostname, session_id, ains, max_ains);
if (0 == found) {
printf(" * No switches found!\n");
} else {
for(short i = 0; i < found; i++) {
char *name = switch_name(c.avm.hostname, session_id, ains[i]);
printf(" * Found: %s\n", ains[i]);
printf(" * Name: %s\n", name);
if (SWITCH_PRESENT == switch_present(c.avm.hostname,
session_id, ains[i])) {
printf(" * Present: yes (connected)\n");
} else {
printf(" * Present: no (not connected)\n");
}
if (SWITCH_STATE_ON == switch_state(c.avm.hostname,
session_id, ains[i])) {
printf(" * State: on\n");
} else {
printf(" * State: off\n");
}
if (i < found-1) {
printf("\n");
}
free(name);
free(ains[i]);
}
}
session_end(c.avm.hostname, session_id);
free_config(&c);
return 0;
}
void intan_setup(void){
int setup_reg[13] = {1,2,4,8,9,10,11,12,13,14,15,16,17};
int setup_data[13] = {ADC_bias & 0x3F, MUX_bias, ADC_twoscomp, RH1_DAC1,RH1_DAC2,RH2_DAC1,RH2_DAC2,RL_DAC1,((RL_DAC3 << 6) + RL_DAC2),0xFF,0xFF,0xFF,0xFF};
for (int i =0; i<13; i++){
while (!m_transfer_completed){};
m_transfer_completed = false;
intan_write(m_tx_data_spi, m_rx_data_spi,setup_reg[i],setup_data[i]);
switch_state();
//nrf_delay_ms(DELAY_MS);
}
for (int j =0; j<2; j++){
while (!m_transfer_completed){};
m_transfer_completed = false;
intan_dummy(m_tx_data_spi, m_rx_data_spi);
switch_state();
//nrf_delay_ms(DELAY_MS);
}
}
int main(int argc, char *argv[]) {
printf("\n Starting State Test ...\n");
LOG_FILE_PATH = "../../extras/log.txt";
create_window();
switch_state(EXIT);
assert(game_state == EXIT);
start_game();
printf(" Window State passed...\n");
return 0;
}
void d7asp_signal_transaction_response_period_elapsed()
{
if(d7asp_state == D7ASP_STATE_MASTER)
on_request_completed();
else if((d7asp_state == D7ASP_STATE_SLAVE) ||
(d7asp_state == D7ASP_STATE_SLAVE_PENDING_MASTER))
{
if (current_response_packet)
{
DPRINT("Discard the response since the response period is expired");
packet_queue_free_packet(current_response_packet);
current_response_packet = NULL;
}
if (d7asp_state == D7ASP_STATE_SLAVE)
switch_state(D7ASP_STATE_IDLE);
else if(d7asp_state == D7ASP_STATE_SLAVE_PENDING_MASTER)
{
switch_state(D7ASP_STATE_MASTER);
DPRINT("Schedule task to flush the fifo");
sched_post_task(&flush_fifos);
}
}
}
DIRECTION::DIRECTION(){
//Set wheels speed
diff_wheels.setSpeed(0,0);
diff_wheels.enableEncoders(TIME_STEP);
diff_wheels.setEncoders(0,0);
orientation = DIR_NORTH;
old_orientation = DIR_NORTH;
ve_node_to_node.reset(diff_wheels);
int i;
char ds_name[] = "ps0";
char led_name[] = "led0";
counttime = 0;
x = 0;
y = 0;
//Init distance sensors
dist_sensors = new DistanceSensor* [NR_DIST_SENSORS];
led = new LED * [NB_LEDS];
for(i = 0; i < NR_DIST_SENSORS; i++){
dist_sensors[i] = new DistanceSensor(ds_name);
dist_sensors[i]->enable(TIME_STEP);
ds_name[2]++;
}
for(i = 0; i < NB_LEDS; i++){
led[i] = new LED(led_name);
led[i]->set(0);
led_name[3]++;
}
wh_delta = 0;
wr_delta = 0;
ds_left_10 = ds_left_45 = ds_left_90 = 0;
ds_right_10 = ds_right_45 = ds_right_90 = 0;
total_delta_left = total_delta_right = 0;
switch_state(STATE_FOLLOW_CORIDOR); //STATE_FOLLOW_CORIDOR;
direction_rotate = DIR_CLK;
angle_rotate = ANGLE_90;
}
void execute( const aosl::Change& change, aosl::Object& object, bool reverse )
{
UTILCPP_ASSERT( std::find( std::begin( change.object() ), std::end( change.object() ), object.id() ) == std::end( change.object() )
, "Tried to execute a change \"" << typeid(change).name()
<< "\" that should be executed with object(s) \"" << change.object()
<< "\" but is executed with object \"" << object.id() << "\"!"
);
typedef std::map< std::type_index, ChangeExecutor > ChangeFunctionMap;
static const ChangeFunctionMap CHANGE_FUNCTION_MAP = []() -> ChangeFunctionMap
{
ChangeFunctionMap change_function_map;
change_function_map[ typeid(aosl::Change_activate) ] = ChangeExecutor(
[]( aosl::Object& object ) { activate( object ); }
, []( aosl::Object& object ) { reverse_activate( object ); }
);
change_function_map[ typeid(aosl::Change_deactivate) ] = ChangeExecutor(
[]( aosl::Object& object ) { deactivate( object ); }
, []( aosl::Object& object ) { reverse_activate( object ); }
);
change_function_map[ typeid(aosl::Change_switch) ] = ChangeExecutor(
[]( aosl::Object& object ) { switch_state( object ); }
, []( aosl::Object& object ) { reverse_switch_state( object ); }
);
// TODO : add transformation change here
return change_function_map;
}();
const auto& change_typeid = typeid(change);
auto func_it = CHANGE_FUNCTION_MAP.find( change_typeid );
if( func_it != CHANGE_FUNCTION_MAP.end() )
{
if( reverse )
func_it->second.reverse( object );
else
func_it->second.action( object );
}
else
{
UTILCPP_LOG_ERROR << "Algorithm for Change of type \"" << change_typeid.name() << "\" not found! ";
}
}
static uint8
accel_read(uint8 addr)
{
struct i2c_msg msgs[2];
uint8 byte;
byte = addr;
msgs[0].addr = msgs[1].addr = ACCEL_ADDR;
msgs[0].length = msgs[1].length = sizeof(byte);
msgs[0].data = msgs[1].data = &byte;
msgs[0].flags = 0;
msgs[1].flags = I2C_MSG_READ;
if (switch_state(&back_switch))
i2c_master_xfer(i2c, msgs, 2, 1);
return byte;
}
/** @brief Function for main application entry.
*/
int main(void)
{
// Setup bsp module.
bsp_configuration();
for (;; )
{
#if defined(SPI_MASTER_0_ENABLE) || defined(SPI_MASTER_1_ENABLE)
if (m_transfer_completed)
{
m_transfer_completed = false;
switch_state();
}
#endif // defined(SPI_MASTER_0_ENABLE) || defined(SPI_MASTER_1_ENABLE)
}
}
void sample_data(void)
{
if(m_transfer_completed)
{
m_transfer_completed = false;
intan_convert(m_tx_data_spi, m_rx_data_spi,intan_convert_channel);
intan_convert_channel++;
switch_state();
}
// Previous transfer still in progress. Data lost
else
{
while(app_uart_put(69) != NRF_SUCCESS);
}
}
void CTeleWhirlwindObject:: keep ()
{
CPhysicsShell* p = get_object() ->PPhysicsShell();
if(!p||!p->isActive())
return;
else
{
p->SetAirResistance(0.f,0.f);
p->set_ApplyByGravity(FALSE);
}
u16 element_number = p ->get_ElementsNumber();
Fvector center = m_telekinesis ->Center();
CPhysicsElement* maxE=p->get_ElementByStoreOrder(0);
for(u16 element=0;element<element_number;++element)
{
CPhysicsElement* E= p->get_ElementByStoreOrder(element);
if(maxE->getMass()<E->getMass())maxE=E;
Fvector dir;dir.sub(center,E->mass_Center());
dir.normalize_safe();
Fvector vel;
E->get_LinearVel(vel);
float force=dir.dotproduct(vel)*E->getMass()/2.f;
if(force<0.f)
{
dir.mul(force);
}
}
maxE->setTorque(Fvector().set(0,500.f,0));
Fvector dist;dist.sub(center,maxE->mass_Center());
if(dist.magnitude()>m_telekinesis->keep_radius()*1.5f)
{
p->setTorque(Fvector().set(0,0,0));
p->setForce(Fvector().set(0,0,0));
p->set_LinearVel(Fvector().set(0,0,0));
p->set_AngularVel(Fvector().set(0,0,0));
p->set_ApplyByGravity(TRUE);
switch_state(TS_Raise);
}
}
/**
* Checks position of "Sleep" (standby) switch at the back of the device and
* take action if switch position changed.
*/
void Controller::check_sleep_switch() {
#ifndef NEVERSLEEP
bool sstate = switch_state();
if(sstate != m_last_switch_state) {
m_last_switch_state = sstate;
if(sstate == false) {
if(m_alarm_log && (!m_sleeping)) { m_sleeping=true; display_powerdown(); } else {
m_sleeping=true;
power_standby();
}
}
if(sstate == true) {
m_powerup = true;
}
}
#endif
if(m_powerup == true) {
buzzer_nonblocking_buzz(0.5);
display_powerup();
m_gui->set_sleeping(false);
m_gui->redraw();
m_sleeping=false;
m_powerup =false;
const char *devicetag = flashstorage_keyval_get("DEVICETAG");
char revtext[10];
sprintf(revtext,"VERSION: %s ",OS100VERSION);
display_splashscreen(devicetag,revtext);
delay_us(3000000);
display_clear(0);
}
if(m_sleeping) {
// go back to sleep.
if((!rtc_alarmed()) && (!m_alarm_log)) {
power_standby();
}
return;
}
}
/* for some time coopth_leave() was implemented on top of coopth_detach().
* This appeared not the best implementation. In particular, the commit
* 551371689 was needed to make leaving operation atomic, but this is
* not needed for detached threads at all. While the detached threads
* has a separate entry point (via coopth_run()), the left thread must
* not have a separate entry point. So it appeared better to return the
* special type "left" threads.
* Additionally the leave operation now calls the post handler immediately,
* and it should be called from the context of the main thread. This is
* the reason why coopth_leave() for detached thread cannot be a no-op. */
void coopth_leave(void)
{
struct coopth_thrdata_t *thdata;
if (!_coopth_is_in_thread_nowarn())
return;
thdata = co_get_data(co_current());
ensure_single(thdata);
if (thdata->left)
return;
/* leaving detached thread should be atomic even wrt other detached
* threads. This is needed so that DPMI cannot run concurrently with
* leavedos().
* for joinable threads leaving should be atomic only wrt DOS code,
* but, because of an optimization loop in run_vm86(), it is actually
* also atomic wrt detached threads. */
if (!thdata->attached)
thdata->atomic_switch = 1;
switch_state(COOPTH_LEAVE);
}
static int
accel_write(uint8 addr, uint8 value)
{
struct i2c_msg msg;
uint8 bytes[2];
int result;
bytes[0] = addr;
bytes[1] = value;
msg.addr = ACCEL_ADDR;
msg.flags = 0;
msg.length = sizeof(bytes);
msg.xferred = 0;
msg.data = bytes;
if (switch_state(&back_switch))
result = i2c_master_xfer(i2c, &msg, 1, 1);
return result;
}
void CTeleWhirlwindObject:: release ()
{
if (!object ||object->getDestroy() ||!object->m_pPhysicsShell || !object->m_pPhysicsShell->isActive()) return;
Fvector dir_inv;
dir_inv.sub(object->Position(),m_telekinesis->Center());
float magnitude = dir_inv.magnitude();
// включить гравитацию
//Fvector zer;zer.set(0,0,0);
//object->m_pPhysicsShell->set_LinearVel(zer);
object->m_pPhysicsShell->set_ApplyByGravity(TRUE);
/////////////////////////////////////
float impulse=0.f;
if(magnitude>0.2f)
{
dir_inv.mul(1.f/magnitude);
impulse=throw_power/magnitude/magnitude;
}
else
{
dir_inv.random_dir();
impulse=throw_power*100.f;
}
/////////////////////////////////////////////////
bool b_destroyed=false;
if(magnitude<2.f*object->Radius())
{
b_destroyed=destroy_object(dir_inv,throw_power*100.f);
}
if(!b_destroyed)object->m_pPhysicsShell->applyImpulse(dir_inv,impulse);
switch_state(TS_None);
}
static void send_message() {
// while(1) {
SSN = LOW;
setDisplayStart(0);
setCursor(6, 0);
printf("Transmitting!");
radio_send_packet(compose_buffer_);
setCursor(7, 0);
putchar('8');
SSN = HIGH;
while (radio_still_sending()) {
clock_delayms(400);
SSN = LOW;
putchar('=');
SSN = HIGH;
}
radio_listen(); // go back into receive mode
clock_delayms(500);
SSN = LOW;
putchar('D');
SSN = HIGH;
clock_delayms(500);
// }
/* Reset the compose view. */
state_ = COMPOSE_STATE_WRITING;
compose_new_message();
/* Switch back to the inbox view. */
switch_state(STATE_VIEW);
}
// For testing; read out intan id
static void spi_intan_id_handler(void *pvParameter)
{
static int company_title = 0;
UNUSED_PARAMETER(pvParameter);
if(m_transfer_completed)
{
//if(intan_convert_channel < NUM_CHANNELS) {
// printf("%d\r\n", intan_convert_channel);
//}
m_transfer_completed = false;
intan_read(m_tx_data_spi, m_rx_data_spi,company_title + 40);
company_title++;
company_title = company_title % 5;
switch_state();
}
// Previous transfer still in progress. Data lost
else
{
while(app_uart_put(69) != NRF_SUCCESS);
}
}
uint8
accel_read_state(int *x, int *y, int *z)
{
struct i2c_msg msgs[2];
signed char values[6];
uint8 addr = 0x00; /* 10-bits read value */
uint8 result = 0;
accel_wakeup();
while (!accel_ready())
delay_us(1000);
msgs[0].addr = ACCEL_ADDR;
msgs[0].length = sizeof(byte);
msgs[0].data = &addr;
msgs[0].flags = 0;
msgs[1].addr = ACCEL_ADDR;
msgs[1].length = sizeof(values);
msgs[1].data = (uint8 *)values;
msgs[1].flags = I2C_MSG_READ;
if (switch_state(&back_switch))
result = i2c_master_xfer(i2c, msgs, 2, 1);
if (x)
*x = (values[1]<<2) | (values[0]);
if (y)
*y = (values[3]<<2) | (values[2]);
if (z)
*z = (values[5]<<2) | (values[4]);
accel_sleep();
return result;
}
void
initialize()
{
// OpenGL nonchanging settings
glEnable(GL_TEXTURE_2D);
glEnableVertexAttribArray(0);
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
glClearDepth(1.0f);
glEnable(GL_CULL_FACE);
Renderer::initialize();
world.reset(new WorldDummy());
paused.reset(new Paused());
MiniMap::initialize();
Projectile::initialize();
ParticleSystem::initialize();
//Menu::initialize();
initialize_meridians();
switch_state(WORLD);
}
static bool state_machine_source(int source_id, enum state_function *state)
{
static dsp16_t volume;
static unsigned int frequency;
struct signal_source *source = NULL;
if (source_id == 1)
source = &signal1_generator;
else if (source_id == 2)
source = &signal2_generator;
switch (*state)
{
case STATE_FCT_IDLE:
if (source_id == 1)
{
if (new_state_fct)
{
gui_set_selection(GUI_SOURCE1_ID);
gui_text_print(GUI_COMMENT_ID, TEXT_SOURCE1);
}
else
{
if (controller_wheel_right(2))
switch_state(STATE_SOURCE2);
return false;
}
}
else if (source_id == 2)
{
if (new_state_fct)
{
gui_set_selection(GUI_SOURCE2_ID);
gui_text_print(GUI_COMMENT_ID, TEXT_SOURCE2);
}
else
{
if (controller_wheel_left(2))
switch_state(STATE_SOURCE1);
else if (controller_wheel_right(2))
switch_state(STATE_INPUT);
return false;
}
}
break;
// Amplitude
case STATE_FCT_FUNCTION1:
volume = signal_source_get_volume(source);
if (controller_wheel_right(1) && volume < DSP16_Q(1.))
{
if (volume < DSP16_Q(1.) - DSP16_Q(1./16))
volume += DSP16_Q(1./16);
else
volume = DSP16_Q(1.);
new_state_fct = true;
}
else if (controller_wheel_left(1))
{
if (volume > DSP16_Q(1./16))
volume -= DSP16_Q(1./16);
else
volume = 0;
new_state_fct = true;
}
if (new_state_fct)
{
signal_source_set_volume(source, volume);
gui_text_printf(GUI_COMMENT_ID, "Source%i - " TEXT_FUNC1 "\nAmplitude %f\n\n\n\n" TEXT_WHEEL, source_id, volume);
}
break;
// Frequency
case STATE_FCT_FUNCTION2:
frequency = signal_source_get_freq(source);
if (controller_wheel_right(1) && frequency < 10000)
{
frequency *= 1.1;
new_state_fct = true;
}
else if (controller_wheel_left(1) && frequency > 100)
{
frequency *= 0.9;
new_state_fct = true;
}
if (new_state_fct)
{
signal_source_set_freq(source, frequency);
gui_text_printf(GUI_COMMENT_ID, "Source%i - " TEXT_FUNC2 "\nFrequency %iHz\n\n\n\n" TEXT_WHEEL, source_id, frequency);
}
break;
case STATE_FCT_FUNCTION3:
break;
// Zoom
case STATE_FCT_ZOOM:
if (new_state_fct)
{
zoom_view = true;
if (source_id == 1)
zoom_view_id = GUI_SOURCE1_ID;
else if (source_id == 2)
zoom_view_id = GUI_SOURCE2_ID;
controller_reset();
}
break;
}
return true;
}
void coopth_yield(void)
{
assert(_coopth_is_in_thread());
switch_state(COOPTH_YIELD);
check_cancel();
}
// TODO we assume a fifo contains only ALP commands, but according to spec this can be any kind of "Request"
// we will see later what this means. For instance how to add a request which starts D7AAdvP etc
d7asp_queue_result_t d7asp_queue_alp_actions(d7asp_master_session_t* session, uint8_t* alp_payload_buffer, uint8_t alp_payload_length)
{
DPRINT("Queuing ALP actions");
// TODO can be called in all session states?
assert(session == ¤t_master_session); // TODO tmp
assert(session->request_buffer_tail_idx + alp_payload_length < MODULE_D7AP_FIFO_COMMAND_BUFFER_SIZE);
assert(session->next_request_id < MODULE_D7AP_FIFO_MAX_REQUESTS_COUNT); // TODO do not assert but let upper layer handle this
single_request_retry_limit = 3; // TODO read from SEL config file
// add request to buffer
// TODO request can contain 1 or more ALP commands, find a way to group commands in requests instead of dumping all requests in one buffer
uint8_t request_id = session->next_request_id;
session->requests_indices[request_id] = session->request_buffer_tail_idx;
session->requests_lengths[request_id] = alp_payload_length;
memcpy(session->request_buffer + session->request_buffer_tail_idx, alp_payload_buffer, alp_payload_length);
session->request_buffer_tail_idx += alp_payload_length + 1;
session->next_request_id++;
// TODO for master only set to pending when asked by upper layer (ie new function call)
if(d7asp_state == D7ASP_STATE_IDLE)
switch_state(D7ASP_STATE_MASTER);
else if(d7asp_state == D7ASP_STATE_SLAVE)
switch_state(D7ASP_STATE_SLAVE_PENDING_MASTER);
return (d7asp_queue_result_t){ .fifo_token = session->token, .request_id = request_id };
}
bool d7asp_process_received_packet(packet_t* packet, bool extension)
{
hw_watchdog_feed(); // TODO do here?
d7asp_result_t result = {
.channel = packet->hw_radio_packet.rx_meta.rx_cfg.channel_id,
.rx_level = - packet->hw_radio_packet.rx_meta.rssi,
.link_budget = (packet->dll_header.control_eirp_index + 32) - packet->hw_radio_packet.rx_meta.rssi,
.target_rx_level = 80, // TODO not implemented yet, use default for now
.status = {
.ucast = 0, // TODO
.nls = packet->d7anp_ctrl.origin_addressee_ctrl_nls_enabled,
.retry = false, // TODO
.missed = false, // TODO
},
.response_to = packet->d7atp_tc,
.addressee = packet->d7anp_addressee
// .fifo_token and .seqnr filled below
};
if(d7asp_state == D7ASP_STATE_MASTER)
{
assert(packet->d7atp_dialog_id == current_master_session.token);
assert(packet->d7atp_transaction_id == current_request_id);
// received ack
DPRINT("Received ACK");
if(current_master_session.config.qos.qos_resp_mode != SESSION_RESP_MODE_NO
&& current_master_session.config.qos.qos_resp_mode != SESSION_RESP_MODE_NO_RPT)
{
// for SESSION_RESP_MODE_NO and SESSION_RESP_MODE_NO_RPT the request was already marked as done
// upon successfull CSMA insertion. We don't care about response in these cases.
result.fifo_token = current_master_session.token;
result.seqnr = current_request_id;
bitmap_set(current_master_session.success_bitmap, current_request_id);
mark_current_request_done();
assert(packet != current_request_packet);
}
alp_d7asp_request_completed(result, packet->payload, packet->payload_length);
// if(d7asp_init_args != NULL && d7asp_init_args->d7asp_fifo_request_completed_cb != NULL)
// d7asp_init_args->d7asp_fifo_request_completed_cb(result, packet->payload, packet->payload_length); // TODO ALP should notify app if needed, refactor
packet_queue_free_packet(packet); // ACK can be cleaned
// switch to the state slave when the D7ATP Dialog Extension Procedure is initiated and all request are handled
if ((extension) && (current_request_id == current_master_session.next_request_id - 1))
{
DPRINT("Dialog Extension Procedure is initiated, mark the FIFO flush"
" completed before switching to a responder state");
alp_d7asp_fifo_flush_completed(current_master_session.token, current_master_session.progress_bitmap,
current_master_session.success_bitmap, REQUESTS_BITMAP_BYTE_COUNT);
current_master_session.state = D7ASP_MASTER_SESSION_IDLE;
d7atp_signal_dialog_termination();
switch_state(D7ASP_STATE_SLAVE);
}
return true;
}
else if(d7asp_state == D7ASP_STATE_IDLE || d7asp_state == D7ASP_STATE_SLAVE)
{
// received a request, start slave session, process and respond
if(d7asp_state == D7ASP_STATE_IDLE)
switch_state(D7ASP_STATE_SLAVE); // don't switch when already in slave state
result.fifo_token = packet->d7atp_dialog_id;
result.seqnr = packet->d7atp_transaction_id;
// TODO move to ALP
if(packet->payload_length > 0)
{
if(alp_get_operation(packet->payload) == ALP_OP_RETURN_FILE_DATA)
{
// received unsollicited data, notify appl
DPRINT("Received unsollicited data");
if(d7asp_init_args != NULL && d7asp_init_args->d7asp_received_unsollicited_data_cb != NULL)
d7asp_init_args->d7asp_received_unsollicited_data_cb(result, packet->payload, packet->payload_length);
packet->payload_length = 0; // no response payload
}
else
{
// build response, we will reuse the same packet for this
// we will first try to process the command against the local FS
// if the FS handler cannot process this, and a status response is requested, a status operand will be present in the response payload
bool handled = alp_process_command(packet->payload, packet->payload_length, packet->payload, &packet->payload_length, ALP_CMD_ORIGIN_D7ASP);
// ... and if not handled we'll give the application a chance to handle this by returning an ALP response.
// if the application fails to handle the request as well the ALP status operand supplied by alp_process_command_fs_itf() will be transmitted (if requested)
if(!handled)
{
DPRINT("ALP command could not be processed by local FS");
if(d7asp_init_args != NULL && d7asp_init_args->d7asp_received_unhandled_alp_command_cb != NULL)
{
DPRINT("ALP command passed to application for processing");
d7asp_init_args->d7asp_received_unhandled_alp_command_cb(packet->payload, packet->payload_length, packet->payload, &packet->payload_length);
}
}
}
}
// TODO notify upper layer?
// execute slave transaction
if(packet->payload_length == 0 && !packet->d7atp_ctrl.ctrl_is_ack_requested)
goto discard_request; // no need to respond, clean up
DPRINT("Sending response");
current_response_packet = packet;
/*
* activate the dialog extension procedure in the unicast response if the dialog is terminated
* and a master session is pending
*/
if((packet->dll_header.control_target_address_set) && (packet->d7atp_ctrl.ctrl_is_stop)
&& (d7asp_state == D7ASP_STATE_SLAVE_PENDING_MASTER))
{
packet->d7atp_ctrl.ctrl_is_start = true;
// TODO set packet->d7anp_listen_timeout according the time remaining in the current transaction
// + the maximum time to send the first request of the pending session.
}
else
packet->d7atp_ctrl.ctrl_is_start = 0;
d7atp_respond_dialog(packet);
return true;
}
else
assert(false);
discard_request:
packet_queue_free_packet(packet);
return false;
}
static void earjack_detect_func( struct work_struct *test_earjack)
{
int err;
dbg_func_in();
dbg("currnet earjack->type: ");
switch(earjack->type){
case EARJACK_STATE_OFF :
{
dbg_without_label("EARJACK_STATE_OFF\n");
if(EARJACK_INSERTED){
earjack->type = EARJACK_STATE_CHECK;
#if defined(CONFIG_MIC_BIAS_1_8V)
#else
err=regulator_is_enabled(hs_jack_l8);
if(err<=0) err = regulator_enable(hs_jack_l8);
#endif //defined(CONFIG_MACH_MSM8X60_PRESTO)
pm8058_mpp_config_digital_in(XOADC_MPP_3,PM8058_MPP_DIG_LEVEL_S3, PM_MPP_DIN_TO_INT);
schedule_delayed_work(&earjack_work, 5); //50ms
// return without enable IRQ, wake_unlock.
return;
}
break;
}
case EARJACK_STATE_CHECK :
{
dbg_without_label("EARJACK_STATE_CHECK\n");
if(EARJACK_INSERTED)
{
// 3pole
if(!is_4pole_earjack()){
dbg("3pole headset inserted.\n");
earjack->type= EARJACK_STATE_ON_3POLE_CHECK;
earjack->mic_on = 0;
earjack->hs_on = 1;
input_report_switch(earjack->ipdev, SW_HEADPHONE_INSERT,earjack->hs_on);
switch_set_state(&earjack->sdev, switch_state());
schedule_delayed_work(&earjack_work, 60); // check if 4pole 600ms
// return without enable IRQ, wake_unlock.
return;
}
//4pole
else {
dbg("4pole headset inserted.\n");
earjack->type= EARJACK_STATE_ON;
err=request_threaded_irq(gpio_to_irq(REMOTEKEY_DET),NULL,Remotekey_Det_handler,IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING, "remote_det-irq", earjack);
if(err)
dbg("request_threaded_irq failed\n");
earjack->mic_on = 1;
earjack->hs_on = 1;
input_report_switch(earjack->ipdev, SW_MICROPHONE_INSERT,earjack->mic_on); //TODO: NO USE?
input_report_switch(earjack->ipdev, SW_HEADPHONE_INSERT,earjack->hs_on);
switch_set_state(&earjack->sdev, switch_state());
}
}
else // if EARJACK_RELEASED
{
earjack->type = EARJACK_STATE_OFF;
dbg("earjack_type: -> EARJACK_STATE_OFF");
}
break; // case EARJACK_STATE_CHECK
}
case EARJACK_STATE_ON_3POLE_CHECK :
{
// CHECKING IF 4POLE EARJACK IS INSERTIND?
dbg_without_label("EARJACK_STATE_ON_3POLE_CHECK\n");
if(EARJACK_INSERTED){
earjack->type= EARJACK_STATE_ON;
if(!is_4pole_earjack()){
dbg("3pole earjack insert.\n");
#if defined(CONFIG_MIC_BIAS_1_8V)
#else
err=regulator_is_enabled(hs_jack_l8);
dbg("regulator_is_enabled(hs_jack_l8) value => %d\n",err);
if(err>0) regulator_disable(hs_jack_l8);
#endif //defined(CONFIG_MACH_MSM8X60_PRESTO)
}
else {
dbg("4pole earjack insert.\n");
earjack->mic_on =1;
input_report_switch(earjack->ipdev, SW_MICROPHONE_INSERT,earjack->mic_on);
switch_set_state(&earjack->sdev, switch_state());
err=request_threaded_irq(gpio_to_irq(REMOTEKEY_DET),NULL,Remotekey_Det_handler,IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING, "remote_det-irq", earjack);
if(err) dbg("request_threaded_irq failed\n");
}
}
else{
#if defined(CONFIG_MIC_BIAS_1_8V)
#else
err=regulator_is_enabled(hs_jack_l8);
dbg("regulator_is_enabled(hs_jack_l8) value => %d\n",err);
if(err>0) regulator_disable(hs_jack_l8);
#endif// defined(CONFIG_MACH_MSM8X60_PRESTO)
earjack->type = EARJACK_STATE_OFF;
earjack->hs_on=0;
input_report_switch(earjack->ipdev, SW_HEADPHONE_INSERT,earjack->hs_on);
switch_set_state(&earjack->sdev, switch_state());
}
break;
}
case EARJACK_STATE_ON:
{
dbg_without_label("EARJACK_STATE_ON\n");
if(EARJACK_RELEASED){
earjack->type = EARJACK_STATE_OFF;
// if 4pole
if (earjack->mic_on) {
earjack->mic_on = 0;
input_report_switch(earjack->ipdev, SW_MICROPHONE_INSERT,earjack->mic_on);
// free remote key irq and turn off mic power.
free_irq(gpio_to_irq(REMOTEKEY_DET), earjack);
#if defined(CONFIG_MIC_BIAS_1_8V)
#else
err=regulator_is_enabled(hs_jack_l8);
dbg("regulator_is_enabled(hs_jack_l8) value => %d\n",err);
if(err>0){
regulator_disable(hs_jack_l8);
}
#endif //defined(CONFIG_MACH_MSM8X60_PRESTO)
// release remote key if pressed
if(earjack->remotekey_pressed){
earjack->remotekey_pressed = 0;
if (earjack->remotekey_index != 0)
input_report_key(earjack->ipdev, remotekey_type[earjack->remotekey_index].key_index, earjack->remotekey_pressed);
dbg("remote key: %s : %d->%d \n", remotekey_type[earjack->remotekey_index].key_name, !earjack->remotekey_pressed, earjack->remotekey_pressed);
input_sync(earjack->ipdev);
}
dbg("earjack_release \n");
}
earjack->hs_on = 0;
input_report_switch(earjack->ipdev, SW_HEADPHONE_INSERT,earjack->hs_on);
switch_set_state(&earjack->sdev, switch_state());
}
break;
}
default :
dbg("earjack_detect_func default.\n");
}
enable_irq(gpio_to_irq(EARJACK_DET));
wake_unlock(&earjack_wake_lock);
dbg_func_out();
return;
}
// Sysfs
static ssize_t show_headset(struct device *dev, struct device_attribute *attr
, char *buf)
{
int conn_headset_type = switch_state();
return snprintf(buf, PAGE_SIZE, "%d\n", conn_headset_type);
}
void switch_state( aosl::Canvas& canvas, const aosl::Object_ref& object_ref )
{
get_check_act_object( canvas, object_ref, []( aosl::Object& obj ){ switch_state(obj); } );
}
static bool state_machine_idle(enum state_function *state)
{
uint32_t frame_rate_ms;
S8 volume;
const S8 volume_inc = (TPA6130_VOL_MAX - TPA6130_VOL_MIN) / 20;
switch (*state)
{
case STATE_FCT_IDLE:
if (new_state_fct)
{
gui_set_selection(GUI_NO_SEL);
gui_text_print(GUI_COMMENT_ID, TEXT_IDLE);
}
else
{
if (controller_wheel_right(1) || controller_wheel_left(1))
switch_state(STATE_SOURCE1);
return false;
}
break;
// Frame rate
case STATE_FCT_FUNCTION1:
frame_rate_ms = gui_get_update_fs();
if (controller_wheel_left(1) && frame_rate_ms < 1000)
{
if (!frame_rate_ms)
frame_rate_ms = 40;
else
frame_rate_ms = 1000 / ((1000 / frame_rate_ms) - 1);
new_state_fct = true;
}
else if (controller_wheel_right(1))
{
if (frame_rate_ms <= 40)
frame_rate_ms = 0;
else
frame_rate_ms = 1000 / ((1000 / frame_rate_ms) + 1);
new_state_fct = true;
}
if (new_state_fct)
{
gui_change_update_fs(frame_rate_ms);
if (frame_rate_ms)
{
gui_text_printf(GUI_COMMENT_ID, TEXT_FUNC1 "\n%i Frame(s)/s\n\n\n\n" TEXT_WHEEL, (1000 / frame_rate_ms));
}
else
gui_text_print(GUI_COMMENT_ID, TEXT_FUNC1 "\nMAX Frame(s)/s\n\n\n\n" TEXT_WHEEL);
}
break;
// Volume
case STATE_FCT_FUNCTION2:
volume = tpa6130_get_volume();
if (volume < TPA6130_VOL_MIN)
volume = TPA6130_VOL_MIN;
if (controller_wheel_right(1))
{
if (volume < TPA6130_VOL_MAX - volume_inc)
volume += volume_inc;
else
volume = TPA6130_VOL_MAX;
new_state_fct = true;
}
else if (controller_wheel_left(1))
{
if (volume > TPA6130_VOL_MIN + volume_inc)
volume -= volume_inc;
else
volume = TPA6130_VOL_MIN;
new_state_fct = true;
}
if (new_state_fct)
{
tpa6130_set_volume(volume);
gui_text_printf(GUI_COMMENT_ID, TEXT_FUNC2 "\nVolume %i%%\n\n\n\n" TEXT_WHEEL, ((int) volume * 100) / (TPA6130_VOL_MAX - TPA6130_VOL_MIN));
}
break;
/*
extern U8 tpa6130_get_volume(void);
extern void tpa6130_set_volume(U8 volume);
*/
case STATE_FCT_FUNCTION3:
if (new_state_fct)
gui_text_print(GUI_COMMENT_ID, TEXT_FUNC_NOT_IMPLEMENTED);
else if (controller_wheel_right(1) || controller_wheel_left(1)) {
switch_state(STATE_SOURCE1);
return false;
}
break;
case STATE_FCT_ZOOM:
break;
}
return true;
}
void CTeleWhirlwindObject:: raise (float step)
{
CPhysicsShell* p = get_object() ->PPhysicsShell();
if(!p||!p->isActive())
return;
else
{
p->SetAirResistance(0.f,0.f);
p->set_ApplyByGravity(TRUE);
}
u16 element_number = p ->get_ElementsNumber();
Fvector center = m_telekinesis ->Center();
CPhysicsElement* maxE=p->get_ElementByStoreOrder(0);
for(u16 element=0;element<element_number;++element)
{
float k=strength;//600.f;
float predict_v_eps=0.1f;
float mag_eps =.01f;
CPhysicsElement* E= p->get_ElementByStoreOrder(element);
if(maxE->getMass()<E->getMass()) maxE=E;
if (!E->isActive()) continue;
Fvector pos=E->mass_Center();
Fvector diff;
diff.sub(center,pos);
float mag=_sqrt(diff.x*diff.x+diff.z*diff.z);
Fvector lc;lc.set(center);
if(mag>1.f)
{
lc.y/=mag;
}
diff.sub(lc,pos);
mag=diff.magnitude();
float accel=k/mag/mag/mag;//*E->getMass()
Fvector dir;
if(mag<mag_eps)
{
accel=0.f;
//Fvector zer;zer.set(0,0,0);
//E->set_LinearVel(zer);
dir.random_dir();
}
else
{
dir.set(diff);dir.mul(1.f/mag);
}
Fvector vel;
E->get_LinearVel(vel);
float delta_v=accel*fixed_step;
Fvector delta_vel; delta_vel.set(dir);delta_vel.mul(delta_v);
Fvector predict_vel;predict_vel.add(vel,delta_vel);
Fvector delta_pos;delta_pos.set(predict_vel);delta_pos.mul(fixed_step);
Fvector predict_pos;predict_pos.add(pos,delta_pos);
Fvector predict_diff;predict_diff.sub(lc,predict_pos);
float predict_mag=predict_diff.magnitude();
float predict_v=predict_vel.magnitude();
Fvector force;force.set(dir);
if(predict_mag>mag && predict_vel.dotproduct(dir)>0.f && predict_v>predict_v_eps)
{
Fvector motion_dir;motion_dir.set(predict_vel);motion_dir.mul(1.f/predict_v);
float needed_d=diff.dotproduct(motion_dir);
Fvector needed_diff;needed_diff.set(motion_dir);needed_diff.mul(needed_d);
Fvector nearest_p;nearest_p.add(pos,needed_diff);//
Fvector needed_vel;needed_vel.set(needed_diff);needed_vel.mul(1.f/fixed_step);
force.sub(needed_vel,vel);
force.mul(E->getMass()/fixed_step);
}
else
{
force.mul(accel*E->getMass());
}
E->applyForce(force.x,force.y+get_object()->EffectiveGravity()*E->getMass(),force.z);
}
Fvector dist;dist.sub(center,maxE->mass_Center());
if(dist.magnitude()<m_telekinesis->keep_radius()&&b_destroyable)
{
p->setTorque(Fvector().set(0,0,0));
p->setForce(Fvector().set(0,0,0));
p->set_LinearVel(Fvector().set(0,0,0));
p->set_AngularVel(Fvector().set(0,0,0));
switch_state(TS_Keep);
}
}
uint8_t menu_dispatch(uint8_t tick) {
uint8_t result = MENURESULT_NOTHING;
if (tick && (state == STATE_ABOOT2)) {
aboot_anim();
}
if (inactivity) inactivity--;
if (!inactivity && state == STATE_FSEL) {
menu_init();
}
if (JOYSTICK != joy_status) {
joy_status = JOYSTICK;
inactivity = INACTIVITY;
switch (state) {
case STATE_ABOOT:
if (joy_status == 0) {
state = STATE_ABOOT2;
}
break;
case STATE_MENU:
if (joy_status & JOY_UP) menu_y = 0;
else if (joy_status & JOY_DN) menu_y = 2;
else menu_y = 1;
if (joy_status & JOY_LT) menu_x = 0;
else if (joy_status & JOY_RT) menu_x = 2;
else menu_x = 1;
draw_menu();
if (joy_status & JOY_FIRE) {
state = STATE_WAITBREAK;
menu_selected = menu_x+menu_y*3;
}
break;
case STATE_WAITBREAK:
if (!(joy_status & JOY_FIRE)) {
switch_state();
}
break;
case STATE_WAITBREAK2:
if (!(joy_status & JOY_FIRE)) {
fsel_getselected(ptrfile + 10);
ser_puts("Selected image: "); ser_puts(ptrfile); ser_nl();
menu_init();
result = MENURESULT_DISK;
}
break;
case STATE_ABOOT2:
if (!(joy_status & JOY_FIRE)) {
menu_init();
}
break;
case STATE_FSEL:
if (joy_status & JOY_UP) {
if (menu_y > 0) {
menu_y -= 1;
} else if (fsel_pagestart != 0) {
fsel_pagestart -= FSEL_PAGESIZE-1;
menu_y = FSEL_NLINES-1;
fsel_redraw = 1;
}
}
if (joy_status & JOY_DN) {
if (menu_y < FSEL_NLINES-1) {
menu_y += 1;
} else if (fsel_hasnextpage) {
menu_y = 0;
fsel_pagestart += FSEL_PAGESIZE-1;
fsel_redraw = 1;
}
}
if (joy_status & JOY_LT) {
menu_x = (menu_x - 1) % 2;
}
if (joy_status & JOY_RT) {
menu_x = (menu_x + 1) % 2;
}
if (fsel_redraw) {
fsel_redraw = 0;
draw_fsel();
}
fsel_showselection(0);
menu_selected = menu_y*2 + menu_x;
fsel_showselection(1);
if (joy_status & JOY_FIRE != 0) {
state = STATE_WAITBREAK2;
}
break;
}
}
return result;
}
static bool state_machine_signal(enum state_master id, enum state_function *state)
{
switch (*state)
{
case STATE_FCT_IDLE:
if (id == STATE_INPUT)
{
if (new_state_fct)
{
gui_set_selection(GUI_INPUT_ID);
gui_text_print(GUI_COMMENT_ID, TEXT_INPUT);
}
else
{
if (controller_wheel_right(2))
switch_state(STATE_FILTER);
else if (controller_wheel_left(2))
switch_state(STATE_SOURCE2);
return false;
}
}
else if (id == STATE_OUTPUT)
{
if (new_state_fct)
{
gui_set_selection(GUI_OUTPUT_ID);
gui_text_print(GUI_COMMENT_ID, TEXT_OUTPUT);
}
else
{
if (controller_wheel_left(2))
switch_state(STATE_FILTER);
return false;
}
}
break;
// FFT
case STATE_FCT_FUNCTION1:
if (new_state_fct)
{
if (id == STATE_INPUT)
{
input_fft_view = !input_fft_view;
if (input_fft_view)
gui_text_print(GUI_COMMENT_ID, "Input - " TEXT_FUNC1 "\nFrequency domain\n\n\n\n(Use " TEXT_FUNC1 ")");
else
gui_text_print(GUI_COMMENT_ID, "Input - " TEXT_FUNC1 "\nTemporal domain\n\n\n\n(Use " TEXT_FUNC1 ")");
}
else if (id == STATE_OUTPUT)
{
output_fft_view = !output_fft_view;
if (output_fft_view)
gui_text_print(GUI_COMMENT_ID, "Output - " TEXT_FUNC1 "\nFrequency domain\n\n\n\n(Use " TEXT_FUNC1 ")");
else
gui_text_print(GUI_COMMENT_ID, "Output - " TEXT_FUNC1 "\nTemporal domain\n\n\n\n(Use " TEXT_FUNC1 ")");
}
}
break;
case STATE_FCT_FUNCTION2:
case STATE_FCT_FUNCTION3:
if (new_state_fct)
gui_text_print(GUI_COMMENT_ID, TEXT_FUNC_NOT_IMPLEMENTED);
break;
// Zoom
case STATE_FCT_ZOOM:
if (new_state_fct)
{
zoom_view = true;
if (id == STATE_INPUT)
zoom_view_id = GUI_INPUT_ID;
else if (id == STATE_OUTPUT)
zoom_view_id = GUI_OUTPUT_ID;
// controller_reset();
}
break;
}
if (controller_wheel_right(0) || controller_wheel_left(0))
{
*state = STATE_FCT_IDLE;
controller_reset();
}
return true;
}