void Quadcopter::sendRCMessage(
unsigned channelOne,
unsigned channelTwo,
unsigned channelThree,
unsigned channelFour,
unsigned channelFive,
unsigned channelSix,
unsigned channelSeven,
unsigned channelEight)
{
if (!failsafe)
{
mavlink_msg_rc_channels_override_pack(
SYSTEMID,
COMPONENTID,
&message,
TARGET_SYSTEMID,
TARGET_COMPONENTID,
channelOne,
channelTwo,
channelThree,
channelFour,
channelFive,
channelSix,
channelSeven,
channelEight);
communicator.enqueueMessage(message);
lastRCSent = std::chrono::system_clock::now();
}
}
int main(int argc, char* argv[])
{
initialize_defaults();
open_serial();
uart_name = "/dev/ttyAMA0";
baudrate = 57600;
uint16_t u = 0xFFFF;
sleep(1);
mavlink_rc_channels_override_t rco;
rco.chan1_raw = 1500;
rco.chan2_raw = 1500;
rco.chan3_raw = 1500;
rco.chan4_raw = 2000;
rco.chan5_raw = 1500;
rco.chan6_raw = 1500;
rco.chan7_raw = 1500;
rco.chan8_raw = 1500;
rco.target_system =0;
rco.target_component = 0;
mavlink_message_t message;
/*int i;*/
/*for(i = 0; i<1; i++){*/
mavlink_msg_rc_channels_override_encode(0xff,0,&message, &rco);
int len = write_message(message);
/*}*/
sleep(1);
mavlink_msg_rc_channels_override_pack(0xff,0, &message,0,0,u,u,u,1000,u,u,u,u );
len = write_message(message);
sleep(1);
mavlink_msg_rc_channels_override_pack(0xff,0, &message,0,0,u,u,u,1500,u,u,u,u );
len = write_message(message);
sleep(1);
mavlink_msg_rc_channels_override_pack(0xff,0, &message,0,0,0,0,0,0,0,0,0,0 );
len = write_message(message);
sleep(1);
close_serial();
sleep(1);
pthread_mutex_destroy(&lock);
return 0;
}
// ----------------------------------------------------------------------------
// Overwriting the RC channels
// ----------------------------------------------------------------------------
void Pixhawk_Interface::
write_RC_command(mavlink_message_t msg, int roll, int yaw)
{
// MSG: #70 RC_CHANNELS_OVERRIDE
// target ID pitch throttle ch5 6 7 8
mavlink_msg_rc_channels_override_pack(id.sysid, id.compid, &msg, 1, 1, roll, 0, 0, yaw, 0, 0, 0, 0);
serial_port->write_message(msg);
//debug print
//printf("RC override sys ID %d, comp ID %d\n", id.sysid, id.compid);
}// end write RC command
void mavlink_rc(double x, double y, bool give_up){
if( give_up ){ // 0 means give up
roll = 0;
pitch = 0;
}else{
roll = pid.rollC + x*57.3f*10.0f;
pitch = pid.pitchC + y*57.3f*10.0f;
}
// Pack the message
mavlink_msg_rc_channels_override_pack(0xff, 0xbe, &tx_msg, 0x01, 0x01, roll, pitch, 0, 0, 0, 0, 0, 0);
// Copy the message to the send buffer
len = mavlink_msg_to_send_buffer(buf, &tx_msg);
if( Uart1Ready ){ //
Uart1Ready = false;
HAL_UART_Transmit_IT(&Uart1Handle, buf, len);
}/*else{
HAL_UART_DeInit(&Uart1Handle);
uart1Init();
Uart1Ready = true;
}*/
}
/* RC Override Sender */
bool MAVLink::sendRCOverride(uint16_t c1, uint16_t c2, uint16_t c3, uint16_t c4, uint16_t c5, uint16_t c6, uint16_t c7, uint16_t c8) {
mavlink_message_t msg;
mavlink_msg_rc_channels_override_pack(mySystemId,myComponentId,&msg,targetSystemId,targetComponentId,c1,c2,c3,c4,c5,c6,c7,c8);
return sendMessage(msg);
}
