void GPS_NewData(void) {
static uint32_t nav_loopTimer;
uint32_t dist;
int32_t dir;
uint8_t axis;
int16_t speed;
uint16_t c = GPSBufferAvailable();
while (c--) {
if (GPS_newFrame(GPSBufferRead())) {
if (GPS_Info.GPS_update == 1)
GPS_Info.GPS_update = 0;
else
GPS_Info.GPS_update = 1;
if (fgps.GPS_FIX && GPS_Info.GPS_numSat >= 5) {
if (!checkArm()) {
fgps.GPS_FIX_HOME = 0;
}
if (!fgps.GPS_FIX_HOME && checkArm()) {
GPS_reset_home_position();
}
//Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis< 2; axis++) {
GPS_data[axis] = GPS_Info.GPS_coord[axis]; //latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_data[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
// How close we are to a degree line ? its the first three digits from the fractions of degree
// later we use it to Check if we are close to a degree line, if yes, disable averaging,
fraction3[axis] = (GPS_data[axis]- GPS_degree[axis]*10000000) / 10000;
GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
GPS_filter[axis][GPS_filter_index] = GPS_data[axis] - (GPS_degree[axis]*10000000);
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000);
if ( nav_mode == NAV_MODE_POSHOLD) { //we use gps averaging only in poshold mode...
if ( fraction3[axis]>1 && fraction3[axis]<999 )
GPS_Info.GPS_coord[axis] = GPS_filtered[axis];
}
}
#endif
//Time for calculating x,y speed and navigation pids
dTnav = (float)(getTickCount() - nav_loopTimer)/ 10000.0f;
nav_loopTimer = getTickCount();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0f);
//calculate distance and bearings for gui and other stuff continously - From home to copter
GPS_distance_cm_bearing(&GPS_Info.GPS_coord[LAT],&GPS_Info.GPS_coord[LON],&GPS_Info.GPS_home[LAT],&GPS_Info.GPS_home[LON],&dist,&dir);
GPS_Info.GPS_distanceToHome = dist/100;
GPS_Info.GPS_directionToHome = dir/100;
if (!fgps.GPS_FIX_HOME) { //If we don't have home set, do not display anything
GPS_Info.GPS_distanceToHome = 0;
GPS_Info.GPS_directionToHome = 0;
}
//calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
if (fgps.GPS_HOLD_MODE || fgps.GPS_HOME_MODE){ //ok we are navigating
//do gps nav calculations here, these are common for nav and poshold
#if defined(GPS_LEAD_FILTER)
GPS_distance_cm_bearing(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]);
#else
GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]);
#endif
switch (nav_mode) {
case NAV_MODE_POSHOLD:
//Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_poshold();
break;
case NAV_MODE_WP:
speed = GPS_calc_desired_speed(NAV_SPEED_MAX, NAV_SLOW_NAV); //slow navigation
// use error as the desired rate towards the target
//Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_nav_rate(speed);
//Tail control
if (NAV_CONTROLS_HEADING) {
if (NAV_TAIL_FIRST) {
magHold = wrap_18000(nav_bearing-18000)/100;
} else {
magHold = nav_bearing/100;
}
}
// Are we there yet ?(within 2 meters of the destination)
if ((wp_distance <= GPS_wp_radius) || check_missed_wp()){ //if yes switch to poshold mode
nav_mode = NAV_MODE_POSHOLD;
if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; }
}
break;
}
} //end of gps calcs
}
}
}
}
void ArmContactModelProvider::update(ArmContactModel& model)
{
MODIFY("module:ArmContactModelProvider:parameters", p);
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftX");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightX");
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftY");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightY");
DECLARE_PLOT("module:ArmContactModelProvider:errorDurationLeft");
DECLARE_PLOT("module:ArmContactModelProvider:errorDurationRight");
DECLARE_PLOT("module:ArmContactModelProvider:errorYThreshold");
DECLARE_PLOT("module:ArmContactModelProvider:errorXThreshold");
DECLARE_PLOT("module:ArmContactModelProvider:contactLeft");
DECLARE_PLOT("module:ArmContactModelProvider:contactRight");
DECLARE_DEBUG_DRAWING("module:ArmContactModelProvider:armContact", "drawingOnField");
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 400, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 600, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 800, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1000, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
/* Buffer arm angles */
struct ArmAngles angles;
angles.leftX = theJointRequest.angles[JointData::LShoulderPitch];
angles.leftY = theJointRequest.angles[JointData::LShoulderRoll];
angles.rightX = theJointRequest.angles[JointData::RShoulderPitch];
angles.rightY = theJointRequest.angles[JointData::RShoulderRoll];
angleBuffer.add(angles);
/* Reset in case of a fall or penalty */
if(theFallDownState.state == FallDownState::onGround || theRobotInfo.penalty != PENALTY_NONE)
{
leftErrorBuffer.init();
rightErrorBuffer.init();
}
Pose2D odometryOffset = theOdometryData - lastOdometry;
lastOdometry = theOdometryData;
const Vector3<>& leftHandPos3D = theRobotModel.limbs[MassCalibration::foreArmLeft].translation;
Vector2<> leftHandPos(leftHandPos3D.x, leftHandPos3D.y);
Vector2<> leftHandSpeed = (odometryOffset + Pose2D(leftHandPos) - Pose2D(lastLeftHandPos)).translation / theFrameInfo.cycleTime;
float leftFactor = std::max(0.f, 1.f - leftHandSpeed.abs() / p.speedBasedErrorReduction);
lastLeftHandPos = leftHandPos;
const Vector3<>& rightHandPos3D = theRobotModel.limbs[MassCalibration::foreArmRight].translation;
Vector2<> rightHandPos(rightHandPos3D.x, rightHandPos3D.y);
Vector2<> rightHandSpeed = (odometryOffset + Pose2D(rightHandPos) - Pose2D(lastRightHandPos)).translation / theFrameInfo.cycleTime;
float rightFactor = std::max(0.f, 1.f - rightHandSpeed.abs() / p.speedBasedErrorReduction);
lastRightHandPos = rightHandPos;
/* Check for arm contact */
// motion types to take into account: stand, walk (if the robot is upright)
if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
(theFallDownState.state == FallDownState::upright || theFallDownState.state == FallDownState::staggering) &&
(theGameInfo.state == STATE_PLAYING || theGameInfo.state == STATE_READY) &&
(theRobotInfo.penalty == PENALTY_NONE)) // TICKET 897: ArmContact only if robot is not penalized
{
checkArm(LEFT, leftFactor);
checkArm(RIGHT, rightFactor);
//left and right are projections of the 3 dimensional shoulder-joint vector
//onto the x-y plane.
Vector2f left = leftErrorBuffer.getAverageFloat();
Vector2f right = rightErrorBuffer.getAverageFloat();
//Determine if we are being pushed or not
bool leftX = fabs(left.x) > fromDegrees(p.errorXThreshold);
bool leftY = fabs(left.y) > fromDegrees(p.errorYThreshold);
bool rightX = fabs(right.x)> fromDegrees(p.errorXThreshold);
bool rightY = fabs(right.y)> fromDegrees(p.errorYThreshold);
// update the model
model.contactLeft = leftX || leftY;
model.contactRight = rightX || rightY;
// The duration of the contact is counted upwards as long as the error
//remains. Otherwise it is reseted to 0.
model.durationLeft = model.contactLeft ? model.durationLeft + 1 : 0;
model.durationRight = model.contactRight ? model.durationRight + 1 : 0;
model.contactLeft &= model.durationLeft < p.malfunctionThreshold;
model.contactRight &= model.durationRight < p.malfunctionThreshold;
if(model.contactLeft)
{
model.timeOfLastContactLeft = theFrameInfo.time;
}
if(model.contactRight)
{
model.timeOfLastContactRight = theFrameInfo.time;
}
model.pushDirectionLeft = getDirection(LEFT, leftX, leftY, left);
model.pushDirectionRight = getDirection(RIGHT, rightX, rightY, right);
model.lastPushDirectionLeft = model.pushDirectionLeft != ArmContactModel::NONE ? model.pushDirectionLeft : model.lastPushDirectionLeft;
model.lastPushDirectionRight = model.pushDirectionRight != ArmContactModel::NONE ? model.pushDirectionRight : model.lastPushDirectionRight;
PLOT("module:ArmContactModelProvider:errorLeftX", toDegrees(left.x));
PLOT("module:ArmContactModelProvider:errorRightX", toDegrees(right.x));
PLOT("module:ArmContactModelProvider:errorLeftY", toDegrees(left.y));
PLOT("module:ArmContactModelProvider:errorRightY", toDegrees(right.y));
PLOT("module:ArmContactModelProvider:errorDurationLeft", model.durationLeft);
PLOT("module:ArmContactModelProvider:errorDurationRight", model.durationRight);
PLOT("module:ArmContactModelProvider:errorYThreshold", toDegrees(p.errorYThreshold));
PLOT("module:ArmContactModelProvider:errorXThreshold", toDegrees(p.errorXThreshold));
PLOT("module:ArmContactModelProvider:contactLeft", model.contactLeft ? 10.0 : 0.0);
PLOT("module:ArmContactModelProvider:contactRight", model.contactRight ? 10.0 : 0.0);
ARROW("module:ArmContactModelProvider:armContact", 0, 0, -(toDegrees(left.y) * SCALE), toDegrees(left.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::green);
ARROW("module:ArmContactModelProvider:armContact", 0, 0, toDegrees(right.y) * SCALE, toDegrees(right.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::red);
COMPLEX_DRAWING("module:ArmContactModelProvider:armContact",
{
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1300, 20, ColorClasses::black, "LEFT");
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(left.x));
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 900, 20, ColorClasses::black, "ErrorY: " << toDegrees(left.y));
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 500, 20, ColorClasses::black, ArmContactModel::getName(model.pushDirectionLeft));
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 300, 20, ColorClasses::black, "Time: " << model.timeOfLastContactLeft);
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1300, 20, ColorClasses::black, "RIGHT");
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(right.x));
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 900, 20, ColorClasses::black, "ErrorY: " << toDegrees(right.y));
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 500, 20, ColorClasses::black, ArmContactModel::getName(model.pushDirectionRight));
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 300, 20, ColorClasses::black, "Time: " << model.timeOfLastContactRight);
if (model.contactLeft)
{
CROSS("module:ArmContactModelProvider:armContact", -2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
}
if (model.contactRight)
{
CROSS("module:ArmContactModelProvider:armContact", 2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
}
});
void ArmContactModelProvider::update(ArmContactModel& ArmContactModel,
const JointRequest& theJointRequest,
const FallDownState& theFallDownState,
const MotionInfo& theMotionInfo,
const FilteredJointData& theFilteredJointData,
const FrameInfo& theFrameInfo)
{
//MODIFY("module:ArmContactModelProvider:parameters", p);
/*
DECLARE_PLOT("module:ArmContactModelProvider:contactLeft");
DECLARE_PLOT("module:ArmContactModelProvider:contactRight");
PLOT("module:ArmContactModelProvider:contactLeft", ArmContactModel.contactLeft ? 7.5 : 2.5);
PLOT("module:ArmContactModelProvider:contactRight", ArmContactModel.contactRight ? 7.5 : 2.5);
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftX");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightX");
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftY");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightY");
PLOT("module:ArmContactModelProvider:errorLeftX", errorLeftX);
PLOT("module:ArmContactModelProvider:errorRightX", errorRightX);
PLOT("module:ArmContactModelProvider:errorLeftY", errorLeftY);
PLOT("module:ArmContactModelProvider:errorRightY", errorRightY);
*/
/* Buffer arm angles */
struct ArmAngles angles;
angles.leftX = theJointRequest.angles[JointData::LShoulderPitch];
angles.leftY = theJointRequest.angles[JointData::LShoulderRoll];
angles.rightX = theJointRequest.angles[JointData::RShoulderPitch];
angles.rightY = theJointRequest.angles[JointData::RShoulderRoll];
angleBuffer.add(angles);
/* Decrease errors */
errorLeftX = max(0.0f, errorLeftX - p.errorXDecrease);
errorLeftY = max(0.0f, errorLeftY - p.errorYDecrease);
errorRightX = max(0.0f, errorRightX - p.errorXDecrease);
errorRightY = max(0.0f, errorRightY - p.errorYDecrease);
/* Reset in case of a fall */
if(theFallDownState.state == FallDownState::onGround)
{
errorLeftX = 0.0f;
errorLeftY = 0.0f;
errorRightX = 0.0f;
errorRightY = 0.0f;
}
/* Check for arm contact */
// motion types to take into account: stand, walk (if the robot is upright)
if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
(theFallDownState.state == FallDownState::upright || theFallDownState.state == FallDownState::staggering))
{
bool left = checkArm(true, theFilteredJointData);
bool right = checkArm(false, theFilteredJointData);
if(p.debugMode && theFrameInfo.getTimeSince(lastSoundTime) > soundDelay &&
((left && !ArmContactModel.contactLeft) || (right && !ArmContactModel.contactRight)))
{
lastSoundTime = theFrameInfo.time;
//SoundPlayer::play("arm.wav");
}
ArmContactModel.contactLeft = left;
ArmContactModel.contactRight = right;
}
else
{
ArmContactModel.contactLeft = false;
ArmContactModel.contactRight = false;
}
}
