/** 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);
}
