/** Set Mode */ void MavSerialPort::set_mode_disarm(){ setButtons(BUTTON_ALLZERO); //send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_PREFLIGHT,0,0,0,0,0,0); send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_MANUAL_DISARMED,0,0,0,0,0,0); //preflight mode is all zeros qDebug() << "MODE_MODE_MANUAL_DISARMED"; }
//16 MAV_CMD_NAV_WAYPOINT void MavSerialPort::cmd_nav_waypoint(){ // Navigate to MISSION. float f1 = 15; //Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing) float f2 = 0; //Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached) float f3 = 0; //0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. float f4 = 0; //Desired yaw angle at MISSION (rotary wing) float f5 = 22.33; // Latitude22.3371147,114.268395 float f6 = 114.26; // Longitude float f7 = 5; // Altitude //confirmation 0 //go back later send_command_long(MAV_CMD_NAV_WAYPOINT, 0, f1, f2, f3, f4, f5, f6, f7); }
//176 MAV_CMD_DO_SET_MODE void MavSerialPort::cmd_do_set_mode(){//MAV_MODE mode){ /* Set system mode.*/ /** MAV_MODE_PREFLIGHT=0, // System is not ready to fly, booting, calibrating, etc. No flag is set. MAV_MODE_MANUAL_DISARMED=64, // System is allowed to be active, under manual (RC) control, no stabilization MAV_MODE_TEST_DISARMED=66, // UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. MAV_MODE_STABILIZE_DISARMED=80, // System is allowed to be active, under assisted RC control. MAV_MODE_GUIDED_DISARMED=88, // System is allowed to be active, under autonomous control, manual setpoint MAV_MODE_AUTO_DISARMED=92, // System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) MAV_MODE_MANUAL_ARMED=192, // System is allowed to be active, under manual (RC) control, no stabilization MAV_MODE_TEST_ARMED=194, // UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. MAV_MODE_STABILIZE_ARMED=208, // System is allowed to be active, under assisted RC control. MAV_MODE_GUIDED_ARMED=216, // System is allowed to be active, under autonomous control, manual setpoint MAV_MODE_AUTO_ARMED=220, // System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by MISSIONs) MAV_MODE_ENUM_END=221 */ // Mode, as defined by ENUM MAV_MODE, will be ignored if custom main mode is set // Custom mode - this is system specific, please refer to the individual autopilot specifications for details. // Empty| Empty| Empty| Empty| Empty // leave the confirmation as 0 send_command_long(MAV_CMD_DO_SET_MODE,0,194,0,0,0,0,0,0); qDebug() << "send do set mode"; }
void MavSerialPort::set_mode_auto_loiter(){ send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_PREFLIGHT,0,0,0,0,0,0); qDebug() << "MODE_AUTO_ARMED"; }
void MavSerialPort::set_mode_auto_mission(){ send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_AUTO_ARMED,0,0,0,0,0,0); qDebug() << "MODE_AUTO_ARMED"; }
void MavSerialPort::set_mode_assist_posctl(){ send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_STABILIZE_ARMED,0,0,0,0,0,0); qDebug() << "MODE_STABILIZE_ARMED"; }
void MavSerialPort::set_mode_manual(){ send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_MANUAL_ARMED,0,0,0,0,0,0); qDebug() << "MODE_MANUAL_ARMED"; }
void MavSerialPort::set_mode_return(){ send_command_long(MAV_CMD_NAV_RETURN_TO_LAUNCH,0,0,0,0,0,0,0,0); qDebug() << "SEND RETURN TO LAUNCH"; }
/** Set Mode */ void MavSerialPort::set_mode_disarm(){ send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_PREFLIGHT,0,0,0,0,0,0); qDebug() << "MODE_PREFLIGHT"; }
void MavSerialPort::set_mode_arm(){ send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_FLAG_SAFETY_ARMED,0,0,0,0,0,0); qDebug() << "MODE_FLAG_SAFETY_ARMED"; }
void MavSerialPort::set_mode_auto_delivery(){ send_command_long(MAV_CMD_DO_SET_MODE,0, MAV_MODE_FLAG_SAFETY_ARMED + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PX4_CUSTOM_MAIN_MODE_AUTO_DELIVERY,0,0,0,0,0); qDebug() << "PX4_CUSTOM_MAIN_MODE_DELIVERY"; }
void MavSerialPort::set_mode_auto_loiter(){ send_command_long(MAV_CMD_DO_SET_MODE,0, MAV_MODE_FLAG_SAFETY_ARMED + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PX4_CUSTOM_MAIN_MODE_AUTO_LOITER, 0,0,0,0,0); qDebug() << "PX4_CUSTOM_MAIN_MODE_AUTO_LOITER"; }
void MavSerialPort::set_mode_assist_posctl(){ send_command_long(MAV_CMD_DO_SET_MODE,0, MAV_MODE_FLAG_SAFETY_ARMED + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PX4_CUSTOM_MAIN_MODE_POSCTL,0,0,0,0,0); qDebug() << "PX4_CUSTOM_MAIN_MODE_POSCTL"; }
void MavSerialPort::set_mode_manual(){ send_command_long(MAV_CMD_DO_SET_MODE,0, MAV_MODE_FLAG_SAFETY_ARMED + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PX4_CUSTOM_MAIN_MODE_MANUAL,0,0,0,0,0); qDebug() << "PX4_CUSTOM_MAIN_MODE_MANUAL"; }
void MavSerialPort::set_mode_return(){ send_command_long(MAV_CMD_DO_SET_MODE,0, MAV_MODE_FLAG_SAFETY_ARMED + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,PX4_CUSTOM_MAIN_MODE_AUTO_RTL, 0,0,0,0,0); qDebug() << "PX4_CUSTOM_SUB_MODE_AUTO_RTL"; }
void MavSerialPort::set_mode_arm(){ setButtons(BUTTON_ALLZERO); send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_MANUAL_ARMED,0,0,0,0,0,0); qDebug() << "MODE_MODE_MANUAL_ARMED"; }
/** Set Mode */ void MavSerialPort::set_mode_offboard(){ send_command_long(MAV_CMD_DO_SET_MODE,0,MAV_MODE_FLAG_SAFETY_ARMED + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PX4_CUSTOM_MAIN_MODE_OFFBOARD,0,0,0,0,0); }