Skip to content

Commit

Permalink
AP_Mount: gate gimbal att msgs with gimbal presence detect
Browse files Browse the repository at this point in the history
  • Loading branch information
@Gussy @georgehines
Gussy authored and georgehines committed Aug 12, 2016
1 parent 100a198 commit a1c7f24
Show file tree
Hide file tree
Showing 8 changed files with 108 additions and 37 deletions.
8 changes: 1 addition & 7 deletions ArduCopter/GCS_Mavlink.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1066,14 +1066,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)

case MAVLINK_MSG_ID_R10C_GIMBAL_REPORT:
{
static int32_t last_received;
#if MOUNT==ENABLED
mavlink_r10c_gimbal_report_t r10c_report;
mavlink_msg_r10c_gimbal_report_decode(msg, &r10c_report);
float pitch_ref = (float)r10c_report.pitch_ref/(1<<22);
float roll_out = (float)r10c_report.roll_out/(1<<22);
float pitch_out = (float)r10c_report.pitch_out/(1<<22);
DataFlash.Log_Write_R10CGimbal(pitch_ref, roll_out, pitch_out, r10c_report.roll_pwm, r10c_report.pitch_pwm);
handle_r10c_gimbal_report(camera_mount, msg);
#endif
break;
}
Expand Down
10 changes: 10 additions & 0 deletions libraries/AP_Mount/AP_Mount.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -666,6 +666,16 @@ void AP_Mount::handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_messa
}
}

// pass an R10C_GIMBAL_REPORT message to the backend
void AP_Mount::handle_r10c_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
{
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != NULL) {
_backends[instance]->handle_r10c_gimbal_report(chan, msg);
}
}
}

// send a GIMBAL_REPORT message to the GCS
void AP_Mount::send_gimbal_report(mavlink_channel_t chan)
{
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Mount/AP_Mount.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,7 @@ class AP_Mount
// handle a GIMBAL_REPORT message
void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
void handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg);
void handle_r10c_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);

// send a GIMBAL_REPORT message to GCS
void send_gimbal_report(mavlink_channel_t chan);
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Mount/AP_Mount_Backend.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ class AP_Mount_Backend
// handle a GIMBAL_REPORT message
virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) {}
virtual void handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg) {}
virtual void handle_r10c_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) {}

// send a GIMBAL_REPORT message to the GCS
virtual void send_gimbal_report(mavlink_channel_t chan) {}
Expand Down
101 changes: 71 additions & 30 deletions libraries/AP_Mount/AP_Mount_R10C.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,17 @@ void AP_Mount_R10C::init(const AP_SerialManager& serial_manager)
check_servo_map();
}

bool AP_Mount_R10C::present()
{
if (_r10c_state != R10C_GIMBAL_STATE_NOT_PRESENT && hal.scheduler->millis()-_last_report_msg_ms > 3000) {
// gimbal went away
_r10c_state = R10C_GIMBAL_STATE_NOT_PRESENT;
return false;
}

return _r10c_state != R10C_GIMBAL_STATE_NOT_PRESENT;
}

// update mount position - should be called periodically
void AP_Mount_R10C::update()
{
Expand Down Expand Up @@ -85,35 +96,37 @@ void AP_Mount_R10C::update()

void AP_Mount_R10C::gmb_att_update()
{
const AP_InertialSensor &ins = _frontend._ahrs.get_ins();
const Vector3f &gyro = ins.get_gyro();
mavlink_channel_t chan = MAVLINK_COMM_2;


Quaternion quat;
_frontend._ahrs.get_NavEKF_const().getQuaternion(quat);
Vector3f euler312;
quat.to_vector312(euler312.x,euler312.y,euler312.z);
//Quaternion quat;
//quat.from_euler(_frontend._ahrs.roll,_frontend._ahrs.pitch,0);
//if(degrees(_frontend._ahrs.roll) > 150 || degrees(_frontend._ahrs.roll) < -150) {
// return;
//}

if (get_mode() != MAV_MOUNT_MODE_NEUTRAL) {
mavlink_msg_r10c_gimbal_update_send(
chan,
euler312.x, //these will be normalised again on gimbal
euler312.y,
euler312.z,
_angle_bf_output_deg.y*100);
} else {
mavlink_msg_r10c_gimbal_update_send(
chan,
0,
0,
0,
_angle_bf_output_deg.y*100);
if (present()) {
const AP_InertialSensor &ins = _frontend._ahrs.get_ins();
const Vector3f &gyro = ins.get_gyro();
mavlink_channel_t chan = MAVLINK_COMM_2;


Quaternion quat;
_frontend._ahrs.get_NavEKF_const().getQuaternion(quat);
Vector3f euler312;
quat.to_vector312(euler312.x,euler312.y,euler312.z);
//Quaternion quat;
//quat.from_euler(_frontend._ahrs.roll,_frontend._ahrs.pitch,0);
//if(degrees(_frontend._ahrs.roll) > 150 || degrees(_frontend._ahrs.roll) < -150) {
// return;
//}

if (get_mode() != MAV_MOUNT_MODE_NEUTRAL) {
mavlink_msg_r10c_gimbal_update_send(
chan,
euler312.x, //these will be normalised again on gimbal
euler312.y,
euler312.z,
_angle_bf_output_deg.y*100);
} else {
mavlink_msg_r10c_gimbal_update_send(
chan,
0,
0,
0,
_angle_bf_output_deg.y*100);
}
}
}
// set_mode - sets mount's mode
Expand All @@ -137,7 +150,9 @@ void AP_Mount_R10C::check_servo_map()
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
void AP_Mount_R10C::status_msg(mavlink_channel_t chan)
{
mavlink_msg_mount_status_send(chan, 0, 0, _angle_bf_output_deg.y*100, _angle_bf_output_deg.x*100, _angle_bf_output_deg.z*100);
if (present()) {
mavlink_msg_mount_status_send(chan, 0, 0, _angle_bf_output_deg.y*100, _angle_bf_output_deg.x*100, _angle_bf_output_deg.z*100);
}
}

// stabilize - stabilizes the mount relative to the Earth's frame
Expand Down Expand Up @@ -187,3 +202,29 @@ void AP_Mount_R10C::move_servo(uint8_t function_idx, int16_t angle, int16_t angl

mavlink_msg_command_long_send(chan,mavlink_system.sysid,MAV_COMP_ID_R10C_GIMBAL,MAV_CMD_DO_SET_SERVO,0,function_idx - RC_Channel_aux::k_mount_pan,servo_out*10,0,0,0,0,0);
}

void AP_Mount_R10C::handle_r10c_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) {
mavlink_r10c_gimbal_report_t r10c_report;
mavlink_msg_r10c_gimbal_report_decode(msg, &r10c_report);
uint32_t tnow_ms = hal.scheduler->millis();
_last_report_msg_ms = tnow_ms;

if (r10c_report.target_system != 1) {
_r10c_state = R10C_GIMBAL_STATE_NOT_PRESENT;
}

switch(_r10c_state) {
case R10C_GIMBAL_STATE_NOT_PRESENT:
// gimbal was just connected or we just rebooted
_r10c_state = R10C_GIMBAL_STATE_PRESENT_RUNNING;
break;

case R10C_GIMBAL_STATE_PRESENT_RUNNING:
break;
}

float pitch_ref = (float)r10c_report.pitch_ref/(1<<22);
float roll_out = (float)r10c_report.roll_out/(1<<22);
float pitch_out = (float)r10c_report.pitch_out/(1<<22);
_frontend._dataflash->Log_Write_R10CGimbal(pitch_ref, roll_out, pitch_out, r10c_report.roll_pwm, r10c_report.pitch_pwm);
}
14 changes: 14 additions & 0 deletions libraries/AP_Mount/AP_Mount_R10C.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,18 @@
#include <RC_Channel_aux.h>
#include "AP_Mount_Backend.h"

enum r10c_gimbal_state_t {
R10C_GIMBAL_STATE_NOT_PRESENT = 0,
R10C_GIMBAL_STATE_PRESENT_RUNNING
};

class AP_Mount_R10C : public AP_Mount_Backend
{
public:
// Constructor
AP_Mount_R10C(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance):
AP_Mount_Backend(frontend, state, instance),
_r10c_state(R10C_GIMBAL_STATE_NOT_PRESENT),
_roll_idx(RC_Channel_aux::k_none),
_tilt_idx(RC_Channel_aux::k_none),
_pan_idx(RC_Channel_aux::k_none),
Expand Down Expand Up @@ -50,7 +56,13 @@ class AP_Mount_R10C : public AP_Mount_Backend
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
virtual void status_msg(mavlink_channel_t chan);

bool present();

// handle an R10C_GIMBAL_REPORT message
void handle_r10c_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);

private:
r10c_gimbal_state_t _r10c_state;

// flags structure
struct {
Expand Down Expand Up @@ -81,6 +93,8 @@ class AP_Mount_R10C : public AP_Mount_Backend
Vector3f _angle_bf_output_deg; // final body frame output angle in degrees

uint32_t _last_check_servo_map_ms; // system time of latest call to check_servo_map function

uint32_t _last_report_msg_ms;
};

#endif // __AP_Mount_R10C_H__
1 change: 1 addition & 0 deletions libraries/GCS_MAVLink/GCS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -307,6 +307,7 @@ class GCS_MAVLINK
void handle_set_mode(mavlink_message_t* msg, bool (*set_mode)(uint8_t mode));
void handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const;
void handle_gimbal_torque_report(AP_Mount &mount, mavlink_message_t *msg) const;
void handle_r10c_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const;

void handle_gps_inject(const mavlink_message_t *msg, AP_GPS &gps);

Expand Down
9 changes: 9 additions & 0 deletions libraries/GCS_MAVLink/GCS_Common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -414,6 +414,15 @@ void GCS_MAVLINK::handle_gimbal_torque_report(AP_Mount &mount, mavlink_message_t
{
mount.handle_gimbal_torque_report(chan,msg);
}

/*
handle r10c gimbal report
*/
void GCS_MAVLINK::handle_r10c_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const
{
mount.handle_r10c_gimbal_report(chan,msg);
}

/*
return true if a channel has flow control
*/
Expand Down

0 comments on commit a1c7f24

Please sign in to comment.