void atmega_i2c_cam_ctrl_send(uint8_t cmd) { static uint8_t zoom = 0; static uint8_t mode = 0; unsigned char cam_ret[1]; if (cmd == DC_SHOOT) { dc_send_shot_position(); } else if (cmd == DC_TALLER) { zoom = 1; } else if (cmd == DC_WIDER) { zoom = 0; } else if (cmd == DC_GET_STATUS) { mode++; if (mode > 15) mode = 0; } cam_ret[0] = mode + zoom * 0x20; RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, cam_ret )); }
// Called on each vision analysis result after receiving the struct void run_avoid_navigation_onvision(void) { // Send ALL vision data to the ground DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 5, avoid_navigation_data.stereo_bin); switch (avoid_navigation_data.mode) { case 0: // Go to Goal and stop at obstacles //count 4 subsequent obstacles if (avoid_navigation_data.stereo_bin[0] > 1) { counter = counter + 1; if (counter > 1) { counter = 0; //Obstacle detected, go to turn until clear mode obstacle_detected = TRUE; avoid_navigation_data.mode = 1; } } else { counter = 0; } break; case 1: // Turn until clear //count 20 subsequent free frames if (avoid_navigation_data.stereo_bin[0] < 1) { counter = counter + 1; if (counter > 12) { counter = 0; //Stop and put waypoint 2.5 m ahead struct EnuCoor_i new_coor; struct EnuCoor_i *pos = stateGetPositionEnu_i(); float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH)); new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH)); new_coor.z = pos->z; waypoint_set_xy_i(WP_W1, new_coor.x, new_coor.y); obstacle_detected = FALSE; avoid_navigation_data.mode = 0; } } else { counter = 0; } break; case 2: break; default: // do nothing break; } avoid_navigation_data.stereo_bin[2] = avoid_navigation_data.stereo_bin[0] > 20; avoid_navigation_data.stereo_bin[3] = avoid_navigation_data.mode; avoid_navigation_data.stereo_bin[4] = counter; #ifdef STEREO_LED if (obstacle_detected) { LED_ON(STEREO_LED); } else { LED_OFF(STEREO_LED); } #endif }
void vision_periodic( void ) { static uint8_t nr = 0; vision_data[nr] ++; nr ++; if (nr >= VISION_DATA_SIZE) nr = 0; RunOnceEvery(10,DOWNLINK_SEND_PAYLOAD(DefaultChannel, VISION_DATA_SIZE, vision_data)); }
void atmega_i2c_cam_ctrl_event( void ) { if (atmega_i2c_cam_ctrl_trans.status == I2CTransSuccess) { unsigned char cam_ret[1]; cam_ret[0] = atmega_i2c_cam_ctrl_trans.buf[0]; RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, cam_ret )); atmega_i2c_cam_ctrl_trans.status = I2CTransDone; } }
// Called on each vision analysis result after receiving the struct void run_avoid_navigation_onvision(void) { // Send ALL vision data to the ground DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, N_BINS, gst2ppz.obstacle_bins); switch (avoid_navigation_data.mode) { case 1: // climb until clear run_avoid_navigation_climb_until_clear(); break; case 2: run_avoid_navigation_move_target_waypoint(); break; default: // do nothing break; } }
void atmega48_periodic( void ) { from_atmega48[10] = to_atmega48[0]; from_atmega48[11] = to_atmega48[1]; RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, ATMEGA_RX_SIZE, from_atmega48)); }
void stereocam_droplet_periodic(void) { static float heading = 0; // Read Serial while (StereoChAvailable()) { stereo_parse(StereoGetch()); } if (avoid_navigation_data.timeout <= 0) return; avoid_navigation_data.timeout --; // Results DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, avoid_navigation_data.stereo_bin); volatile bool_t once = TRUE; // Move waypoint with constant speed in current direction if ( (avoid_navigation_data.stereo_bin[0] == 97) || (avoid_navigation_data.stereo_bin[0] == 100) ) { once = TRUE; struct EnuCoor_f enu; enu.x = waypoint_get_x(WP_GOAL); enu.y = waypoint_get_y(WP_GOAL); enu.z = waypoint_get_alt(WP_GOAL); float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); enu.x += (sin_heading * 1.3 / 20); enu.y += (cos_heading * 1.3 / 20); waypoint_set_enu(WP_GOAL, &enu); } else if (avoid_navigation_data.stereo_bin[0] == 98) { // STOP!!! if (once) { NavSetWaypointHere(WP_GOAL); once = FALSE; } } else { once = TRUE; } switch (avoid_navigation_data.stereo_bin[0]) { case 99: // Turn heading += 4; if (heading > 360) { heading = 0; } nav_set_heading_rad(RadOfDeg(heading)); break; default: // do nothing break; } #ifdef STEREO_LED if (obstacle_detected) { LED_ON(STEREO_LED); } else { LED_OFF(STEREO_LED); } #endif }