|
@@ -1030,37 +1030,38 @@ void GCS_MAVLINK::update_send()
|
|
|
|
|
|
mav_motor_speed_back.Ltrack = 0;
|
|
mav_motor_speed_back.Ltrack = 0;
|
|
mav_motor_speed_back.Rtrack =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.motor7 =0;
|
|
mav_motor_speed_back.motor8 =0;
|
|
mav_motor_speed_back.motor8 =0;
|
|
|
|
|
|
rov_message.floodlight = sub.lights;
|
|
rov_message.floodlight = sub.lights;
|
|
//pressure_level 在各个模式中已经赋值
|
|
//pressure_level 在各个模式中已经赋值
|
|
rov_message.low_voltage = (float)sub._telemetry[0].voltage/10;
|
|
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.deep =fabsf(sub.barometer.get_altitude());//深度
|
|
//rov_message.temp = 0;
|
|
//rov_message.temp = 0;
|
|
rov_message.motor_block_flag = 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[6] = 0;
|
|
rov_message.motor_twine_flag[7] = 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;
|
|
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()) {
|
|
if (!hal.scheduler->in_delay_callback()) {
|