Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Mount_Servo: check_servo_map every 3sec

This commit is contained in:
Randy Mackay 2015-01-12 20:51:56 +09:00 committed by Andrew Tridgell
parent cb5a122dab
commit 2ed4ca409c
2 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -2,6 +2,8 @@
#include <AP_Mount_Servo.h>
extern const AP_HAL::HAL& hal;
// init - performs any required initialisation for this instance
void AP_Mount_Servo::init()
{
@ -27,6 +29,13 @@ void AP_Mount_Servo::update()
{
static bool mount_open = 0; // 0 is closed
// check servo map every three seconds to allow users to modify parameters
uint32_t now = hal.scheduler->millis();
if (now - _last_check_servo_map_ms > 3000) {
check_servo_map();
_last_check_servo_map_ms = now;
}
switch(_frontend.get_mode(_instance)) {
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT:
@ -139,12 +148,11 @@ void AP_Mount_Servo::set_roi_target(const struct Location &target_loc)
// should be called periodically (i.e. 1hz or less)
void AP_Mount_Servo::check_servo_map()
{
_flags.roll_control = RC_Channel_aux::function_assigned(_roll_idx);
_flags.tilt_control = RC_Channel_aux::function_assigned(_tilt_idx);
_flags.pan_control = RC_Channel_aux::function_assigned(_pan_idx);
_flags.roll_control = RC_Channel_aux::function_assigned(_roll_idx);
_flags.tilt_control = RC_Channel_aux::function_assigned(_tilt_idx);
_flags.pan_control = RC_Channel_aux::function_assigned(_pan_idx);
}
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
void AP_Mount_Servo::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad)
{

5
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -24,7 +24,8 @@ public:
_roll_idx(RC_Channel_aux::k_none),
_tilt_idx(RC_Channel_aux::k_none),
_pan_idx(RC_Channel_aux::k_none),
_open_idx(RC_Channel_aux::k_none)
_open_idx(RC_Channel_aux::k_none),
_last_check_servo_map_ms(0)
{
// init to no axis being controlled
_flags.roll_control = false;
@ -93,6 +94,8 @@ private:
Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and pan angles in radians
Vector3f _angle_bf_output_deg; // final body frame output angle in degres
uint32_t _last_check_servo_map_ms; // system time of latest call to check_servo_map function
};
#endif // __AP_MOUNT_SERVO_H__