Line data    Source code

       1             : #include "AP_Mount_config.h"
       2             : 
       3             : #if HAL_MOUNT_SCRIPTING_ENABLED
       4             : 
       5             : #include "AP_Mount_Scripting.h"
       6             : 
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <AP_AHRS/AP_AHRS.h>
       9             : #include <GCS_MAVLink/GCS.h>
      10             : 
      11             : extern const AP_HAL::HAL& hal;
      12             : 
      13             : #define AP_MOUNT_SCRIPTING_TIMEOUT_MS    1000   // scripting mount becomes unhealthy after 1sec with no updates
      14             : 
      15             : #define AP_MOUNT_SCRIPTING_DEBUG 0
      16             : #define debug(fmt, args ...) do { if (AP_MOUNT_SCRIPTING_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scripting: " fmt, ## args); } } while (0)
      17             : 
      18             : // update mount position - should be called periodically
      19       13176 : void AP_Mount_Scripting::update()
      20             : {
      21       13176 :     AP_Mount_Backend::update();
      22             : 
      23             :     // reset script target type so get_angle_target / get_rate_target return
      24             :     // false until send_target_to_gimbal() writes a fresh target this cycle
      25       13176 :     _script_target_type = ScriptTargetType::NONE;
      26             : 
      27       13176 :     update_mnt_target();
      28             : 
      29       13176 :     send_target_to_gimbal();
      30       13176 : }
      31             : 
      32             : // return true if healthy
      33          95 : bool AP_Mount_Scripting::healthy() const
      34             : {
      35             :     // healthy if scripting backend has updated actual angles recently
      36          95 :     return (AP_HAL::millis() - last_update_ms <= AP_MOUNT_SCRIPTING_TIMEOUT_MS);
      37             : }
      38             : 
      39             : // update mount's actual angles (to be called by script communicating with mount)
      40        2548 : void AP_Mount_Scripting::set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg)
      41             : {
      42        2548 :     last_update_ms = AP_HAL::millis();
      43        2548 :     current_angle_deg.x = roll_deg;
      44        2548 :     current_angle_deg.y = pitch_deg;
      45        2548 :     current_angle_deg.z = yaw_bf_deg;
      46        2548 : }
      47             : 
      48             : // called by send_target_to_gimbal() with the angle target for this cycle.
      49             : // Store it so get_angle_target() can return it to the Lua script.
      50       13176 : void AP_Mount_Scripting::send_target_angles(const MountAngleTarget &angle_rad)
      51             : {
      52       13176 :     _angle_target = angle_rad;
      53       13176 :     _script_target_type = ScriptTargetType::ANGLE;
      54       13176 : }
      55             : 
      56             : // called by send_target_to_gimbal() with the rate target for this cycle.
      57             : // Store it so get_rate_target() can return it to the Lua script.
      58           0 : void AP_Mount_Scripting::send_target_rates(const MountRateTarget &rate_rads)
      59             : {
      60           0 :     _rate_target = rate_rads;
      61           0 :     _script_target_type = ScriptTargetType::RATE;
      62           0 : }
      63             : 
      64             : // get target angle in deg. returns true on success
      65        1195 : bool AP_Mount_Scripting::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
      66             : {
      67        1195 :     if (_script_target_type != ScriptTargetType::ANGLE) {
      68           2 :         return false;
      69             :     }
      70        1193 :     roll_deg = degrees(_angle_target.roll);
      71        1193 :     pitch_deg = degrees(_angle_target.pitch);
      72        1193 :     yaw_deg = degrees(_angle_target.yaw);
      73        1193 :     yaw_is_earth_frame = _angle_target.yaw_is_ef;
      74        1193 :     return true;
      75             : }
      76             : 
      77             : // get target rate in deg/sec. returns true on success
      78           0 : bool AP_Mount_Scripting::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
      79             : {
      80           0 :     if (_script_target_type != ScriptTargetType::RATE) {
      81           0 :         return false;
      82             :     }
      83           0 :     roll_degs = degrees(_rate_target.roll);
      84           0 :     pitch_degs = degrees(_rate_target.pitch);
      85           0 :     yaw_degs = degrees(_rate_target.yaw);
      86           0 :     yaw_is_earth_frame = _rate_target.yaw_is_ef;
      87           0 :     return true;
      88             : }
      89             : 
      90             : // get attitude as a quaternion.  returns true on success
      91        3932 : bool AP_Mount_Scripting::get_attitude_quaternion(Quaternion& att_quat)
      92             : {
      93             :     // construct quaternion
      94        3932 :     att_quat.from_euler(radians(current_angle_deg.x), radians(current_angle_deg.y), radians(current_angle_deg.z));
      95        3932 :     return true;
      96             : }
      97             : 
      98             : #endif // HAL_MOUNT_SCRIPTING_ENABLED