Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Fix MSP

Browse source
This commit is contained in:
breadoven 2022-08-24 15:44:19 +01:00
parent 7322fbedc8
commit 0598080dc7
2 changed files with 11 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/fc/fc_msp.c
View file

@ -1580,7 +1580,7 @@ static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
{ {
const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number
navWaypoint_t msp_wp; navWaypoint_t msp_wp;
getWaypoint(msp_wp_no + wpMissionStartIndex, &msp_wp); getWaypoint(msp_wp_no, &msp_wp);
sbufWriteU8(dst, msp_wp_no); // wp_no sbufWriteU8(dst, msp_wp_no); // wp_no
sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT) sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT)
sbufWriteU32(dst, msp_wp.lat); // lat sbufWriteU32(dst, msp_wp.lat); // lat
@ -3223,7 +3223,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
int processedRows = 16; int processedRows = 16;
while (bytesCount < 80) //whole response should be less 155 bytes at worst. while (bytesCount < 80) //whole response should be less 155 bytes at worst.
{ {
bool blink1; bool blink1;
uint16_t lastChar; uint16_t lastChar;
@ -3290,14 +3290,14 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
else if (count > 2 || cmd !=0 ) else if (count > 2 || cmd !=0 )
{ {
cmd |= count; //long command for blink/bank switch and symbol repeat cmd |= count; //long command for blink/bank switch and symbol repeat
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU8(dst, cmd); sbufWriteU8(dst, cmd);
sbufWriteU8(dst, lastChar & 0xff); sbufWriteU8(dst, lastChar & 0xff);
bytesCount += 3; bytesCount += 3;
} }
else if (count == 2) //cmd == 0 here else if (count == 2) //cmd == 0 here
{ {
sbufWriteU8(dst, lastChar & 0xff); sbufWriteU8(dst, lastChar & 0xff);
sbufWriteU8(dst, lastChar & 0xff); sbufWriteU8(dst, lastChar & 0xff);
bytesCount+=2; bytesCount+=2;
} }
@ -3425,8 +3425,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
else if (!areSensorsCalibrating()) { else if (!areSensorsCalibrating()) {
if (!ARMING_FLAG(SIMULATOR_MODE)) { // just once if (!ARMING_FLAG(SIMULATOR_MODE)) { // just once
#ifdef USE_BARO #ifdef USE_BARO
baroStartCalibration(); baroStartCalibration();
#endif #endif
#ifdef USE_MAG #ifdef USE_MAG
if (compassConfig()->mag_hardware != MAG_NONE){ if (compassConfig()->mag_hardware != MAG_NONE){
sensorsSet(SENSOR_MAG); sensorsSet(SENSOR_MAG);
@ -3530,7 +3530,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
} }
if (simulatorData.flags & SIMU_AIRSPEED) { if (simulatorData.flags & SIMU_AIRSPEED) {
simulatorData.airSpeed = sbufReadU16(src); simulatorData.airSpeed = sbufReadU16(src);
} }
} }
else { else {
@ -3548,8 +3548,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
simulatorData.debugIndex = 0; simulatorData.debugIndex = 0;
} }
tmp_u8 = simulatorData.debugIndex | tmp_u8 = simulatorData.debugIndex |
((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
(ARMING_FLAG(ARMED) ? 64 : 0) | (ARMING_FLAG(ARMED) ? 64 : 0) |
(!feature(FEATURE_OSD) ? 32: 0) | (!feature(FEATURE_OSD) ? 32: 0) |
(!isOSDTypeSupportedBySimulator() ? 16: 0); (!isOSDTypeSupportedBySimulator() ? 16: 0);

4
src/main/navigation/navigation.c
View file

@ -3072,8 +3072,8 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
} }
// WP #1 - #60 - common waypoints - pre-programmed mission // WP #1 - #60 - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) { else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
if (wpNumber <= posControl.waypointCount + wpMissionStartIndex) { if (wpNumber <= posControl.waypointCount) {
*wpData = posControl.waypointList[wpNumber - 1]; *wpData = posControl.waypointList[wpNumber + wpMissionStartIndex - 1];
if(wpData->action == NAV_WP_ACTION_JUMP) { if(wpData->action == NAV_WP_ACTION_JUMP) {
wpData->p1 += 1; // make WP # (vice index) wpData->p1 += 1; // make WP # (vice index)
} }