更改了灯光 和mavlink协议

ArduSub/UserCode.cpp

@@ -87,20 +87,17 @@ void Sub::USBL_PowerSwitch(void)
 	RC_Channel* chan = RC_Channels::rc_channel(8);
 	uint16_t min = chan->get_radio_min();
 	uint16_t max = chan->get_radio_max();
-	static uint8_t last_light = 0;
 
-		if (rov_control.floodlight == 1  && motors.armed())
-	   {
-	   		last_light = 1;
-			lights1 = constrain_float(max, min, max);
-	   }
-		else if(!motors.armed()){
-			last_light = 0;
-			lights1 = constrain_float(min, min, max);	
-	   }else if(last_light == 1){
-	   		last_light = 0;
+	   	if(!motors.armed()){	
 	   		lights1 = constrain_float(min, min, max);
 	   	}
+	   if (lights1>1000)
+		  {
+		   lights = 1;
+		  	}else{
+	   		lights =0;
+		}
+	   
 		RC_Channels::set_override(8, lights1, tnow);
 		if(motors.armed() && (rov_control.USBL_flag==1 || usblpoweroff == 1))
 		{

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1030,37 +1030,38 @@ void GCS_MAVLINK::update_send()
 			
 			mav_motor_speed_back.Ltrack = 0;
 			mav_motor_speed_back.Rtrack =0;
-			mav_motor_speed_back.motor1 =(float)sub._telemetry[0].rpm/7;//uint32 rpm
-			mav_motor_speed_back.motor2 =(float)sub._telemetry[1].rpm/7;
-			mav_motor_speed_back.motor3 =(float)sub._telemetry[2].rpm/7;
-			mav_motor_speed_back.motor4 =(float)sub._telemetry[3].rpm/7;
-			mav_motor_speed_back.motor5 =(float)sub._telemetry[4].rpm/7;
-			mav_motor_speed_back.motor6 =(float)sub._telemetry[5].rpm/7;
+			mav_motor_speed_back.motor1 =(float)0.0;//sub._telemetry[0].rpm/7;//uint32 rpm
+			mav_motor_speed_back.motor2 =(float)0.0;//sub._telemetry[1].rpm/7;
+			mav_motor_speed_back.motor3 =(float)0.0;//sub._telemetry[2].rpm/7;
+			mav_motor_speed_back.motor4 =(float)0.0;//sub._telemetry[3].rpm/7;
+			mav_motor_speed_back.motor5 =(float)0.0;//sub._telemetry[4].rpm/7;
+			mav_motor_speed_back.motor6 =(float)0.0;//sub._telemetry[5].rpm/7;
 			mav_motor_speed_back.motor7 =0;
 			mav_motor_speed_back.motor8 =0;
 
 			rov_message.floodlight = sub.lights;
 			//pressure_level  在各个模式中已经赋值
 			rov_message.low_voltage = (float)sub._telemetry[0].voltage/10;
-			rov_message.high_voltage = (float)sub._telemetry[1].voltage/10;//sub._telemetry[0].voltage;//电压
+			rov_message.high_voltage = (float)sub._telemetry[1].voltage/10;
 			rov_message.deep =fabsf(sub.barometer.get_altitude());//深度
 			//rov_message.temp = 0;
 			rov_message.motor_block_flag =	0;
-			rov_message.motor_twine_flag[0] = 0;//(uint16_t)uavcan.thruster[0].error_count;//故障码？
-			rov_message.motor_twine_flag[1] = 0;//(uint16_t)uavcan.thruster[1].error_count;
-			rov_message.motor_twine_flag[2] = 0;//(uint16_t)uavcan.thruster[2].error_count;
-			rov_message.motor_twine_flag[3] = 0;//(uint16_t)uavcan.thruster[3].error_count;
-			rov_message.motor_twine_flag[4] = 0;//(uint16_t)uavcan.thruster[4].error_count;
-			rov_message.motor_twine_flag[5] = 0;//(uint16_t)uavcan.thruster[5].error_count;
+			rov_message.motor_twine_flag[0] = 0;
+			rov_message.motor_twine_flag[1] = 0;
+			rov_message.motor_twine_flag[2] = 0;
+			rov_message.motor_twine_flag[3] = 0;
+			rov_message.motor_twine_flag[4] = 0;
+			rov_message.motor_twine_flag[5] = 0;
 			rov_message.motor_twine_flag[6] = 0;
 			rov_message.motor_twine_flag[7] = 0;
-			rov_message.motor_power[0] = (int16_t)sub._telemetry[0].totalcurrent;//float current
-			rov_message.motor_power[1] = (int16_t)sub._telemetry[1].totalcurrent;
-			rov_message.motor_power[2] = (int16_t)sub._telemetry[2].totalcurrent;
-			rov_message.motor_power[3] = (int16_t)sub._telemetry[3].totalcurrent;
-			rov_message.motor_power[4] = (int16_t)sub._telemetry[4].totalcurrent;
-			rov_message.motor_power[5] = (int16_t)sub._telemetry[5].totalcurrent;
-			
+			rov_message.motor_power[0] = 0;
+			rov_message.motor_power[1] = 0;
+			rov_message.motor_power[2] = 0;
+			rov_message.motor_power[3] = 0;
+			rov_message.motor_power[4] = 0;
+			rov_message.motor_power[5] = 0;
+			rov_message.motor_power[6] = 0;
+			rov_message.motor_power[7] = 0;			
 
 
 			get_stm32_param.number = 0;
@@ -1088,32 +1089,7 @@ void GCS_MAVLINK::update_send()
 				
 			}
 			
-
-			/*const NavEKF2 &ekf2 = AP::ahrs_navekf().get_NavEKF2_const();
-								Matrix3f mat;
-								ekf2.getRotationBodyToNED(mat);
-
-			countper++;
-			if (countper>50)
-			{
-				countper = 0;
-				
-				mavlink_msg_rotation_matrix_array_send(chan,
-											   mat.c.x,mat.c.z,mat.c.y,
-											   mat.b.x,mat.b.z,mat.b.y, 
-											   mat.a.x,mat.a.z,mat.a.y);
-				mavlink_msg_rov_state_monitoring_send_struct(chan,&rov_message);
-							
-				mavlink_msg_set_slave_parameter_send_struct(chan,&get_stm32_param); 
-									
-				//mavlink_msg_motor_speed_send_struct(chan,&mav_motor_speed_back);
-				//mavlink_msg_hv_reg_get_send_struct(chan,&hv_reg_get);
-			}*/
-			
-
-			
-
-		  	
+	  	
 		
 
     if (!hal.scheduler->in_delay_callback()) {

modules/mavlink/message_definitions/v1.0/ardupilotmega.xml

@@ -1519,7 +1519,7 @@
       <field type="uint16_t" name="turn">Motor turning direction.</field>
       <field type="uint16_t" name="hvMotorMod">HV Motor Mod.</field>
       <field type="uint16_t[8]" name="motor_twine_flag">motors twine flag.</field>
-      <field type="uint16_t[6]" name="motor_power">motors twine flag.</field>
+      <field type="uint16_t[8]" name="motor_power">motors twine flag.</field>
     </message>
 	<message id="235" name="MOTOR_SPEED">
 	  <description>motor speed communication.</description>