dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: more streams ported to new API

This commit is contained in:
Anton Babushkin 2014-07-28 13:53:45 +02:00
parent f1b55e578f
commit e087ec81c3
1 changed files with 268 additions and 254 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

522
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1254,260 +1254,274 @@ protected:
}; };
//class MavlinkStreamHILControls : public MavlinkStream class MavlinkStreamHILControls : public MavlinkStream
//{ {
//public: public:
// const char *get_name() const const char *get_name() const
// { {
// return MavlinkStreamHILControls::get_name_static(); return MavlinkStreamHILControls::get_name_static();
// } }
//
// static const char *get_name_static() static const char *get_name_static()
// { {
// return "HIL_CONTROLS"; return "HIL_CONTROLS";
// } }
//
// uint8_t get_id() uint8_t get_id()
// { {
// return MAVLINK_MSG_ID_HIL_CONTROLS; return MAVLINK_MSG_ID_HIL_CONTROLS;
// } }
//
// static MavlinkStream *new_instance(Mavlink *mavlink) static MavlinkStream *new_instance(Mavlink *mavlink)
// { {
// return new MavlinkStreamHILControls(); return new MavlinkStreamHILControls(mavlink);
// } }
//
//private: unsigned get_size()
// MavlinkOrbSubscription *status_sub; {
// uint64_t status_time; return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
// }
// MavlinkOrbSubscription *pos_sp_triplet_sub;
// uint64_t pos_sp_triplet_time; private:
// MavlinkOrbSubscription *_status_sub;
// MavlinkOrbSubscription *act_sub; uint64_t _status_time;
// uint64_t act_time;
// MavlinkOrbSubscription *_pos_sp_triplet_sub;
// /* do not allow top copying this class */ uint64_t _pos_sp_triplet_time;
// MavlinkStreamHILControls(MavlinkStreamHILControls &);
// MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); MavlinkOrbSubscription *_act_sub;
// uint64_t _act_time;
//protected:
// explicit MavlinkStreamHILControls() : MavlinkStream(mavlink), /* do not allow top copying this class */
// status_sub(nullptr), MavlinkStreamHILControls(MavlinkStreamHILControls &);
// status_time(0), MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &);
// pos_sp_triplet_sub(nullptr),
// pos_sp_triplet_time(0), protected:
// act_sub(nullptr), explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink),
// act_time(0) _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
// {} _status_time(0),
// _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
// void subscribe() _pos_sp_triplet_time(0),
// { _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))),
// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); _act_time(0)
// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); {}
// act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
// } void send(const hrt_abstime t)
// {
// void send(const hrt_abstime t) struct vehicle_status_s status;
// { struct position_setpoint_triplet_s pos_sp_triplet;
// struct vehicle_status_s status; struct actuator_outputs_s act;
// struct position_setpoint_triplet_s pos_sp_triplet;
// struct actuator_outputs_s act; bool updated = _act_sub->update(&_act_time, &act);
// updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet);
// bool updated = act_sub->update(&act_time, &act); updated |= _status_sub->update(&_status_time, &status);
// updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
// updated |= status_sub->update(&status_time, &status); if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
// /* translate the current syste state to mavlink state and mode */
// if (updated && (status.arming_state == ARMING_STATE_ARMED)) { uint8_t mavlink_state;
// /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_base_mode;
// uint8_t mavlink_state; uint32_t mavlink_custom_mode;
// uint8_t mavlink_base_mode; get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
// uint32_t mavlink_custom_mode;
// get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); float out[8];
//
// float out[8]; const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
//
// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; /* scale outputs depending on system type */
// if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
// /* scale outputs depending on system type */ mavlink_system.type == MAV_TYPE_HEXAROTOR ||
// if (mavlink_system.type == MAV_TYPE_QUADROTOR || mavlink_system.type == MAV_TYPE_OCTOROTOR) {
// mavlink_system.type == MAV_TYPE_HEXAROTOR || /* multirotors: set number of rotor outputs depending on type */
// mavlink_system.type == MAV_TYPE_OCTOROTOR) {
// /* multirotors: set number of rotor outputs depending on type */ unsigned n;
//
// unsigned n; switch (mavlink_system.type) {
// case MAV_TYPE_QUADROTOR:
// switch (mavlink_system.type) { n = 4;
// case MAV_TYPE_QUADROTOR: break;
// n = 4;
// break; case MAV_TYPE_HEXAROTOR:
// n = 6;
// case MAV_TYPE_HEXAROTOR: break;
// n = 6;
// break; default:
// n = 8;
// default: break;
// n = 8; }
// break;
// } for (unsigned i = 0; i < 8; i++) {
// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
// for (unsigned i = 0; i < 8; i++) { if (i < n) {
// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { /* scale PWM out 900..2100 us to 0..1 for rotors */
// if (i < n) { out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
// /* scale PWM out 900..2100 us to 0..1 for rotors */
// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } else {
// /* scale PWM out 900..2100 us to -1..1 for other channels */
// } else { out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
// /* scale PWM out 900..2100 us to -1..1 for other channels */ }
// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
// } } else {
// /* send 0 when disarmed */
// } else { out[i] = 0.0f;
// /* send 0 when disarmed */ }
// out[i] = 0.0f; }
// }
// } } else {
// /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
// } else {
// /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) {
// if (i != 3) {
// for (unsigned i = 0; i < 8; i++) { /* scale PWM out 900..2100 us to -1..1 for normal channels */
// if (i != 3) { out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
// /* scale PWM out 900..2100 us to -1..1 for normal channels */
// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } else {
// /* scale PWM out 900..2100 us to 0..1 for throttle */
// } else { out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
// /* scale PWM out 900..2100 us to 0..1 for throttle */ }
// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
// } }
// }
// }
// } mavlink_hil_controls_t msg;
//
// mavlink_msg_hil_controls_send(_channel, msg.time_usec = hrt_absolute_time();
// hrt_absolute_time(), msg.roll_ailerons = out[0];
// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], msg.pitch_elevator = out[1];
// mavlink_base_mode, msg.yaw_rudder = out[2];
// 0); msg.throttle = out[3];
// } msg.aux1 = out[4];
// } msg.aux2 = out[5];
//}; msg.aux3 = out[6];
// msg.aux4 = out[7];
// msg.mode = mavlink_base_mode;
//class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream msg.nav_mode = 0;
//{
//public: _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg);
// const char *get_name() const }
// { }
// return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); };
// }
//
// static const char *get_name_static() class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
// { {
// return "GLOBAL_POSITION_SETPOINT_INT"; public:
// } const char *get_name() const
// {
// uint8_t get_id() return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
// { }
// return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
// } static const char *get_name_static()
// {
// static MavlinkStream *new_instance(Mavlink *mavlink) return "GLOBAL_POSITION_SETPOINT_INT";
// { }
// return new MavlinkStreamGlobalPositionSetpointInt();
// } uint8_t get_id()
// {
//private: return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
// MavlinkOrbSubscription *pos_sp_triplet_sub; }
//
// /* do not allow top copying this class */ static MavlinkStream *new_instance(Mavlink *mavlink)
// MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); {
// MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); return new MavlinkStreamGlobalPositionSetpointInt(mavlink);
// }
//protected:
// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(mavlink), unsigned get_size()
// pos_sp_triplet_sub(nullptr) {
// {} return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
// }
// void subscribe()
// { private:
// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); MavlinkOrbSubscription *_pos_sp_triplet_sub;
// }
// /* do not allow top copying this class */
// void send(const hrt_abstime t) MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &);
// { MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &);
// struct position_setpoint_triplet_s pos_sp_triplet;
// protected:
// if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink),
// mavlink_msg_global_position_setpoint_int_send(_channel, _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
// MAV_FRAME_GLOBAL, {}
// (int32_t)(pos_sp_triplet.current.lat * 1e7),
// (int32_t)(pos_sp_triplet.current.lon * 1e7), void send(const hrt_abstime t)
// (int32_t)(pos_sp_triplet.current.alt * 1000), {
// (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); struct position_setpoint_triplet_s pos_sp_triplet;
// }
// } if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
//}; mavlink_global_position_setpoint_int_t msg;
//
// msg.coordinate_frame = MAV_FRAME_GLOBAL;
//class MavlinkStreamLocalPositionSetpoint : public MavlinkStream msg.latitude = pos_sp_triplet.current.lat * 1e7;
//{ msg.longitude = pos_sp_triplet.current.lon * 1e7;
//public: msg.altitude = pos_sp_triplet.current.alt * 1000;
// const char *get_name() const msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f;
// {
// return MavlinkStreamLocalPositionSetpoint::get_name_static(); _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg);
// } }
// }
// static const char *get_name_static() };
// {
// return "LOCAL_POSITION_SETPOINT";
// } class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
// {
// uint8_t get_id() public:
// { const char *get_name() const
// return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; {
// } return MavlinkStreamLocalPositionSetpoint::get_name_static();
// }
// static MavlinkStream *new_instance(Mavlink *mavlink)
// { static const char *get_name_static()
// return new MavlinkStreamLocalPositionSetpoint(); {
// } return "LOCAL_POSITION_SETPOINT";
// }
//private:
// MavlinkOrbSubscription *pos_sp_sub; uint8_t get_id()
// uint64_t pos_sp_time; {
// return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
// /* do not allow top copying this class */ }
// MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &);
// MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); static MavlinkStream *new_instance(Mavlink *mavlink)
// {
//protected: return new MavlinkStreamLocalPositionSetpoint(mavlink);
// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(mavlink), }
// pos_sp_sub(nullptr),
// pos_sp_time(0) unsigned get_size()
// {} {
// return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
// void subscribe() }
// {
// pos_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); private:
// } MavlinkOrbSubscription *_pos_sp_sub;
// uint64_t _pos_sp_time;
// void send(const hrt_abstime t)
// { /* do not allow top copying this class */
// struct vehicle_local_position_setpoint_s pos_sp; MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &);
// MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &);
// if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
// mavlink_msg_local_position_setpoint_send(_channel, protected:
// MAV_FRAME_LOCAL_NED, explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
// pos_sp.x, _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))),
// pos_sp.y, _pos_sp_time(0)
// pos_sp.z, {}
// pos_sp.yaw);
// } void send(const hrt_abstime t)
// } {
//}; struct vehicle_local_position_setpoint_s pos_sp;
//
// if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
mavlink_local_position_setpoint_t msg;
msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
msg.x = pos_sp.x;
msg.y = pos_sp.y;
msg.z = pos_sp.z;
msg.yaw = pos_sp.yaw;
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg);
}
}
};
//class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream //class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
//{ //{
//public: //public: