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
}
