/**
* cpml_arc_info:
* @arc: (in): the #CpmlPrimitive arc data
* @center: (out) (allow-none): where to store the center coordinates
* @r: (out) (allow-none): where to store the radius
* @start: (out) (allow-none): where to store the starting angle
* @end: (out) (allow-none): where to store the ending angle
*
* Given an @arc, this function calculates and returns its basic data.
* Any pointer can be <constant>NULL</constant>, in which case the requested
* info is not returned. This function can fail (when the three points lay on
* a straight line, for example) in which case 0 is returned and no data can
* be considered valid.
*
* The radius @r can be 0 when the three points are coincidents: a
* circle with radius 0 is considered a valid path.
*
* When the start and end angle are returned, together with their
* values these angles implicitely gives another important information:
* the arc direction.
*
* If @start < @end the arc must be rendered with increasing angle
* value (clockwise direction using the ordinary cairo coordinate
* system) while if @start > @end the arc must be rendered in reverse
* order (that is counterclockwise in the cairo world). This is the
* reason the angle values are returned in the range <constant>-M_PI
* < <varname>value</varname> < 3 M_PI</constant> inclusive instead of
* the usual <constant>-M_PI < <varname>value</varname> < M_PI</constant>.
*
* Returns: (type boolean): 1 if the function worked succesfully, 0 on errors.
*
* Since: 1.0
**/
int
cpml_arc_info(const CpmlPrimitive *arc, CpmlPair *center,
double *r, double *start, double *end)
{
CpmlPair p[3], l_center;
cpml_pair_from_cairo(&p[0], arc->org);
cpml_pair_from_cairo(&p[1], &arc->data[1]);
cpml_pair_from_cairo(&p[2], &arc->data[2]);
if (! get_center(p, &l_center))
return 0;
if (center)
*center = l_center;
if (r != NULL)
*r = cpml_pair_distance(&p[0], &l_center);
if (start != NULL || end != NULL) {
double l_start, l_end;
get_angles(p, &l_center, &l_start, &l_end);
if (start != NULL)
*start = l_start;
if (end != NULL)
*end = l_end;
}
return 1;
}
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
void AP_Mount_Alexmos::status_msg(mavlink_channel_t chan)
{
if (!_initialised) {
return;
}
get_angles();
mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y*100, _current_angle.x*100, _current_angle.z*100);
}
void xmodel::updatepos()
{
physx::PxTransform pt;
pt = actor -> getGlobalPose();
physx::PxVec3 p=pt.p;
physx::PxQuat q=pt.q;
physx::PxVec3 up(0,0,1);
up = pt.rotate(up);
up_vec.x = up.x;
up_vec.y = up.y;
up_vec.z = up.z;
pos.x=p.x;
pos.y=p.y;
pos.z=p.z;
PxVec3 _vel = actor->getLinearVelocity();
vector3f vel;
vel.x = _vel.x;
vel.y = _vel.y;
vel.z = _vel.z;
vector3f acc = (vel - this->vel) /deltatime;
if (acc.abs() < 5 * this->acc.abs() || this->acc.abs() <1e-2 )
{
this -> acc = acc;
}
this->vel = vel;
get_angles(q.w,q.x,q.y,q.z);
_quat.w = q.w;
_quat.x = q.x;
_quat.y = q.y;
_quat.z = q.z;
angle=2*acos(q.w)/M_PI*180.0f;
yaw_rate = actor->getAngularVelocity().z;
roll_rate = actor ->getAngularVelocity().x;
pitch_rate = actor ->getAngularVelocity().y;
float scale=sqrt(q.x*q.x+q.y*q.y+q.z*q.z);
if(scale>1e-3)
{
ax=q.x/scale;
ay=q.y/scale;
az=q.z/scale;
}
}
void wait_signal()
{
struct timeval start, stop;
int waiting = 0;
gettimeofday(&start, 0);
double prev_time = start.tv_sec + (double)(start.tv_usec / 1000000.0);
int wait_count = 0;
while(waiting==0)
{
gettimeofday(&stop, 0);
double curr_time = (double)(stop.tv_sec + stop.tv_usec/1000000.0);
double elapsed = curr_time - (double)(start.tv_sec + start.tv_usec/1000000.0);
gettimeofday(&start, 0);
if(wait_count > 10)
{
char recv = 0;
uart_read_nc(&recv);// read to empty buffer
send_string("+IPD\r\n");
usleep(2000);
uart_read_nc(&recv);//try to remove while
if(recv == 1 && t_com < -3260)
{
//check throttle zero (t_com = -3276;) --> -3260
waiting = 1;
}
printf("Angle Pitch: %f Angle Roll: %f Throttle control: %d\n",comp_angle_pitch,comp_angle_roll,t_com);
wait_count = 0;
}
get_angles(elapsed);
wait_count +=1;
usleep(10000);//100hz
}
}
pair<Strip, Strip> cutstrips(Mat *left_image,
Mat *right_image,
Point2f epipole_left,
Point2f epipole_right,
Mat epipolar_line,
Point2f direction_point,
double bandwidth) {
Mat strip1, strip2, mask1, mask2;
Mat hline = (Mat_<float>(3, 1) << 0, 1, 0);
pair<Size, Size> sizes =
pair<Size, Size>((*left_image).size(),
(*right_image).size());
pair<Point2f, Point2f> centers =
pair<Point2f, Point2f>(
Point2f(sizes.first.width/2,
sizes.first.height/2),
Point2f(sizes.second.width/2,
sizes.second.height/2));
pair<double, double> angles =
get_angles(epipole_left,
direction_point,
epipolar_line,
hline);
pair<Point2f, Point2f> shifts =
pair<Point2f, Point2f>(
get_rotation_shift(sizes.first.width,
sizes.first.height,
angles.first),
get_rotation_shift(sizes.second.width,
sizes.second.height,
angles.second));
Point2f new_epipole_left =
rotatePoint(epipole_left,
angles.first,
centers.first) + shifts.first;
Point2f new_epipole_right =
rotatePoint(epipole_right,
angles.second,
centers.second) + shifts.second;
pair<Rect, Rect> bounds =
get_cut_bounds(bandwidth,
pair<Point2f, Point2f>(
new_epipole_left,
new_epipole_right));
pair<Point2f, Point2f> rshifts =
pair<Point2f, Point2f>(
new_epipole_left - epipole_left
- Point2f(0, bounds.first.y),
new_epipole_right - epipole_right
- Point2f(0, bounds.second.y));
mask1 = Mat::ones(sizes.first, CV_8U);
mask2 = Mat::ones(sizes.first, CV_8U);
init_mat_with_border(&mask1, 10, 10);
init_mat_with_border(&mask2, 10, 10);
strip1 = rotateImage(*left_image,
-angles.first,
true, false,
bounds.first.y,
bounds.first.height);
strip2 = rotateImage(*right_image,
-angles.second,
true, false,
bounds.second.y,
bounds.second.height);
mask1 = rotateImage(mask1,
-angles.first,
true, false,
bounds.first.y,
bounds.first.height);
mask2 = rotateImage(mask2,
-angles.second,
true, false,
bounds.second.y,
bounds.second.height);
return pair<Strip, Strip>(
Strip(strip1,
mask1,
angles.first,
rshifts.first),
Strip(strip2,
mask2,
angles.second,
rshifts.second));
}
// update mount position - should be called periodically
void AP_Mount_SToRM32_serial::update()
{
// exit immediately if not initialised
if (!_initialised) {
return;
}
read_incoming(); // read the incoming messages from the gimbal
// flag to trigger sending target angles to gimbal
bool resend_now = false;
// update based on mount mode
switch(get_mode()) {
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT:
{
const Vector3f &target = _state._retract_angles.get();
_angle_ef_target_rad.x = ToRad(target.x);
_angle_ef_target_rad.y = ToRad(target.y);
_angle_ef_target_rad.z = ToRad(target.z);
}
break;
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
const Vector3f &target = _state._neutral_angles.get();
_angle_ef_target_rad.x = ToRad(target.x);
_angle_ef_target_rad.y = ToRad(target.y);
_angle_ef_target_rad.z = ToRad(target.z);
}
break;
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
// do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's rc inputs
update_targets_from_rc();
resend_now = true;
break;
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
resend_now = true;
}
break;
default:
// we do not know this mode so do nothing
break;
}
// resend target angles at least once per second
resend_now = resend_now || ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_SERIAL_RESEND_MS);
if ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_SERIAL_RESEND_MS*2) {
_reply_type = ReplyType_UNKNOWN;
}
if (can_send(resend_now)) {
if (resend_now) {
send_target_angles(ToDeg(_angle_ef_target_rad.y), ToDeg(_angle_ef_target_rad.x), ToDeg(_angle_ef_target_rad.z));
get_angles();
_reply_type = ReplyType_ACK;
_reply_counter = 0;
_reply_length = get_reply_size(_reply_type);
} else {
get_angles();
_reply_type = ReplyType_DATA;
_reply_counter = 0;
_reply_length = get_reply_size(_reply_type);
}
}
}
