int main(){
int monte = 1;
printf("%d/%d valid messages in test_gps\n",test_gps(monte),monte);
printf("%d/%d valid messages in test_control\n",test_control(monte),monte);
printf("%d/%d valid messages in test_command\n",test_command(monte),monte);
printf("%d/%d valid messages in test_pid\n",test_pid(monte),monte);
// weirdness
uint8_t msg[] = {0x7F,0x53,0x01,0xC0,0xAE,0x93,0xC6,0xC0,0x3D,0x40,0x12,0xEC,0x51,0x20,0x41,0x87};
int32_t lon=0,lat=0;
float t=0;
if (esp_unpack_gps(msg,&lon,&lat,&t) > 0){
printf("Hardcoded msg: lon = %ld, lat = %ld, t=%f\n",lon,lat,t);
}
printf("%ld\n",0xC0);
printf("%ld\n",0xAE<<8);
printf("%ld\n",0x93<<16);
printf("%ld\n",0xC6<<24);
printf("%ld",(0xC0) + (0xAE << 8) + (0x93<<16) + (0xC6 << 24));
return 0;
}
static int
_fork_test_daemon(int num_disks, int test_typ, int loop_cnt, int from, int seed)
{
int i;
int type;
int dowait = 0, verify = 0;
if (num_disks == -1) {
return (test_control(test_typ, loop_cnt, from, seed));
}
type = test_typ;
cmn_err(CE_NOTE,
"!sd_test %d %d %d %d %d", num_disks, type, loop_cnt, from, seed);
if (type == 100) {
test_stop = 1;
return (0);
}
if (type == 99) {
/* Set some parameters for other tests */
switch (num_disks) {
/* Params set for this test */
#if 0
case 302 :
_sd_write_len = loop_cnt;
break;
case 303 :
_sd_write_len = loop_cnt;
break;
case 304 :
_sd_trk_zero = loop_cnt;
_sd_trk_size = from;
break;
case 305 :
_sd_min_blks = loop_cnt;
_sd_max_blks = from;
break;
#endif
default :
cmn_err(CE_WARN,
"!Usage : sd_test <test_num> 99"
" <param1> <param2> <param3>");
break;
}
return (0);
} /* type == 99 */
if (type > 1000) {
dowait = 1;
type -= 1000;
}
if (type > 1000) {
verify = 1;
type -= 1000;
}
again:
set_parameters();
for (i = from; i < (from+num_disks); i++) {
if (daemon_awake(i)) {
cmn_err(CE_WARN, "!Daemon %d awake!?", i);
return (-1);
}
if (daemon_nexist(i)) {
cmn_err(CE_WARN, "!Daemon %d nexist!?", i);
return (-1);
}
gld[i].type = type;
gld[i].loop = loop_cnt;
gld[i].seed = seed;
daemon_wakeup(i);
}
cmn_err(CE_CONT, "!%d daemons woken (test %d)\n", num_disks, type);
if (num_disks <= 0)
return (0);
if (dowait) {
wait:
mutex_enter(&tdaemon_lock);
if (!cv_wait_sig(&_wait_daemons, &tdaemon_lock)) {
mutex_exit(&tdaemon_lock);
test_stop = 1;
cmn_err(CE_WARN, "!Interrupt: stopping tests");
return (-1); /* interrupt */
}
mutex_exit(&tdaemon_lock);
/* wait for all to stop */
if (test_stop)
return (-1);
for (i = from; i < (from+num_disks); i++) {
if (daemon_awake(i))
goto wait;
}
}
if (verify) {
verify = 0;
type++; /* next test */
goto again;
}
return (0);
}
static void test_control()
{
/* Could add more widgets to the list. */
test_control(gui2::label(gui2::implementation::builder_label(config())));
}
static void test_grid()
{
/* An empty grid behaves the same as a control so test here. */
test_control(gui2::grid());
/* Test the child part here. */
gui2::grid grid(2 ,2);
grid.set_id("0");
gui2::grid* g = new gui2::grid(2, 2);
add_widget(grid, g, "1", 0, 0);
add_widget(grid, new gui2::label(gui2::implementation::builder_label(config())), "2", 1, 0);
add_widget(grid, new gui2::label(gui2::implementation::builder_label(config())), "3", 0, 1);
add_widget(grid, new gui2::label(gui2::implementation::builder_label(config())), "4", 1, 1);
add_widget(*g, new gui2::label(gui2::implementation::builder_label(config())), "5", 0, 0);
add_widget(*g, new gui2::label(gui2::implementation::builder_label(config())), "6", 1, 0);
add_widget(*g, new gui2::label(gui2::implementation::builder_label(config())), "7", 0, 1);
add_widget(*g, new gui2::label(gui2::implementation::builder_label(config())), "8", 1, 1);
{
std::stringstream sstr;
lg::redirect_output_setter redirect_output(sstr);
gui2::iteration::iterator<gui2::iteration::policy::order::top_down<
true
, true
, true>>
iterator(grid);
while(iterator.next()) {
/* DO NOTHING */
}
BOOST_CHECK_EQUAL(top_down_t_t_t_result(), sstr.str());
}
{
std::stringstream sstr;
lg::redirect_output_setter redirect_output(sstr);
gui2::iteration::iterator<gui2::iteration::policy::order::top_down<
true
, true
, true>>
iterator(grid);
for( ; !iterator.at_end(); ++iterator) {
/* DO NOTHING */
}
BOOST_CHECK_EQUAL(top_down_t_t_t_result(), sstr.str());
}
{
std::stringstream sstr;
lg::redirect_output_setter redirect_output(sstr);
gui2::iteration::iterator<gui2::iteration::policy::order::bottom_up<
true
, true
, true>>
iterator(grid);
while(iterator.next()) {
/* DO NOTHING */
}
BOOST_CHECK_EQUAL(bottom_up_t_t_t_result(), sstr.str());
}
{
std::stringstream sstr;
lg::redirect_output_setter redirect_output(sstr);
gui2::iteration::iterator<gui2::iteration::policy::order::bottom_up<
true
, true
, true>>
iterator(grid);
for( ; !iterator.at_end(); ++iterator) {
/* DO NOTHING */
}
BOOST_CHECK_EQUAL(bottom_up_t_t_t_result(), sstr.str());
}
}
void gradient_descent_local_planner_t::gradient_steer(const state_t* start, const state_t* goal, plan_t& plan)
{
//for now only going to do piecewise constant plans
plan.clear();
traj.link_space(world_model->get_state_space());
plan.append_onto_front(max_multiple * simulation::simulation_step);
const space_t* control_space = world_model->get_control_space();
sampler->sample(control_space, plan[0].control);
unsigned count = 0;
std::vector<double> old_control(control_space->get_dimension());
std::vector<double> test_control(control_space->get_dimension());
std::vector<double> control_below(control_space->get_dimension());
std::vector<double> control_above(control_space->get_dimension());
std::vector<std::pair<double, double> > diffs(control_space->get_dimension());
while( count < attempts )
{
traj.clear();
plan[0].duration = max_multiple * simulation::simulation_step;
propagate(start, plan, traj);
unsigned num_sim_steps = 0;
unsigned best_sim_step = 0;
double best_dist = PRX_INFINITY;
for( trajectory_t::iterator it = traj.begin(); it != traj.end(); it++ )
{
num_sim_steps++;
if( metric->distance_function(*it, goal) < best_dist )
{
best_dist = metric->distance_function(*it, goal);
best_sim_step = num_sim_steps;
}
}
plan[0].duration = best_sim_step * simulation::simulation_step;
if( best_dist < accepted_radius )
{
return;
}
else
{
state_t* state = world_model->get_state_space()->alloc_point();
for( unsigned i = 0; i < control_space->get_dimension(); i++ )
{
old_control[i] = plan[0].control->at(i);
control_below[i] = old_control[i] - .01 * (control_space->get_bounds()[i]->get_upper_bound() - control_space->get_bounds()[i]->get_lower_bound());
control_above[i] = old_control[i] + .01 * (control_space->get_bounds()[i]->get_upper_bound() - control_space->get_bounds()[i]->get_lower_bound());
diffs[i].first = diffs[i].second = 0;
}
for( unsigned i = 0; i < control_space->get_dimension(); i++ )
{
test_control = old_control;
test_control[i] = control_below[i];
control_space->set_from_vector(test_control, plan[0].control);
propagate_step(start, plan, state);
diffs[i].first = metric->distance_function(state, goal);
test_control[i] = control_above[i];
control_space->set_from_vector(test_control, plan[0].control);
propagate_step(start, plan, state);
diffs[i].second = metric->distance_function(state, goal);
}
world_model->get_state_space()->free_point(state);
//now that all the differences have been computed, determine the direction to move
test_control = old_control;
for( unsigned i = 0; i < control_space->get_dimension(); i++ )
{
test_control[i] += (diffs[i].first - diffs[i].second)*(learning_rate);
}
control_space->set_from_vector(test_control, plan[0].control);
}
count++;
}
// PRX_INFO_S(plan.print());
}
