From 44d2ae06fb797e215fa6411f2db282df864c0237 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 17 Apr 2015 22:59:34 +0900 Subject: [PATCH] Mount: unix style line endings --- libraries/AP_Mount/AP_Mount_Backend.cpp | 310 ++++++++++++------------ 1 file changed, 155 insertions(+), 155 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 87b4edb3c5..addff24c4a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -1,155 +1,155 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include - -extern const AP_HAL::HAL& hal; - -// set_angle_targets - sets angle targets in degrees -void AP_Mount_Backend::set_angle_targets(float roll, float tilt, float pan) -{ - // set angle targets - _angle_ef_target_rad.x = radians(roll); - _angle_ef_target_rad.y = radians(tilt); - _angle_ef_target_rad.z = radians(pan); - - // set the mode to mavlink targeting - _frontend.set_mode(_instance, MAV_MOUNT_MODE_MAVLINK_TARGETING); -} - -// set_roi_target - sets target location that mount should attempt to point towards -void AP_Mount_Backend::set_roi_target(const struct Location &target_loc) -{ - // set the target gps location - _state._roi_target = target_loc; - - // set the mode to GPS tracking mode - _frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT); -} - -// configure_msg - process MOUNT_CONFIGURE messages received from GCS -void AP_Mount_Backend::configure_msg(mavlink_message_t* msg) -{ - __mavlink_mount_configure_t packet; - mavlink_msg_mount_configure_decode(msg, &packet); - - // set mode - _frontend.set_mode(_instance,(enum MAV_MOUNT_MODE)packet.mount_mode); - - // set which axis are stabilized - _state._stab_roll = packet.stab_roll; - _state._stab_tilt = packet.stab_pitch; - _state._stab_pan = packet.stab_yaw; -} - -// control_msg - process MOUNT_CONTROL messages received from GCS -void AP_Mount_Backend::control_msg(mavlink_message_t *msg) -{ - __mavlink_mount_control_t packet; - mavlink_msg_mount_control_decode(msg, &packet); - - // interpret message fields based on mode - switch (_frontend.get_mode(_instance)) { - case MAV_MOUNT_MODE_RETRACT: - case MAV_MOUNT_MODE_NEUTRAL: - // do nothing with request if mount is retracted or in neutral position - break; - - // set earth frame target angles from mavlink message - case MAV_MOUNT_MODE_MAVLINK_TARGETING: - set_angle_targets(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f); - break; - - // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization - case MAV_MOUNT_MODE_RC_TARGETING: - // do nothing if pilot is controlling the roll, pitch and yaw - break; - - // set lat, lon, alt position targets from mavlink message - case MAV_MOUNT_MODE_GPS_POINT: - Location target_location; - memset(&target_location, 0, sizeof(target_location)); - target_location.lat = packet.input_a; - target_location.lng = packet.input_b; - target_location.alt = packet.input_c; - target_location.flags.relative_alt = true; - set_roi_target(target_location); - break; - - default: - // do nothing - break; - } -} - -// update_targets_from_rc - updates angle targets using input from receiver -void AP_Mount_Backend::update_targets_from_rc() -{ -#define rc_ch(i) RC_Channel::rc_channel(i-1) - - uint8_t roll_rc_in = _state._roll_rc_in; - uint8_t tilt_rc_in = _state._tilt_rc_in; - uint8_t pan_rc_in = _state._pan_rc_in; - - // if joystick_speed is defined then pilot input defines a rate of change of the angle - if (_frontend._joystick_speed) { - // allow pilot speed position input to come directly from an RC_Channel - if (roll_rc_in && rc_ch(roll_rc_in)) { - _angle_ef_target_rad.x += rc_ch(roll_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - constrain_float(_angle_ef_target_rad.x, radians(_state._roll_angle_min*0.01f), radians(_state._roll_angle_max*0.01f)); - } - if (tilt_rc_in && (rc_ch(tilt_rc_in))) { - _angle_ef_target_rad.y += rc_ch(tilt_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - constrain_float(_angle_ef_target_rad.y, radians(_state._tilt_angle_min*0.01f), radians(_state._tilt_angle_max*0.01f)); - } - if (pan_rc_in && (rc_ch(pan_rc_in))) { - _angle_ef_target_rad.z += rc_ch(pan_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - constrain_float(_angle_ef_target_rad.z, radians(_state._pan_angle_min*0.01f), radians(_state._pan_angle_max*0.01f)); - } - } else { - // allow pilot position input to come directly from an RC_Channel - if (roll_rc_in && (rc_ch(roll_rc_in))) { - _angle_ef_target_rad.x = angle_input_rad(rc_ch(roll_rc_in), _state._roll_angle_min, _state._roll_angle_max); - } - if (tilt_rc_in && (rc_ch(tilt_rc_in))) { - _angle_ef_target_rad.y = angle_input_rad(rc_ch(tilt_rc_in), _state._tilt_angle_min, _state._tilt_angle_max); - } - if (pan_rc_in && (rc_ch(pan_rc_in))) { - _angle_ef_target_rad.z = angle_input_rad(rc_ch(pan_rc_in), _state._pan_angle_min, _state._pan_angle_max); - } - } -} - -// returns the angle (degrees*100) that the RC_Channel input is receiving -int32_t AP_Mount_Backend::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max) -{ - return (rc->get_reverse() ? -1 : 1) * (rc->radio_in - rc->radio_min) * (int32_t)(angle_max - angle_min) / (rc->radio_max - rc->radio_min) + (rc->get_reverse() ? angle_max : angle_min); -} - -// returns the angle (radians) that the RC_Channel input is receiving -float AP_Mount_Backend::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max) -{ - return radians(angle_input(rc, angle_min, angle_max)*0.01f); -} - -// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target -void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan) -{ - float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f; - float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f; - float GPS_vector_z = (target.alt-_frontend._current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter). - float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. - - // initialise all angles to zero - angles_to_target_rad.zero(); - - // tilt calcs - if (calc_tilt) { - angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance); - } - - // pan calcs - if (calc_pan) { - // calc absolute heading and then onvert to vehicle relative yaw - angles_to_target_rad.z = wrap_PI(atan2f(GPS_vector_x, GPS_vector_y) - _frontend._ahrs.yaw); - } -} +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +extern const AP_HAL::HAL& hal; + +// set_angle_targets - sets angle targets in degrees +void AP_Mount_Backend::set_angle_targets(float roll, float tilt, float pan) +{ + // set angle targets + _angle_ef_target_rad.x = radians(roll); + _angle_ef_target_rad.y = radians(tilt); + _angle_ef_target_rad.z = radians(pan); + + // set the mode to mavlink targeting + _frontend.set_mode(_instance, MAV_MOUNT_MODE_MAVLINK_TARGETING); +} + +// set_roi_target - sets target location that mount should attempt to point towards +void AP_Mount_Backend::set_roi_target(const struct Location &target_loc) +{ + // set the target gps location + _state._roi_target = target_loc; + + // set the mode to GPS tracking mode + _frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT); +} + +// configure_msg - process MOUNT_CONFIGURE messages received from GCS +void AP_Mount_Backend::configure_msg(mavlink_message_t* msg) +{ + __mavlink_mount_configure_t packet; + mavlink_msg_mount_configure_decode(msg, &packet); + + // set mode + _frontend.set_mode(_instance,(enum MAV_MOUNT_MODE)packet.mount_mode); + + // set which axis are stabilized + _state._stab_roll = packet.stab_roll; + _state._stab_tilt = packet.stab_pitch; + _state._stab_pan = packet.stab_yaw; +} + +// control_msg - process MOUNT_CONTROL messages received from GCS +void AP_Mount_Backend::control_msg(mavlink_message_t *msg) +{ + __mavlink_mount_control_t packet; + mavlink_msg_mount_control_decode(msg, &packet); + + // interpret message fields based on mode + switch (_frontend.get_mode(_instance)) { + case MAV_MOUNT_MODE_RETRACT: + case MAV_MOUNT_MODE_NEUTRAL: + // do nothing with request if mount is retracted or in neutral position + break; + + // set earth frame target angles from mavlink message + case MAV_MOUNT_MODE_MAVLINK_TARGETING: + set_angle_targets(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f); + break; + + // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization + case MAV_MOUNT_MODE_RC_TARGETING: + // do nothing if pilot is controlling the roll, pitch and yaw + break; + + // set lat, lon, alt position targets from mavlink message + case MAV_MOUNT_MODE_GPS_POINT: + Location target_location; + memset(&target_location, 0, sizeof(target_location)); + target_location.lat = packet.input_a; + target_location.lng = packet.input_b; + target_location.alt = packet.input_c; + target_location.flags.relative_alt = true; + set_roi_target(target_location); + break; + + default: + // do nothing + break; + } +} + +// update_targets_from_rc - updates angle targets using input from receiver +void AP_Mount_Backend::update_targets_from_rc() +{ +#define rc_ch(i) RC_Channel::rc_channel(i-1) + + uint8_t roll_rc_in = _state._roll_rc_in; + uint8_t tilt_rc_in = _state._tilt_rc_in; + uint8_t pan_rc_in = _state._pan_rc_in; + + // if joystick_speed is defined then pilot input defines a rate of change of the angle + if (_frontend._joystick_speed) { + // allow pilot speed position input to come directly from an RC_Channel + if (roll_rc_in && rc_ch(roll_rc_in)) { + _angle_ef_target_rad.x += rc_ch(roll_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; + constrain_float(_angle_ef_target_rad.x, radians(_state._roll_angle_min*0.01f), radians(_state._roll_angle_max*0.01f)); + } + if (tilt_rc_in && (rc_ch(tilt_rc_in))) { + _angle_ef_target_rad.y += rc_ch(tilt_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; + constrain_float(_angle_ef_target_rad.y, radians(_state._tilt_angle_min*0.01f), radians(_state._tilt_angle_max*0.01f)); + } + if (pan_rc_in && (rc_ch(pan_rc_in))) { + _angle_ef_target_rad.z += rc_ch(pan_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; + constrain_float(_angle_ef_target_rad.z, radians(_state._pan_angle_min*0.01f), radians(_state._pan_angle_max*0.01f)); + } + } else { + // allow pilot position input to come directly from an RC_Channel + if (roll_rc_in && (rc_ch(roll_rc_in))) { + _angle_ef_target_rad.x = angle_input_rad(rc_ch(roll_rc_in), _state._roll_angle_min, _state._roll_angle_max); + } + if (tilt_rc_in && (rc_ch(tilt_rc_in))) { + _angle_ef_target_rad.y = angle_input_rad(rc_ch(tilt_rc_in), _state._tilt_angle_min, _state._tilt_angle_max); + } + if (pan_rc_in && (rc_ch(pan_rc_in))) { + _angle_ef_target_rad.z = angle_input_rad(rc_ch(pan_rc_in), _state._pan_angle_min, _state._pan_angle_max); + } + } +} + +// returns the angle (degrees*100) that the RC_Channel input is receiving +int32_t AP_Mount_Backend::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max) +{ + return (rc->get_reverse() ? -1 : 1) * (rc->radio_in - rc->radio_min) * (int32_t)(angle_max - angle_min) / (rc->radio_max - rc->radio_min) + (rc->get_reverse() ? angle_max : angle_min); +} + +// returns the angle (radians) that the RC_Channel input is receiving +float AP_Mount_Backend::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max) +{ + return radians(angle_input(rc, angle_min, angle_max)*0.01f); +} + +// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target +void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan) +{ + float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f; + float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f; + float GPS_vector_z = (target.alt-_frontend._current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter). + float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. + + // initialise all angles to zero + angles_to_target_rad.zero(); + + // tilt calcs + if (calc_tilt) { + angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance); + } + + // pan calcs + if (calc_pan) { + // calc absolute heading and then onvert to vehicle relative yaw + angles_to_target_rad.z = wrap_PI(atan2f(GPS_vector_x, GPS_vector_y) - _frontend._ahrs.yaw); + } +}